From 3f13a6e7874c8d27ae53770e0ebf0c727bf7caf1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 1 May 2023 12:54:31 +1200 Subject: [PATCH] gimbal: implement gimbal_device_id It turns out that I had omitted implementing the gimbal_device_id which is the component ID of the gimbal device that the gimbal manager (in this case PX4) is responsible for. Signed-off-by: Julian Oes --- src/modules/gimbal/common.h | 1 + src/modules/gimbal/gimbal.cpp | 1 + src/modules/gimbal/input_mavlink.cpp | 12 +++++++++--- src/modules/gimbal/input_mavlink.h | 4 +++- src/modules/gimbal/output.h | 2 +- src/modules/gimbal/output_mavlink.cpp | 13 +++++++++++-- src/modules/gimbal/output_mavlink.h | 4 ++-- src/modules/gimbal/output_rc.cpp | 5 ++++- src/modules/gimbal/output_rc.h | 2 +- .../mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp | 2 +- 10 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/modules/gimbal/common.h b/src/modules/gimbal/common.h index f3061a818f..e7f29b726b 100644 --- a/src/modules/gimbal/common.h +++ b/src/modules/gimbal/common.h @@ -82,6 +82,7 @@ struct ControlData { uint8_t compid_primary_control = 0; // The MAVLink component ID selected to be in control, 0 for no one. // uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet. // uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet. + uint8_t device_compid = 0; }; diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 9d62505425..7b128bbeaa 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -211,6 +211,7 @@ static int gimbal_thread_main(int argc, char *argv[]) // Reset control as no one is active anymore, or yet. thread_data.control_data.sysid_primary_control = 0; thread_data.control_data.compid_primary_control = 0; + thread_data.control_data.device_compid = 0; } InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index a9fe31b538..78cf6ae806 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -375,7 +375,6 @@ void InputMavlinkCmdMount::print_status() const InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters ¶meters) : InputBase(parameters) { - _stream_gimbal_manager_information(); } InputMavlinkGimbalV2::~InputMavlinkGimbalV2() @@ -453,7 +452,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont gimbal_manager_status_s gimbal_manager_status{}; gimbal_manager_status.timestamp = hrt_absolute_time(); gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags; - gimbal_manager_status.gimbal_device_id = 0; + gimbal_manager_status.gimbal_device_id = control_data.device_compid; gimbal_manager_status.primary_control_sysid = control_data.sysid_primary_control; gimbal_manager_status.primary_control_compid = control_data.compid_primary_control; gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control @@ -462,7 +461,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont } } -void InputMavlinkGimbalV2::_stream_gimbal_manager_information() +void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data) { // TODO: Take gimbal_device_information into account. @@ -483,6 +482,8 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information() gimbal_manager_info.yaw_max = M_PI_F; gimbal_manager_info.yaw_min = -M_PI_F; + gimbal_manager_info.gimbal_device_id = control_data.device_compid; + _gimbal_manager_info_pub.publish(gimbal_manager_info); } @@ -578,6 +579,11 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, _stream_gimbal_manager_status(control_data); + if (_last_device_compid != control_data.device_compid) { + _last_device_compid = control_data.device_compid; + _stream_gimbal_manager_information(control_data); + } + return update_result; } diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index 0bc0a7f1af..04264410d7 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -114,7 +114,7 @@ private: void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q, const matrix::Vector3f &angular_velocity); void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result); - void _stream_gimbal_manager_information(); + void _stream_gimbal_manager_information(const ControlData &control_data); void _stream_gimbal_manager_status(const ControlData &control_data); void _read_control_data_from_position_setpoint_sub(ControlData &control_data); @@ -128,6 +128,8 @@ private: uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; + + uint8_t _last_device_compid = 0; }; } /* namespace gimbal */ diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index 9b2e3693ea..4a7724de2b 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -55,7 +55,7 @@ public: explicit OutputBase(const Parameters ¶meters); virtual ~OutputBase() = default; - virtual void update(const ControlData &control_data, bool new_setpoints) = 0; + virtual void update(ControlData &control_data, bool new_setpoints) = 0; virtual void print_status() const = 0; diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index 8f4f55b7aa..81da7bc637 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -46,7 +46,7 @@ OutputMavlinkV1::OutputMavlinkV1(const Parameters ¶meters) : OutputBase(parameters) {} -void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints) +void OutputMavlinkV1::update(ControlData &control_data, bool new_setpoints) { vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); @@ -142,7 +142,7 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters ¶meters) { } -void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints) +void OutputMavlinkV2::update(ControlData &control_data, bool new_setpoints) { _check_for_gimbal_device_information(); @@ -162,6 +162,8 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints _last_update = t; } + control_data.device_compid = _gimbal_device_found ? _gimbal_device_compid : 0; + _publish_gimbal_device_set_attitude(); } } @@ -206,6 +208,13 @@ void OutputMavlinkV2::print_status() const (double)_angle_velocity[0], (double)_angle_velocity[1], (double)_angle_velocity[2]); + + if (_gimbal_device_found) { + PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid); + + } else { + PX4_INFO_RAW(" gimbal device compid not found\n"); + } } void OutputMavlinkV2::_publish_gimbal_device_set_attitude() diff --git a/src/modules/gimbal/output_mavlink.h b/src/modules/gimbal/output_mavlink.h index cc1f957e5b..303a2ed77e 100644 --- a/src/modules/gimbal/output_mavlink.h +++ b/src/modules/gimbal/output_mavlink.h @@ -51,7 +51,7 @@ public: OutputMavlinkV1(const Parameters ¶meters); virtual ~OutputMavlinkV1() = default; - virtual void update(const ControlData &control_data, bool new_setpoints); + virtual void update(ControlData &control_data, bool new_setpoints); virtual void print_status() const; @@ -69,7 +69,7 @@ public: OutputMavlinkV2(const Parameters ¶meters); virtual ~OutputMavlinkV2() = default; - virtual void update(const ControlData &control_data, bool new_setpoints); + virtual void update(ControlData &control_data, bool new_setpoints); virtual void print_status() const; diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index b7ac745cf3..d6a00a78b7 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -48,7 +48,7 @@ OutputRC::OutputRC(const Parameters ¶meters) { } -void OutputRC::update(const ControlData &control_data, bool new_setpoints) +void OutputRC::update(ControlData &control_data, bool new_setpoints) { if (new_setpoints) { _set_angle_setpoints(control_data); @@ -61,6 +61,9 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints) _stream_device_attitude_status(); + // If the output is RC, then it means we are also the gimbal device. + control_data.device_compid = (uint8_t)_parameters.mnt_mav_compid_v1; + // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{}; gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( diff --git a/src/modules/gimbal/output_rc.h b/src/modules/gimbal/output_rc.h index 20bdf9f462..7862889265 100644 --- a/src/modules/gimbal/output_rc.h +++ b/src/modules/gimbal/output_rc.h @@ -50,7 +50,7 @@ public: explicit OutputRC(const Parameters ¶meters); virtual ~OutputRC() = default; - virtual void update(const ControlData &control_data, bool new_setpoints); + virtual void update(ControlData &control_data, bool new_setpoints); virtual void print_status() const; private: diff --git a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp index 0a849841d5..a6570362e1 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp @@ -71,7 +71,7 @@ private: msg.time_boot_ms = gimbal_manager_information.timestamp / 1000; msg.cap_flags = gimbal_manager_information.cap_flags; - msg.gimbal_device_id = 0; + msg.gimbal_device_id = gimbal_manager_information.gimbal_device_id; msg.roll_min = gimbal_manager_information.roll_min; msg.roll_max = gimbal_manager_information.roll_max; msg.pitch_min = gimbal_manager_information.pitch_min;