diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index ca562ec870..ae5222c674 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -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; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 37328503b9..aa8b50d01a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index c8338996ae..ae7f3cbad1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -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; } diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 00b46becc4..662268abd2 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -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);