From 10e3401d2125252d2b1f8227583efdcb22ea3bb6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 9 Aug 2026 13:27:45 +1200 Subject: [PATCH] fix(gimbal): arbitrate external gimbal manager control cleanly Refine the external gimbal manager client so it coexists with a ground station instead of fighting it: - Only forward setpoints that originated onboard (primary control is the autopilot). A ground station commands the external manager directly, so relaying its commands would make PX4 fight it for control. - Acquire control only on a fresh onboard-intent edge, and yield instead of re-grabbing when another controller takes over - this removes the acquire/release ping-pong. A new onboard edge is needed to reclaim. - Apply the setpoint every cycle while in control, not only on the new_setpoints edge: the control handshake can delay reaching the in-control state past that edge, which would otherwise stream NaN. Signed-off-by: Julian Oes --- src/modules/gimbal/output_gimbal_manager.cpp | 105 ++++++++++++------- src/modules/gimbal/output_gimbal_manager.h | 2 + 2 files changed, 68 insertions(+), 39 deletions(-) diff --git a/src/modules/gimbal/output_gimbal_manager.cpp b/src/modules/gimbal/output_gimbal_manager.cpp index 64b8f935688..684b0d099e9 100644 --- a/src/modules/gimbal/output_gimbal_manager.cpp +++ b/src/modules/gimbal/output_gimbal_manager.cpp @@ -60,57 +60,76 @@ void OutputToGimbalManager::update(const ControlData &control_data, bool new_set return; } - // We want control whenever there is an active setpoint to forward. - const bool want_control = (control_data.type != ControlData::Type::Neutral); + // Only forward intent that originated onboard (RC, ROI, mission), i.e. where + // the autopilot itself holds primary control. A ground station commands an + // external manager directly, so we must not relay its commands here - + // otherwise we would fight it for control. + const bool onboard_originated = + control_data.sysid_primary_control == (uint8_t)_parameters.mav_sysid && + control_data.compid_primary_control == (uint8_t)_parameters.mav_compid; - if (want_control) { - switch (_control_state) { - case ControlState::Released: + const bool want_control = onboard_originated && (control_data.type != ControlData::Type::Neutral); + + // Acquire only on a fresh edge of onboard intent, never continuously. This + // gives "assert once, then yield if overridden" rather than fighting whoever + // took control (a moved RC stick or a new ROI re-triggers the edge). + const bool want_control_edge = want_control && !_prev_want_control; + _prev_want_control = want_control; + + switch (_control_state) { + case ControlState::Released: + if (want_control_edge) { _send_configure(true); _last_acquire_request = now; _control_state = ControlState::Acquiring; - break; - - case ControlState::Acquiring: - if (_have_primary_control()) { - _control_state = ControlState::InControl; - - } else if (now - _last_acquire_request > kAcquireRetryInterval) { - _send_configure(true); - _last_acquire_request = now; - } - - break; - - case ControlState::InControl: - if (!_have_primary_control()) { - // Someone took control from us. Try to reacquire. - _send_configure(true); - _last_acquire_request = now; - _control_state = ControlState::Acquiring; - } - - break; } - if (_control_state == ControlState::InControl) { - if (new_setpoints) { - _set_angle_setpoints(control_data); - } + break; - // Keep pointing updated as the vehicle moves relative to a location target. - _handle_position_update(control_data); - _publish_set_pitchyaw(); - _last_update = now; + case ControlState::Acquiring: + if (_have_primary_control()) { + _control_state = ControlState::InControl; + + } else if (!want_control) { + _control_state = ControlState::Released; + + } else if (_someone_else_in_control()) { + // We lost the race for control; yield instead of fighting. + _control_state = ControlState::Released; + + } else if (now - _last_acquire_request > kAcquireRetryInterval) { + // No one holds control yet, our request may have been lost: retry. + _send_configure(true); + _last_acquire_request = now; } - } else { - // No active setpoint: release control so other components (e.g. a - // ground station) can command the manager. - if (_control_state != ControlState::Released) { + break; + + case ControlState::InControl: + if (!want_control) { + // Our intent ended: release so a ground station can take over. _send_configure(false); _control_state = ControlState::Released; + + } else if (!_have_primary_control()) { + // Someone took control from us: yield, don't fight. A new onboard + // intent edge is required to reclaim. + _control_state = ControlState::Released; } + + break; + } + + if (_control_state == ControlState::InControl) { + // Apply the current setpoint every cycle, not just on new_setpoints. + // Unlike a direct device output we can reach InControl several cycles + // after new_setpoints (after the control handshake), so gating on it + // could miss the setpoint entirely and stream NaN. control_data always + // holds the latest setpoint, and for a location target _set_angle_setpoints + // re-runs the geo pointing so it keeps tracking as the vehicle moves. + _set_angle_setpoints(control_data); + _publish_set_pitchyaw(); + _last_update = now; } } @@ -140,6 +159,14 @@ bool OutputToGimbalManager::_have_primary_control() const && _status.primary_control_compid == (uint8_t)_parameters.mav_compid; } +bool OutputToGimbalManager::_someone_else_in_control() const +{ + // A non-zero primary control that isn't us means another component holds it. + return _status_valid + && (_status.primary_control_sysid != 0 || _status.primary_control_compid != 0) + && !_have_primary_control(); +} + void OutputToGimbalManager::_send_configure(bool acquire) { // Special values per MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: -2 releases diff --git a/src/modules/gimbal/output_gimbal_manager.h b/src/modules/gimbal/output_gimbal_manager.h index 0e6739c7057..c19251a8980 100644 --- a/src/modules/gimbal/output_gimbal_manager.h +++ b/src/modules/gimbal/output_gimbal_manager.h @@ -68,6 +68,7 @@ private: void _update_manager_status(); bool _have_primary_control() const; + bool _someone_else_in_control() const; void _send_configure(bool acquire); void _publish_set_pitchyaw(); @@ -84,6 +85,7 @@ private: bool _status_valid{false}; ControlState _control_state{ControlState::Released}; + bool _prev_want_control{false}; hrt_abstime _last_acquire_request{0}; static constexpr hrt_abstime kAcquireRetryInterval{3000000}; // 3 s