From 3710a9ef6ea236ac90510f0cb074f640ef2f6b08 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 6 Jun 2024 15:32:50 +1200 Subject: [PATCH] gimbal: fix auto RC and MAVLink mode This fixes various edge cases when input is set to both: RC and MAVLink gimbal protocol v2. Specifically: - We no longer immediately reset primary control if there is no update, otherwise this will reset immediately after an commands. - Talking of commands: GIMBAL_MANAGER_CONFIGURE no longer switches control to MAVLink but only an actual incoming setpoint command does. - And incoming setpoint command only triggers UpdatedActiveOnce which means we will check RC again afterwards and if there is big movement switch back to RC. That's the intuitive thing to do until we have better reporting about who/what is in control. - Also, with RC we no longer always set us to be in control but only on major movement. --- src/modules/gimbal/gimbal.cpp | 10 ---------- src/modules/gimbal/input_mavlink.cpp | 6 ++++-- src/modules/gimbal/input_rc.cpp | 5 ++++- src/modules/gimbal/output_rc.cpp | 3 ++- 4 files changed, 10 insertions(+), 14 deletions(-) diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 5893d29738..804f792fc6 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -207,16 +207,6 @@ static int gimbal_thread_main(int argc, char *argv[]) update_params(param_handles, params); } - if (thread_data.last_input_active == -1) { - // 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; - // If the output is set to AUX we still want the input to be able to control the gimbal - // via mavlink, so we set the device_compid to 1. This follows the mavlink spec which states - // that the gimbal_device_id should be between 1 and 6. - thread_data.control_data.device_compid = params.mnt_mode_out == 0 ? 1 : 0; - } - InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate; if (thread_data.input_objs_len > 0) { diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index ac7cbc4e7f..101ebf1e27 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -856,7 +856,9 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ control_data.compid_primary_control = new_compid_primary_control; } - return UpdateResult::UpdatedActive; + // Just doing the configuration doesn't mean there is actually an update to use yet. + // After that we still need to have an actual setpoint. + return UpdateResult::NoUpdate; // TODO: support secondary control // TODO: support gimbal device id for multiple gimbals @@ -880,7 +882,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ _ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - return UpdateResult::UpdatedActive; + return UpdateResult::UpdatedActiveOnce; } else { PX4_INFO("GIMBAL_MANAGER_PITCHYAW from %d/%d denied, in control: %d/%d", diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index c512cda254..c15f916ab9 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -120,9 +120,12 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData return false; }(); - if (already_active || major_movement) { + if (major_movement) { control_data.sysid_primary_control = _parameters.mav_sysid; control_data.compid_primary_control = _parameters.mav_compid; + } + + if (already_active || major_movement) { if (_parameters.mnt_rc_in_mode == 0) { // We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees. diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index a5878b3801..4b48349a4d 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -61,7 +61,8 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 _stream_device_attitude_status(); - // If the output is RC, then it means we are also the gimbal device. gimbal_device_id = (uint8_t)_parameters.mnt_mav_compid_v1; + // If the output is RC, then we signal this by referring to compid 1. + gimbal_device_id = 1; // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{};