Merge pull request #2384 from KarthiAru/pr-gimbal-fix

Update gimbal_full_control.cpp example
mavlink · Aug 29, 2024 · 349f97a · 349f97a
2 parents 769783a + 17ad3a8
commit 349f97a
Showing 1 changed file with 42 additions and 13 deletions.
diff --git a/examples/gimbal_full_control/gimbal_full_control.cpp b/examples/gimbal_full_control/gimbal_full_control.cpp
@@ -51,55 +51,84 @@ int main(int argc, char** argv)
     // Instantiate plugins.
     auto gimbal = Gimbal{system.value()};
 
+    auto prom = std::promise<int32_t>();
+    auto fut = prom.get_future();
+    std::once_flag once_flag;
+    // Wait for a gimbal.
+    gimbal.subscribe_gimbal_list([&prom, &once_flag](const Gimbal::GimbalList& gimbal_list) {
+        std::cout << "Found a gimbal: " << gimbal_list.gimbals.back().model_name << " by "
+                  << gimbal_list.gimbals.back().vendor_name << '\n';
+        std::call_once(once_flag, [&prom, &gimbal_list]() {
+            prom.set_value(gimbal_list.gimbals.back().gimbal_id);
+        });
+    });
+
+    if (fut.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) {
+        std::cerr << "Could not find a gimbal, exiting...";
+        return 1;
+    }
+
+    const int32_t gimbal_id = fut.get();
+    std::cout << "Use gimbal ID: " << gimbal_id << '\n';
+
     gimbal.subscribe_attitude([](Gimbal::Attitude attitude) {
         std::cout << "Gimbal angle pitch: " << attitude.euler_angle_forward.pitch_deg
                   << " deg, yaw: " << attitude.euler_angle_forward.yaw_deg << " deg\n";
     });
 
     std::cout << "Start controlling gimbal...\n";
-    Gimbal::Result gimbal_result = gimbal.take_control(Gimbal::ControlMode::Primary);
+    Gimbal::Result gimbal_result = gimbal.take_control(gimbal_id, Gimbal::ControlMode::Primary);
     if (gimbal_result != Gimbal::Result::Success) {
         std::cerr << "Could not take gimbal control: " << gimbal_result << '\n';
         return 1;
     }
 
     std::cout << "Set yaw mode to lock to a specific direction...\n";
-    gimbal_result = gimbal.set_mode(Gimbal::GimbalMode::YawLock);
+    gimbal_result = gimbal.set_angles(
+        gimbal_id, 0.0f, 0.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     if (gimbal_result != Gimbal::Result::Success) {
         std::cerr << "Could not set to lock mode: " << gimbal_result << '\n';
         return 1;
     }
 
     std::cout << "Test SetAngles...\n";
     std::cout << "Roll=0 Pitch=0 Yaw=0\n";
-    gimbal.set_angles(0.0f, 0.0f, 0.0f);
+    gimbal.set_angles(
+        gimbal_id, 0.0f, 0.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=-30 Pitch=0 Yaw=0\n";
-    gimbal.set_angles(-30.0f, 0.0f, 0.0f);
+    gimbal.set_angles(
+        gimbal_id, -30.0f, 0.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=30 Pitch=0 Yaw=0\n";
-    gimbal.set_angles(30.0f, 0.0f, 0.0f);
+    gimbal.set_angles(
+        gimbal_id, 30.0f, 0.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=0 Pitch=-30 Yaw=0\n";
-    gimbal.set_angles(0.0f, -30.0f, 0.0f);
+    gimbal.set_angles(
+        gimbal_id, 0.0f, -30.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=0 Pitch=30 Yaw=0\n";
-    gimbal.set_angles(0.0f, 30.0f, 0.0f);
+    gimbal.set_angles(
+        gimbal_id, 0.0f, 30.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=0 Pitch=0 Yaw=-30\n";
-    gimbal.set_angles(0.0f, 0.0f, -30.0f);
+    gimbal.set_angles(
+        gimbal_id, 0.0f, 0.0f, -30.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=0 Pitch=0 Yaw=30\n";
-    gimbal.set_angles(0.0f, 0.0f, 30.0f);
+    gimbal.set_angles(
+        gimbal_id, 0.0f, 0.0f, 30.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
     sleep_for(seconds(5));
     std::cout << "Roll=0 Pitch=0 Yaw=0\n";
-    gimbal.set_angles(0.0f, 0.0f, 0.0f);
-    sleep_for(seconds(2));
+    gimbal.set_angles(
+        gimbal_id, 0.0f, 0.0f, 0.0f, Gimbal::GimbalMode::YawLock, Gimbal::SendMode::Once);
+    sleep_for(seconds(5));
 
     std::cout << "Stop controlling gimbal...\n";
-    gimbal_result = gimbal.release_control();
+    gimbal_result = gimbal.release_control(gimbal_id);
     if (gimbal_result != Gimbal::Result::Success) {
-        std::cerr << "Could not take gimbal control: " << gimbal_result << '\n';
+        std::cerr << "Could not release gimbal control: " << gimbal_result << '\n';
         return 1;
     }