gimbal_controller: fix attitude status

This fixes two things that were implemented incorrectly:
- The flags were not set correctly.
- The quaternion was sent in the absolute frame even without yaw lock.

src/gazebo_gimbal_controller_plugin.cpp

@@ -676,9 +676,20 @@ void GimbalControllerPlugin::SendGimbalDeviceAttitudeStatus()
   const common::Time time = this->model->GetWorld()->SimTime();
   const uint32_t timeMs = time.sec * 1000 + time.nsec / 1000000;
 
-  const uint16_t flags {0};
-
-  const auto q = q_ENU_to_NED * this->cameraImuSensor->Orientation() * q_FLU_to_FRD.Inverse();
+  const uint16_t flags =
+    GIMBAL_DEVICE_FLAGS_ROLL_LOCK |
+    GIMBAL_DEVICE_FLAGS_PITCH_LOCK |
+    (this->yawLock ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
+
+  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()),