From 25138e803ee8525ee5fe4e6d511506e88e3f819c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Jul 2021 11:14:41 +0200 Subject: [PATCH] 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 | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/gazebo_gimbal_controller_plugin.cpp b/src/gazebo_gimbal_controller_plugin.cpp index 11b2e78517..7d6b1b808b 100644 --- a/src/gazebo_gimbal_controller_plugin.cpp +++ b/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(q.W()),