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.
This commit is contained in:
Julian Oes 2022-01-06 12:01:03 +01:00 committed by Daniel Agar
parent 7e7a99b542
commit f2216dff55
3 changed files with 12 additions and 0 deletions

View File

@ -16,3 +16,5 @@ float32 angular_velocity_y
float32 angular_velocity_z
uint32 failure_flags
bool received_from_mavlink

View File

@ -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);
}

View File

@ -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;