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;
  }