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

gimbal: don't spoof gimbal device (#24271)

Browse Source 
The current approach was wrong because the gimbal protocol now
handles the case properly where the autopilot is in charge of a
non-MAVLink gimbal.

This means that we don't need to send message "as if we were a gimbal
device" and instead set thet gimbal_device_id to 1 (up to 6) to indicate
we are in charge or a non-MAVLink gimbal.
This commit is contained in:
Julian Oes
2025-01-31 12:59:41 +13:00
committed by GitHub
parent 4c2e69b2e6
commit a9214b3aa3
4 changed files with 22 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gimbal/output_mavlink.cpp
+3
View File
@@ -109,6 +109,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints
_stream_device_attitude_status();
// If the output is MAVLink v1, then we signal this by referring to compid 1.
gimbal_device_id = 1;
_last_update = now;
}
src/modules/mavlink/mavlink_receiver.cpp
+1 -6
View File
@@ -3075,12 +3075,7 @@ 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;
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_information.gimbal_device_id = msg->compid;
_gimbal_device_information_pub.publish(gimbal_information);
}
src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp
+17 -34
View File
@@ -74,48 +74,31 @@ private:
return false;
}
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{};
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.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
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;
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);
}
mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
return true;
}
src/modules/simulation/gz_bridge/GZGimbal.hpp
+1 -1
View File
@@ -115,7 +115,7 @@ private:
const float _yaw_min = NAN; // infinite yaw
const float _yaw_max = NAN; // infinite yaw
const uint8_t _gimbal_device_id = 154; // TODO the implementation differs from the protocol
const uint8_t _gimbal_device_id = 1; // Gimbal is implemented by the same component: options are 1..6
uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS
bool init(const std::string &world_name, const std::string &model_name);