Commander: gate manual control setpoint processing on new data

Separating the different arming methods is the next step.

src/modules/commander/CMakeLists.txt

@@ -50,6 +50,7 @@ px4_add_module(
 		level_calibration.cpp
 		lm_fit.cpp
 		mag_calibration.cpp
+		ManualControl.cpp
 		rc_calibration.cpp
 		state_machine_helper.cpp
 		worker_thread.cpp

src/modules/commander/Commander.cpp

@@ -426,18 +426,15 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 
 	if (run_preflight_checks) {
 		if (_vehicle_control_mode.flag_control_manual_enabled) {
-			const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f);
-			const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f);
-
-			if (_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_center) {
-				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered");
+			if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied because throttle above center");
 				tune_negative(true);
 				return TRANSITION_DENIED;
 
 			}
 
-			if (!_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_low) {
-				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero");
+			if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied because of high throttle");
 				tune_negative(true);
 				return TRANSITION_DENIED;
 			}
@@ -1556,9 +1553,6 @@ Commander::run()
 	_last_gpos_fail_time_us = _boot_timestamp;
 	_last_lvel_fail_time_us = _boot_timestamp;
 
-	// user adjustable duration required to assert arm/disarm via throttle/rudder stick
-	uint32_t rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
-
 	int32_t airmode = 0;
 	int32_t rc_map_arm_switch = 0;
 
@@ -1633,8 +1627,6 @@ Commander::run()
 
 			_status.rc_input_mode = _param_rc_in_off.get();
 
-			rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
-
 			_arm_requirements.arm_authorization = _param_arm_auth_required.get();
 			_arm_requirements.esc_check = _param_escs_checks_required.get();
 			_arm_requirements.global_position = !_param_arm_without_gps.get();
@@ -2001,9 +1993,6 @@ Commander::run()
 			}
 		}
 
-		// update manual_control_setpoint before geofence (which might check sticks or switches)
-		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
-
 		/* start geofence result check */
 		_geofence_result_sub.update(&_geofence_result);
 
@@ -2105,32 +2094,6 @@ Commander::run()
 			_geofence_violated_prev = false;
 		}
 
-		// abort autonomous mode and switch to position mode if sticks are moved significantly
-		if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
-
-			const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT)
-							&& _vehicle_control_mode.flag_control_auto_enabled;
-
-			const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT)
-							    && _vehicle_control_mode.flag_control_offboard_enabled;
-
-			if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) {
-				const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get();
-
-				if (!_status.rc_signal_lost &&
-				    ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) ||
-				     (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
-
-					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) {
@@ -2148,9 +2111,10 @@ Commander::run()
 			}
 		}
 
