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 */