From 98fde4cbac067908404f8665c609caa460fa669b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Dec 2024 14:05:55 +1300 Subject: [PATCH] gimbal: correctly set gimbal_device_id When we use a gimbal connected via "RC", so PWM via the Aux channels, we need to set the gimbal_device_id to 1 as per the protocol. This was missing for GIMBAL_DEVICE_ATTITUDE_STATUS, so I added that, and fixed the name of that variable while at it. --- msg/GimbalDeviceAttitudeStatus.msg | 3 +++ msg/GimbalDeviceInformation.msg | 2 +- src/modules/gimbal/output_mavlink.cpp | 8 ++++---- src/modules/gimbal/output_mavlink.h | 2 +- src/modules/gimbal/output_rc.cpp | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 2 +- .../mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp | 5 +++++ 7 files changed, 19 insertions(+), 7 deletions(-) diff --git a/msg/GimbalDeviceAttitudeStatus.msg b/msg/GimbalDeviceAttitudeStatus.msg index 0be66babe121..0007a1c03d16 100644 --- a/msg/GimbalDeviceAttitudeStatus.msg +++ b/msg/GimbalDeviceAttitudeStatus.msg @@ -16,5 +16,8 @@ float32 angular_velocity_y float32 angular_velocity_z uint32 failure_flags +float32 delta_yaw +float32 delta_yaw_velocity +uint8 gimbal_device_id bool received_from_mavlink diff --git a/msg/GimbalDeviceInformation.msg b/msg/GimbalDeviceInformation.msg index 8f7a41643900..07351672120e 100644 --- a/msg/GimbalDeviceInformation.msg +++ b/msg/GimbalDeviceInformation.msg @@ -33,4 +33,4 @@ float32 pitch_max # [rad] float32 yaw_min # [rad] float32 yaw_max # [rad] -uint8 gimbal_device_compid +uint8 gimbal_device_id diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index 90a6596f9d0e..ca562ec87047 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -162,7 +162,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints _last_update = now; } - gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0; + gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0; _publish_gimbal_device_set_attitude(); } @@ -191,7 +191,7 @@ void OutputMavlinkV2::_check_for_gimbal_device_information() if (_gimbal_device_information_sub.update(&gimbal_device_information)) { _gimbal_device_found = true; - _gimbal_device_compid = gimbal_device_information.gimbal_device_compid; + _gimbal_device_id = gimbal_device_information.gimbal_device_id; } } @@ -210,7 +210,7 @@ void OutputMavlinkV2::print_status() const (double)_angle_velocity[2]); if (_gimbal_device_found) { - PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid); + PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_id); } else { PX4_INFO_RAW(" gimbal device compid not found\n"); @@ -222,7 +222,7 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude() gimbal_device_set_attitude_s set_attitude{}; set_attitude.timestamp = hrt_absolute_time(); set_attitude.target_system = (uint8_t)_parameters.mav_sysid; - set_attitude.target_component = _gimbal_device_compid; + set_attitude.target_component = _gimbal_device_id; set_attitude.angular_velocity_x = _angle_velocity[0]; set_attitude.angular_velocity_y = _angle_velocity[1]; diff --git a/src/modules/gimbal/output_mavlink.h b/src/modules/gimbal/output_mavlink.h index 79ef0e46e739..0e9d9f217fec 100644 --- a/src/modules/gimbal/output_mavlink.h +++ b/src/modules/gimbal/output_mavlink.h @@ -81,7 +81,7 @@ class OutputMavlinkV2 : public OutputBase uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; - uint8_t _gimbal_device_compid{0}; + uint8_t _gimbal_device_id{0}; hrt_abstime _last_gimbal_device_checked{0}; bool _gimbal_device_found {false}; }; diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index 113fe71b0d51..dba7b02b1af9 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -115,6 +115,10 @@ void OutputRC::_stream_device_attitude_status() q.copyTo(attitude_status.q); attitude_status.failure_flags = 0; + + // If the output is RC, then we signal this by referring to compid 1. + attitude_status.gimbal_device_id = 1; + _attitude_status_pub.publish(attitude_status); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fdb6e9fee4fd..78bb387fdbca 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3063,7 +3063,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; - gimbal_information.gimbal_device_compid = msg->compid; + 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 615a797488d4..44593294a7bf 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -94,6 +94,11 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream msg.failure_flags = gimbal_device_attitude_status.failure_flags; + 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); return true;