From c5a31a715d98bd7ed743157c9f694243e41e4d1d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Nov 2023 15:09:17 +1300 Subject: [PATCH] gimbal_controller: send attitude in Earth frame 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 --- src/gazebo_gimbal_controller_plugin.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/gazebo_gimbal_controller_plugin.cpp b/src/gazebo_gimbal_controller_plugin.cpp index 5cc1512942..2836e1890c 100644 --- 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(q.W()), static_cast(q.X()),