fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 14:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: handle for GIMBAL_DEVICE_ATTITUDE_STATUS

Browse Source 
That way we can log it later.
This commit is contained in:
Julian Oes
2022-02-07 19:21:15 -05:00
committed by Daniel Agar
parent 5b07398b3e
commit 7e7a99b542
2 changed files with 31 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+28
View File
@@ -304,6 +304,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_event(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
handle_message_gimbal_device_attitude_status(msg);
break;
default:
break;
}
@@ -3045,6 +3049,30 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
_gimbal_device_information_pub.publish(gimbal_information);
}
void
MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t *msg)
{
mavlink_gimbal_device_attitude_status_t gimbal_device_attitude_status_msg;
mavlink_msg_gimbal_device_attitude_status_decode(msg, &gimbal_device_attitude_status_msg);
gimbal_device_attitude_status_s gimbal_attitude_status{};
gimbal_attitude_status.timestamp = static_cast<uint64_t>(gimbal_device_attitude_status_msg.time_boot_ms) * 1000;
gimbal_attitude_status.target_system = gimbal_device_attitude_status_msg.target_system;
gimbal_attitude_status.target_component = gimbal_device_attitude_status_msg.target_component;
gimbal_attitude_status.device_flags = gimbal_device_attitude_status_msg.flags;
for (unsigned i = 0; i < 4; ++i) {
gimbal_attitude_status.q[i] = gimbal_device_attitude_status_msg.q[i];
}
gimbal_attitude_status.angular_velocity_x = gimbal_device_attitude_status_msg.angular_velocity_x;
gimbal_attitude_status.angular_velocity_y = gimbal_device_attitude_status_msg.angular_velocity_y;
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_device_attitude_status_pub.publish(gimbal_attitude_status);
}
void
MavlinkReceiver::run()
{