diff --git a/msg/gimbal_device_attitude_status.msg b/msg/gimbal_device_attitude_status.msg index de2d7e892b..0be66babe1 100644 --- a/msg/gimbal_device_attitude_status.msg +++ b/msg/gimbal_device_attitude_status.msg @@ -16,3 +16,5 @@ float32 angular_velocity_y float32 angular_velocity_z uint32 failure_flags + +bool received_from_mavlink diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f191ff716..c66300c057 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3070,6 +3070,8 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t gimbal_attitude_status.angular_velocity_z = gimbal_device_attitude_status_msg.angular_velocity_z; gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags; + gimbal_attitude_status.received_from_mavlink = true; + _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 0d9fe42129..615a797488 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -66,6 +66,14 @@ private: gimbal_device_attitude_status_s gimbal_device_attitude_status{}; if (_gimbal_device_attitude_status_sub.update(&gimbal_device_attitude_status)) { + + if (gimbal_device_attitude_status.received_from_mavlink) { + // If we have already received the gimbal device's attitude via + // mavlink it is already forwarded directly and we don't need + // to re-publish it here. + return false; + } + mavlink_gimbal_device_attitude_status_t msg{}; msg.target_system = gimbal_device_attitude_status.target_system;