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

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

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