From a64536802b5a5b6ba8fe6ef1b7dcb6a54a0a99ea Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 14 Oct 2025 11:45:43 -0800 Subject: [PATCH] gz: fix gimbal yaw, add dds publisher (#25754) * gz: correct gimbal yaw * uxrce_dds: add publisher /fmu/out/gimbal_device_attitude_status * chore: use explicit ENU_to_NED rotation Signed-off-by: Beniamino Pozzan * format --------- Signed-off-by: Beniamino Pozzan Co-authored-by: Beniamino Pozzan --- msg/GimbalDeviceAttitudeStatus.msg | 3 +++ src/modules/simulation/gz_bridge/GZGimbal.cpp | 10 ++++++---- src/modules/uxrce_dds_client/dds_topics.yaml | 4 ++++ 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/msg/GimbalDeviceAttitudeStatus.msg b/msg/GimbalDeviceAttitudeStatus.msg index 0007a1c03d..72eca870f3 100644 --- a/msg/GimbalDeviceAttitudeStatus.msg +++ b/msg/GimbalDeviceAttitudeStatus.msg @@ -9,6 +9,9 @@ uint16 DEVICE_FLAGS_NEUTRAL = 2 uint16 DEVICE_FLAGS_ROLL_LOCK = 4 uint16 DEVICE_FLAGS_PITCH_LOCK = 8 uint16 DEVICE_FLAGS_YAW_LOCK = 16 +uint16 DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME = 32 +uint16 DEVICE_FLAGS_YAW_IN_EARTH_FRAME = 64 + float32[4] q float32 angular_velocity_x diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 451f846563..4fbccc316f 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -100,11 +100,15 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) pthread_mutex_lock(&_node_mutex); static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f); + static const matrix::Quatf q_ENU_to_NED = matrix::Quatf(0.0f, cosf(M_PI_4_F), cosf(M_PI_4_F), 0.0f); + + // Get the gimbal orientation. Gimbal frame is FLU in Gazebo, reference frame is ENU in Gazebo const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(), IMU_data.orientation().x(), IMU_data.orientation().y(), IMU_data.orientation().z()); - _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); + + _q_gimbal = q_ENU_to_NED * q_gimbal_FLU * q_FLU_to_FRD.inversed(); matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(), IMU_data.angular_velocity().y(), @@ -206,13 +210,11 @@ void GZGimbal::publishDeviceInfo() void GZGimbal::publishDeviceAttitude() { - // TODO handle flags - gimbal_device_attitude_status_s gimbal_att{}; gimbal_att.target_system = 0; // Broadcast gimbal_att.target_component = 0; // Broadcast - gimbal_att.device_flags = 0; + gimbal_att.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; _q_gimbal.copyTo(gimbal_att.q); gimbal_att.angular_velocity_x = _gimbal_rate[0]; gimbal_att.angular_velocity_y = _gimbal_rate[1]; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index ef7ba8fc0b..7cc39d199c 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -104,6 +104,10 @@ publications: type: px4_msgs::msg::Wind rate_limit: 1. + - topic: /fmu/out/gimbal_device_attitude_status + type: px4_msgs::msg::GimbalDeviceAttitudeStatus + rate_limit: 20. + # Create uORB::Publication subscriptions: - topic: /fmu/in/register_ext_component_request