mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 18:47:21 +08:00
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:
@@ -16,3 +16,5 @@ float32 angular_velocity_y
|
|||||||
float32 angular_velocity_z
|
float32 angular_velocity_z
|
||||||
|
|
||||||
uint32 failure_flags
|
uint32 failure_flags
|
||||||
|
|
||||||
|
bool received_from_mavlink
|
||||||
|
|||||||
@@ -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.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.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);
|
_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -66,6 +66,14 @@ private:
|
|||||||
gimbal_device_attitude_status_s gimbal_device_attitude_status{};
|
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_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{};
|
mavlink_gimbal_device_attitude_status_t msg{};
|
||||||
|
|
||||||
msg.target_system = gimbal_device_attitude_status.target_system;
|
msg.target_system = gimbal_device_attitude_status.target_system;
|
||||||
|
|||||||
Reference in New Issue
Block a user