Commander: gate manual control setpoint processing on new data

src/modules/commander/Commander.cpp

@@ -2094,21 +2094,6 @@ Commander::run()
 			_geofence_violated_prev = false;
 		}
 
-		_manual_control.setRCAllowed(!_status_flags.rc_input_blocked);
-		_manual_control.update();
-
-		// abort autonomous mode and switch to position mode if sticks are moved significantly
-		if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
-		    && !in_low_battery_failsafe && !_geofence_warning_action_on
-		    && _manual_control.wantsOverride(_vehicle_control_mode)) {
-			if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
-						  &_internal_state) == TRANSITION_CHANGED) {
-				tune_positive(true);
-				mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
-				_status_changed = true;
-			}
-		}
-
 		/* Check for mission flight termination */
 		if (_armed.armed && _mission_result_sub.get().flight_termination &&
 		    !_status_flags.circuit_breaker_flight_termination_disabled) {
@@ -2126,9 +2111,10 @@ Commander::run()
 			}
 		}
 
+		// Manual control input handling
+		_manual_control.setRCAllowed(!_status_flags.rc_input_blocked);
 
-		/* RC input check */
-		if (_manual_control.isRCAvailable()) {
+		if (_manual_control.update()) {
 
 			/* handle the case where RC signal was regained */
 			if (!_status_flags.rc_signal_found_once) {
@@ -2168,6 +2154,18 @@ Commander::run()
 				}
 			}
 
+			// abort autonomous mode and switch to position mode if sticks are moved significantly
+			if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
+			    && !in_low_battery_failsafe && !_geofence_warning_action_on
+			    && _manual_control.wantsOverride(_vehicle_control_mode)) {
+				if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
+							  &_internal_state) == TRANSITION_CHANGED) {
+					tune_positive(true);
+					mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
+					_status_changed = true;
+				}
+			}
+
 			if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
 
 				// handle landing gear switch if configured and in a manual mode
@@ -2233,8 +2231,9 @@ Commander::run()
 			}
 
 			/* no else case: do not change lockdown flag in unconfigured case */
+		}
 
-		} else {
+		if (!_manual_control.isRCAvailable()) {
 			// set RC lost
 			if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
 				// ignore RC lost during calibration

src/modules/commander/ManualControl.cpp

@@ -42,19 +42,27 @@ enum OverrideBits {
 	OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
 };
 
-void ManualControl::update()
+bool ManualControl::update()
 {
-	_rc_available = _rc_allowed
-			&& _last_manual_control_setpoint.timestamp != 0
-			&& (hrt_elapsed_time(&_last_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s));
+	bool ret = false;
 
-	if (_manual_control_setpoint_sub.updated()) {
+	if (_rc_allowed && _manual_control_setpoint_sub.updated()) {
 		manual_control_setpoint_s manual_control_setpoint;
 
 		if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+			_rc_available = true;
 			process(manual_control_setpoint);
 		}
+
+		ret = true;
 	}
+
+	if (_last_manual_control_setpoint.timestamp == 0
+	    || (hrt_elapsed_time(&_last_manual_control_setpoint.timestamp) > (_param_com_rc_loss_t.get() * 1_s))) {
+		_rc_available = false;
+	}
+
+	return ret && _rc_available;
 }
 
 void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint)

src/modules/commander/ManualControl.hpp

@@ -57,7 +57,12 @@ public:
 	~ManualControl() override = default;
 
 	void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; }
-	void update();
+
+	/**
+	 * Check for manual control input changes and process them
+	 * @return true if there was new data
+	 */
+	bool update();
 	bool isRCAvailable() { return _rc_available; }
 	bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode);
 	bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,