Commander: move rc availability to ManualControl

src/modules/commander/Commander.cpp

@@ -2001,9 +2001,6 @@ Commander::run()
 			}
 		}
 
-		_manual_control.update();
-		_manual_control_setpoint = _manual_control._manual_control_setpoint;
-
 		/* start geofence result check */
 		_geofence_result_sub.update(&_geofence_result);
 
@@ -2105,10 +2102,14 @@ Commander::run()
 			_geofence_violated_prev = false;
 		}
 
+		_manual_control.setRCAllowed(!_status_flags.rc_input_blocked);
+		_manual_control.update();
+		_manual_control_setpoint = _manual_control._manual_control_setpoint;
+
 		// 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, !_status.rc_signal_lost)) {
+		    && _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);
@@ -2134,9 +2135,9 @@ Commander::run()
 			}
 		}
 
+
 		/* RC input check */
-		if (!_status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
+		if (_manual_control.isRCAvailable()) {
-		    (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
 
 			/* handle the case where RC signal was regained */
 			if (!_status_flags.rc_signal_found_once) {
@@ -2697,7 +2698,6 @@ Commander::run()
 		_last_condition_global_position_valid = _status_flags.condition_global_position_valid;
 
 		_was_armed = _armed.armed;
-		_last_manual_control_setpoint = _manual_control_setpoint;
 
 		arm_auth_update(now, params_updated || param_init_forced);
 

src/modules/commander/Commander.hpp

@@ -195,7 +195,6 @@ private:
 		(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
 
 		(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
-		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
 		(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
 
 		(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
@@ -356,7 +355,6 @@ private:
 	unsigned int	_leds_counter{0};
 
 	manual_control_setpoint_s _manual_control_setpoint{};
-	manual_control_setpoint_s _last_manual_control_setpoint{};
 	manual_control_switches_s _manual_control_switches{};
 	manual_control_switches_s _last_manual_control_switches{};
 	ManualControl _manual_control{this};

src/modules/commander/ManualControl.cpp

@@ -32,6 +32,9 @@
  ****************************************************************************/
 
 #include "ManualControl.hpp"
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
 
 enum OverrideBits {
 	OVERRIDE_AUTO_MODE_BIT = (1 << 0),
@@ -41,6 +44,10 @@ enum OverrideBits {
 
 void 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));
+
 	if (_manual_control_setpoint_sub.updated()) {
 		manual_control_setpoint_s manual_control_setpoint;
 
@@ -56,7 +63,7 @@ void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint)
 	_manual_control_setpoint = manual_control_setpoint;
 }
 
-bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available)
+bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode)
 {
 	const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
 					&& vehicle_control_mode.flag_control_auto_enabled;
@@ -64,7 +71,7 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
 	const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
 					    && vehicle_control_mode.flag_control_offboard_enabled;
 
-	if (rc_available && (override_auto_mode || override_offboard_mode)) {
+	if (_rc_available && (override_auto_mode || override_offboard_mode)) {
 		const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
 
 		const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)

src/modules/commander/ManualControl.hpp

@@ -53,8 +53,10 @@ public:
 	ManualControl(ModuleParams *parent) : ModuleParams(parent) {};
 	~ManualControl() override = default;
 
+	void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; }
 	void update();
-	bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available);
+	bool isRCAvailable() { return _rc_available; }
+	bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode);
 
 //private:
 	void process(manual_control_setpoint_s &manual_control_setpoint);
@@ -63,7 +65,11 @@ public:
 	manual_control_setpoint_s _manual_control_setpoint{};
 	manual_control_setpoint_s _last_manual_control_setpoint{};
 
+	bool _rc_allowed{false};
+	bool _rc_available{false};
+
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
 		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
 		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov
 	)