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 <julian@oes.ch>

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.

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