-		/* RC input check */
-		if (!_status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
-		    (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
+		// Manual control input handling
+		_manual_control.setRCAllowed(!_status_flags.rc_input_blocked);
+
+		if (_manual_control.update()) {
 
 			/* handle the case where RC signal was regained */
 			if (!_status_flags.rc_signal_found_once) {
@@ -2172,101 +2136,35 @@ Commander::run()
 
 			_status.rc_signal_lost = false;
 
-			const bool in_armed_state = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			const bool arm_switch_or_button_mapped =
-				_manual_control_switches.arm_switch != manual_control_switches_s::SWITCH_POS_NONE;
-			const bool arm_button_pressed = _param_arm_switch_is_button.get()
-							&& (_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
+			const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF);
 
-			/* DISARM
-			 * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
-			 * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
-			 * do it only for rotary wings in manual mode or fixed wing if landed.
-			 * Disable stick-disarming if arming switch or button is mapped */
-			const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
-							 && (_manual_control_setpoint.z < 0.1f)
-							 && !arm_switch_or_button_mapped;
-			const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
-					(_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON) &&
-					(_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF);
-
-			if (in_armed_state &&
-			    (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
-			    (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) &&
-			    (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
-
-				const bool manual_thrust_mode = _vehicle_control_mode.flag_control_manual_enabled
-								&& !_vehicle_control_mode.flag_control_climb_rate_enabled;
-
-				const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst)
-							     || arm_switch_to_disarm_transition;
-
-				if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
-					if (arm_switch_to_disarm_transition) {
-						disarm(arm_disarm_reason_t::RC_SWITCH);
-
-					} else {
-						disarm(arm_disarm_reason_t::RC_STICK);
-					}
+			if (rc_arming_enabled) {
+				if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
+					disarm(arm_disarm_reason_t::RC_STICK);
 				}
 
-				_stick_off_counter++;
+				if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
+					if (_vehicle_control_mode.flag_control_manual_enabled) {
+						arm(arm_disarm_reason_t::RC_STICK);
 
-			} else if (!(_param_arm_switch_is_button.get()
-				     && _manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
-				/* do not reset the counter when holding the arm button longer than needed */
-				_stick_off_counter = 0;
-			}
-
-			/* ARM
-			 * check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
-			 * and we're in MANUAL mode.
-			 * Disable stick-arming if arming switch or button is mapped */
-			const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < 0.1f
-							  && !arm_switch_or_button_mapped;
-			/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
-			 * for example for accidental in-air disarming */
-			const bool in_rearming_grace_period = _param_com_rearm_grace.get() && (_last_disarmed_timestamp != 0)
-							      && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
-
-			const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
-					(_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF) &&
-					(_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) &&
-					(_manual_control_setpoint.z < 0.1f || in_rearming_grace_period);
-
-			if (!in_armed_state &&
-			    (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
-			    (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
-
-				if ((_stick_on_counter == rc_arm_hyst && _stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {
-
-					/* we check outside of the transition function here because the requirement
-					 * for being in manual mode only applies to manual arming actions.
-					 * the system can be armed in auto if armed via the GCS.
-					 */
-					if (!_vehicle_control_mode.flag_control_manual_enabled) {
-						mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Switch to a manual mode first");
+					} else {
+						mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
 						tune_negative(true);
-
-					} else {
-						if (arm_switch_to_arm_transition) {
-							arm(arm_disarm_reason_t::RC_SWITCH);
-
-						} else {
-							arm(arm_disarm_reason_t::RC_STICK);
-						}
 					}
 				}
-
-				_stick_on_counter++;
-
-			} else if (!(_param_arm_switch_is_button.get()
-				     && _manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
-				/* do not reset the counter when holding the arm button longer than needed */
-				_stick_on_counter = 0;
 			}
 
-			_last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch;
+			// 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) {
 
@@ -2333,15 +2231,16 @@ 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
 				if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
 					mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
 					_status.rc_signal_lost = true;
-					_rc_signal_lost_timestamp = _manual_control_setpoint.timestamp;
+					_rc_signal_lost_timestamp = _manual_control.getLastRCTimestamp();
 					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status);
 					_status_changed = true;
 				}

src/modules/commander/Commander.hpp

@@ -37,6 +37,7 @@
 /*   Helper classes  */
 #include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
+#include "ManualControl.hpp"
 #include "state_machine_helper.h"
 #include "worker_thread.hpp"
 
@@ -70,7 +71,6 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
@@ -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,
@@ -236,7 +235,6 @@ private:
 		(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
 
 		(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
-		(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_arm_switch_is_button,
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
 		(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
 		(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
@@ -245,10 +243,7 @@ private:
 		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
 
-		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
 		(ParamInt<px4::params::COM_RC_IN_MODE>) _param_rc_in_off,
-		(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
-		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
 
 		(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
 		(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
@@ -286,17 +281,8 @@ private:
 		ALWAYS = 2
 	};
 
-	enum OverrideMode {
-		OVERRIDE_DISABLED = 0,
-		OVERRIDE_AUTO_MODE_BIT = (1 << 0),
-		OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1)
-	};
-
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
-	static constexpr float COMMANDER_MONITORING_LOOPSPERMSEC{1 / (COMMANDER_MONITORING_INTERVAL / 1000.0f)};
-
-	static constexpr float STICK_ON_OFF_LIMIT{0.9f};
 
 	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
 	static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL{500_ms};
@@ -363,14 +349,11 @@ private:
 
 	unsigned int	_leds_counter{0};
 
-	manual_control_setpoint_s _manual_control_setpoint{};		///< the current manual control setpoint
 	manual_control_switches_s _manual_control_switches{};
 	manual_control_switches_s _last_manual_control_switches{};
+	ManualControl _manual_control{this};
 	hrt_abstime	_rc_signal_lost_timestamp{0};		///< Time at which the RC reception was lost
 	int32_t		_flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
-	uint8_t		_last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
-	uint32_t	_stick_off_counter{0};
-	uint32_t	_stick_on_counter{0};
 
 	hrt_abstime	_boot_timestamp{0};
 	hrt_abstime	_last_disarmed_timestamp{0};
@@ -416,7 +399,6 @@ private:
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
-	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_manual_control_switches_sub{ORB_ID(manual_control_switches)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};

src/modules/commander/ManualControl.cpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ManualControl.hpp"
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+enum OverrideBits {
+	OVERRIDE_AUTO_MODE_BIT = (1 << 0),
+	OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
+	OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
+};
+
+bool ManualControl::update()
+{
+	bool ret = false;
+
+	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)
+{
+	_last_manual_control_setpoint = _manual_control_setpoint;
+	_manual_control_setpoint = manual_control_setpoint;
+}
+
+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;
+
+	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)) {
+		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)
+				       || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
+				       || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
+		const bool throttle_moved =
+			(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
+		const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
+
+		if (rpy_moved || (use_throttle && throttle_moved)) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode,
+				const vehicle_status_s &vehicle_status,
+				manual_control_switches_s &manual_control_switches, const bool landed)
+{
+	bool ret = false;
+
+	// no switch or button is mapped
+	const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
+	// arm switch mapped and "switch is button" configured
+	const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
+	// arm switch mapped and "switch_is_button" configured
+	const bool use_switch = !use_stick && !use_button;
+
+	const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	const bool stick_in_lower_left = use_stick
+					 && isThrottleLow()
+					 && _manual_control_setpoint.r < -.9f;
+	const bool arm_button_pressed = (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)
+					&& use_button;
+	const bool arm_switch_to_disarm_transition = use_switch
+			&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON)
+			&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF);
+	const bool mc_manual_thrust_mode = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+					   && vehicle_control_mode.flag_control_manual_enabled
+					   && !vehicle_control_mode.flag_control_climb_rate_enabled;
+
+	if (armed
+	    && (landed || mc_manual_thrust_mode)
+	    && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
+
+		const bool last_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
+		_stick_disarm_hysteresis.set_state_and_update(true, hrt_absolute_time());
+		const bool disarm_trigger = !last_disarm_hysteresis && _stick_disarm_hysteresis.get_state()
+					    && !_stick_arm_hysteresis.get_state();
+
+		if (disarm_trigger || arm_switch_to_disarm_transition) {
+			ret = true;
+		}
+
+	} else if (!arm_button_pressed) {
+
+		_stick_disarm_hysteresis.set_state_and_update(false, hrt_absolute_time());
+	}
+
+	return ret;
+}
+
+bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
+			     manual_control_switches_s &manual_control_switches, const bool landed)
+{
+	bool ret = false;
+
+	// no switch or button is mapped
+	const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
+	// arm switch mapped and "switch is button" configured
+	const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
+	// arm switch mapped and "switch_is_button" configured
+	const bool use_switch = !use_stick && !use_button;
+
+	const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	const bool stick_in_lower_right = use_stick
+					  && isThrottleLow()
+					  && _manual_control_setpoint.r > .9f;
+	const bool arm_button_pressed = use_button
+					&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
+	const bool arm_switch_to_arm_transition = use_switch
+			&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF)
+			&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
+
+	if (!armed
+	    && (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
+
+		const bool last_arm_hysteresis = _stick_disarm_hysteresis.get_state();
+		_stick_arm_hysteresis.set_state_and_update(true, hrt_absolute_time());
+		const bool arm_trigger = !last_arm_hysteresis && _stick_arm_hysteresis.get_state()
+					 && !_stick_disarm_hysteresis.get_state();
+
+		if (arm_trigger || arm_switch_to_arm_transition) {
+			ret = true;
+		}
+
+	} else if (!arm_button_pressed) {
+
+		_stick_arm_hysteresis.set_state_and_update(false, hrt_absolute_time());
+	}
+
+	_last_manual_control_switches_arm_switch = manual_control_switches.arm_switch; // After disarm and arm check
+
+	return ret;
+}
+
+void ManualControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
+	_stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
+}

src/modules/commander/ManualControl.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ManualControl.hpp
+ *
+ * @brief Logic for handling RC or Joystick input
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/hysteresis/hysteresis.h>
+#include <px4_platform_common/module_params.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+
+class ManualControl : ModuleParams
+{
+public:
+	ManualControl(ModuleParams *parent) : ModuleParams(parent) {};
+	~ManualControl() override = default;
+
+	void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; }
+
+	/**
+	 * 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,
+			 manual_control_switches_s &manual_control_switches, const bool landed);
+	bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
+		      manual_control_switches_s &manual_control_switches, const bool landed);
+	bool isThrottleLow() { return _last_manual_control_setpoint.z < 0.1f; }
+	bool isThrottleAboveCenter() { return _last_manual_control_setpoint.z > 0.6f; }
+	hrt_abstime getLastRCTimestamp() { return _last_manual_control_setpoint.timestamp; }
+
+private:
+	void updateParams() override;
+	void process(manual_control_setpoint_s &manual_control_setpoint);
+
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	manual_control_setpoint_s _manual_control_setpoint{};
+	manual_control_setpoint_s _last_manual_control_setpoint{};
+
+	// Availability
+	bool _rc_allowed{false};
+	bool _rc_available{false};
+
+	// Arming/disarming
+	systemlib::Hysteresis _stick_disarm_hysteresis{false};
+	systemlib::Hysteresis _stick_arm_hysteresis{false};
+	uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
+		(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
+		(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
+		(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
+		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
+		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov
+	)
+};

src/modules/commander/commander_params.c

@@ -312,10 +312,11 @@ PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
 PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
 
 /**
- * Arm switch is only a button
+ * Arm switch is a momentary button
  *
- * The default uses the arm switch as real switch.
- * If parameter set button gets handled like stick arming.
+ * 0: Arming/disarming triggers on switch transition.
+ * 1: Arming/disarming triggers when holding the momentary button down
+ * for COM_RC_ARM_HYST like the stick gesture.
  *
  * @group Commander
  * @boolean
@@ -637,9 +638,10 @@ PARAM_DEFINE_INT32(COM_REARM_GRACE, 1);
  * Note: Only has an effect on multicopters, and VTOLs in multicopter mode.
  *
  * @min 0
- * @max 3
+ * @max 7
  * @bit 0 Enable override during auto modes (except for in critical battery reaction)
  * @bit 1 Enable override during offboard mode
+ * @bit 2 Ignore throttle stick
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
@@ -647,8 +649,8 @@ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
 /**
  * RC stick override threshold
  *
- * If COM_RC_OVERRIDE is enabled and the joystick input controlling the horizontally axis (right stick for RC in mode 2)
- * is moved more than this threshold from the center the autopilot switches to position mode and the pilot takes over control.
+ * If COM_RC_OVERRIDE is enabled and the joystick input is moved more than this threshold
+ * the autopilot the pilot takes over control.
  *
  * @group Commander
  * @unit %