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{};