From c46912f1c9164ef0befe390dad021e70dfd62434 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Sep 2026 11:19:41 +1200 Subject: [PATCH] fix(gimbal): re-request control while nobody is in control Control of the external gimbal manager was only requested once, when onboard intent started. RC input stays active once it has been used, so if that request got lost or the manager restarted, we never asked again and a manager which only accepts setpoints from whoever is in control ignored us from then on. Ask again, rate limited, while we have onboard intent and the manager reports that nobody is in control. This can't take control away from another client, so the arbitration stays with the manager. Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Julian Oes --- src/modules/gimbal/output_gimbal_manager.cpp | 15 +++++++++++++++ src/modules/gimbal/output_gimbal_manager.h | 13 +++++++++---- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/modules/gimbal/output_gimbal_manager.cpp b/src/modules/gimbal/output_gimbal_manager.cpp index 09cbb089ef0..1d93622c28c 100644 --- a/src/modules/gimbal/output_gimbal_manager.cpp +++ b/src/modules/gimbal/output_gimbal_manager.cpp @@ -77,6 +77,14 @@ void OutputToGimbalManager::update(const ControlData &control_data, bool new_set } else if (!onboard_intent && _onboard_intent) { // Intent ended: release so another client can take over. _send_configure(false); + + } else if (onboard_intent && _nobody_in_control() + && hrt_elapsed_time(&_last_configure) > CONFIGURE_RETRY_INTERVAL) { + // We want control but the manager reports that nobody has it, e.g. + // because our request got lost or the manager restarted. Ask again. + // This can't take control away from anyone, so it's still up to the + // manager to arbitrate between its clients. + _send_configure(true); } _onboard_intent = onboard_intent; @@ -110,8 +118,15 @@ void OutputToGimbalManager::_update_manager_status() } } +bool OutputToGimbalManager::_nobody_in_control() const +{ + return _status.primary_control_sysid == 0 && _status.primary_control_compid == 0; +} + void OutputToGimbalManager::_send_configure(bool acquire) { + _last_configure = hrt_absolute_time(); + // Special values per MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: -1 leaves a field // unchanged, -3 removes control if the sender is currently in control. // To acquire, we set ourselves as primary. diff --git a/src/modules/gimbal/output_gimbal_manager.h b/src/modules/gimbal/output_gimbal_manager.h index f9a3592a8da..3c5495e0365 100644 --- a/src/modules/gimbal/output_gimbal_manager.h +++ b/src/modules/gimbal/output_gimbal_manager.h @@ -50,9 +50,10 @@ namespace gimbal // // The external manager does the deconfliction between its clients, so we don't // track or second-guess who is in control. When onboard intent starts we ask -// for control once, while it lasts we stream GIMBAL_MANAGER_SET_PITCHYAW, and -// when it ends we release control. If another client has taken control, the -// manager ignores our setpoints. +// for control, while it lasts we stream GIMBAL_MANAGER_SET_PITCHYAW, and when +// it ends we release control. If another client has taken control, the manager +// ignores our setpoints. We only ask again while the manager reports that nobody +// is in control (e.g. after it restarted). class OutputToGimbalManager : public OutputBase { public: @@ -66,6 +67,7 @@ public: private: void _update_manager_status(); void _send_configure(bool acquire); + bool _nobody_in_control() const; void _publish_set_pitchyaw(); uORB::Subscription _status_sub{ORB_ID(external_gimbal_manager_status)}; @@ -77,10 +79,13 @@ private: uint8_t _manager_compid{0}; uint8_t _gimbal_device_id{0}; - // Last status from the manager, for print_status only. + // Last status from the manager. external_gimbal_manager_status_s _status{}; + static constexpr hrt_abstime CONFIGURE_RETRY_INTERVAL{2000000}; // [us] + bool _onboard_intent{false}; + hrt_abstime _last_configure{0}; }; } /* namespace gimbal */