From f2216dff550731d0dcc712cbf457ea7cf5eac25e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 6 Jan 2022 12:01:03 +0100 Subject: [PATCH] mavlink: don't send gimbal_device_attitude_status If we receive gimbal_device_attitude_status by mavlink we should not re-send it as we are already supposed to be forwarding mavlink traffic from the gimbal to the ground station. --- msg/gimbal_device_attitude_status.msg | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 2 ++ .../mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp | 8 ++++++++ 3 files changed, 12 insertions(+) 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;