fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 12:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

GZ: add gimbal simulation (#23382)

Browse Source
This commit is contained in:
Stefano Colli
2024-12-18 12:15:03 +01:00
committed by GitHub
parent 4a73195007
commit 0561f6c9fc
17 changed files with 558 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+7 -1
View File
@@ -3063,7 +3063,12 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
gimbal_information.gimbal_device_id = msg->compid;
if (gimbal_device_info_msg.gimbal_device_id == 0) {
gimbal_information.gimbal_device_id = msg->compid;
} else {
gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id;
}
_gimbal_device_information_pub.publish(gimbal_information);
}
@@ -3090,6 +3095,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags;
gimbal_attitude_status.received_from_mavlink = true;
gimbal_attitude_status.gimbal_device_id = gimbal_device_attitude_status_msg.gimbal_device_id;
_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
}
src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp
+33 -17
View File
@@ -74,32 +74,48 @@ private:
return false;
}
mavlink_gimbal_device_attitude_status_t msg{};
if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) {
// A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id
mavlink_gimbal_device_attitude_status_t msg{};
msg.target_system = gimbal_device_attitude_status.target_system;
msg.target_component = gimbal_device_attitude_status.target_component;
msg.target_system = gimbal_device_attitude_status.target_system;
msg.target_component = gimbal_device_attitude_status.target_component;
msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
msg.flags = gimbal_device_attitude_status.device_flags;
msg.flags = gimbal_device_attitude_status.device_flags;
msg.q[0] = gimbal_device_attitude_status.q[0];
msg.q[1] = gimbal_device_attitude_status.q[1];
msg.q[2] = gimbal_device_attitude_status.q[2];
msg.q[3] = gimbal_device_attitude_status.q[3];
msg.q[0] = gimbal_device_attitude_status.q[0];
msg.q[1] = gimbal_device_attitude_status.q[1];
msg.q[2] = gimbal_device_attitude_status.q[2];
msg.q[3] = gimbal_device_attitude_status.q[3];
msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x;
msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y;
msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z;
msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x;
msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y;
msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z;
msg.failure_flags = gimbal_device_attitude_status.failure_flags;
msg.failure_flags = gimbal_device_attitude_status.failure_flags;
msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
} else {
// We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID
mavlink_message_t message;
mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL,
_mavlink->get_channel(), &message,
gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component,
gimbal_device_attitude_status.timestamp / 1000,
gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q,
gimbal_device_attitude_status.angular_velocity_x,
gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z,
gimbal_device_attitude_status.failure_flags,
0, 0, 0);
_mavlink->forward_message(&message, _mavlink);
}
return true;
}
src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp
+1
View File
@@ -83,6 +83,7 @@ private:
msg.pitch_max = gimbal_device_information.pitch_max;
msg.yaw_min = gimbal_device_information.yaw_min;
msg.yaw_max = gimbal_device_information.yaw_max;
msg.gimbal_device_id = gimbal_device_information.gimbal_device_id;
mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg);