commander auto disarm if kill switch engaged for 5 seconds

- closes #10133

src/modules/commander/Commander.cpp

@@ -74,10 +74,7 @@
 #include <px4_tasks.h>
 #include <px4_time.h>
 #include <circuit_breaker/circuit_breaker.h>
-#include <systemlib/err.h>
-#include <systemlib/hysteresis/hysteresis.h>
 #include <systemlib/mavlink_log.h>
-#include <parameters/param.h>
 
 #include <cmath>
 #include <float.h>
@@ -140,8 +137,6 @@ static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
 
-static systemlib::Hysteresis auto_disarm_hysteresis(false);
-
 static float min_stick_change = 0.25f;
 
 static struct vehicle_status_s status = {};
@@ -561,6 +556,8 @@ Commander::Commander() :
 	ModuleParams(nullptr),
 	_failure_detector(this)
 {
+	_auto_disarm_landed.set_hysteresis_time_from(false, 10_s);
+	_auto_disarm_killed.set_hysteresis_time_from(false, 5_s);
 	_battery_sub = orb_subscribe(ORB_ID(battery_status));
 }
 
@@ -1742,26 +1739,41 @@ Commander::run()
 			was_falling = land_detector.freefall;
 		}
 
-		// Auto disarm when landed
-		if (!have_taken_off_since_arming) {
-			// pilot has ten seconds time to take off
-			auto_disarm_hysteresis.set_hysteresis_time_from(false, 10_s);
+
+		// Auto disarm when landed or kill switch engaged
+		if (armed.armed) {
+
+			// Check for auto-disarm on landing
+			if (_disarm_when_landed_timeout.get() > 0) {
+
+				if (!have_taken_off_since_arming) {
+					// pilot has ten seconds time to take off
+					_auto_disarm_landed.set_hysteresis_time_from(false, 10_s);
+
+				} else {
+					_auto_disarm_landed.set_hysteresis_time_from(false, _disarm_when_landed_timeout.get() * 1_s);
+				}
+
+				_auto_disarm_landed.set_state_and_update(land_detector.landed);
+
+				if (_auto_disarm_landed.get_state()) {
+					arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
+				}
+			}
+
+
+			// Auto disarm after 5 seconds if kill switch is engaged
+			_auto_disarm_killed.set_state_and_update(armed.manual_lockdown);
+
+			if (_auto_disarm_killed.get_state()) {
+				arm_disarm(false, &mavlink_log_pub, "kill switch still engaged, disarming");
+			}
 
 		} else {
-			auto_disarm_hysteresis.set_hysteresis_time_from(false, _disarm_when_landed_timeout.get() * 1_s);
+			_auto_disarm_landed.set_state_and_update(false);
+			_auto_disarm_killed.set_state_and_update(false);
 		}
 
-		// Check for auto-disarm
-		if (armed.armed && land_detector.landed && _disarm_when_landed_timeout.get() > FLT_EPSILON) {
-			auto_disarm_hysteresis.set_state_and_update(true);
-
-		} else {
-			auto_disarm_hysteresis.set_state_and_update(false);
-		}
-
-		if (auto_disarm_hysteresis.get_state()) {
-			arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
-		}
 
 		if (!warning_action_on) {
 			// store the last good main_state when not in an navigation

src/modules/commander/Commander.hpp

@@ -185,6 +185,9 @@ private:
 
 	void battery_status_check();
 
+	systemlib::Hysteresis	_auto_disarm_landed{false};
+	systemlib::Hysteresis	_auto_disarm_killed{false};
+
 	// Subscriptions
 	Subscription<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
 	Subscription<iridiumsbd_status_s> 		_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};

src/modules/sensors/rc_params.c

@@ -1528,7 +1528,7 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
 /**
- * Kill switch channel
+ * Emergency Kill switch channel
  *
  * @min 0
  * @max 18