gimbal_controller: send attitude in Earth frame (#1023)

We no longer need to swap this based on the yaw lock with the recent flags that have been added to the spec. Signed-off-by: Julian Oes <[email protected]>
PX4 · Mar 3, 2024 · 63b72dc · 63b72dc
1 parent da7206e
commit 63b72dc
Showing 1 changed file with 2 additions and 9 deletions.
diff --git a/src/gazebo_gimbal_controller_plugin.cpp b/src/gazebo_gimbal_controller_plugin.cpp
@@ -695,18 +695,11 @@ void GimbalControllerPlugin::SendGimbalDeviceAttitudeStatus()
   const uint16_t flags =
     GIMBAL_DEVICE_FLAGS_ROLL_LOCK |
     GIMBAL_DEVICE_FLAGS_PITCH_LOCK |
-    (this->yawLock ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
+    (this->yawLock ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0) |
+    GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME;
 
   auto q = q_ENU_to_NED * this->cameraImuSensor->Orientation() * q_FLU_to_FRD.Inverse();
 
-  if (!this->yawLock) {
-    // In follow mode we need to transform the absolute camera orientation to an orientation
-    // relative to the vehicle because that's what the gimbal protocol suggests.
-    const auto q_vehicle = q_ENU_to_NED * ignition::math::Quaterniond(0.0, 0.0, this->vehicleYawRad) * q_FLU_to_FRD.Inverse();
-    const auto e = q.Euler();
-    q.Euler(e[0], e[1], e[2] - q_vehicle.Euler()[2]);
-  }
-
   const float qArr[4] = {
     static_cast<float>(q.W()),
     static_cast<float>(q.X()),