add RC kill switch

msg/manual_control_setpoint.msg

@@ -38,8 +38,9 @@ float32 aux5			 # default function: payload drop
 
 uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
 uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
-uint8 rattitude_switch		 # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
+uint8 rattitude_switch	 # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
 uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
 uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
 uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
-uint8 offboard_switch		 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
+uint8 offboard_switch	 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
+uint8 kill_switch		 # throttle kill: _NORMAL_, KILL

msg/rc_channels.msg

@@ -1,4 +1,4 @@
-int32 RC_CHANNELS_FUNCTION_MAX=20
+int32 RC_CHANNELS_FUNCTION_MAX=21
 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
 uint8 RC_CHANNELS_FUNCTION_ROLL=1
 uint8 RC_CHANNELS_FUNCTION_PITCH=2
@@ -19,11 +19,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
+uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
 uint64 timestamp						# Timestamp in microseconds since boot time
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
-float32[20] channels						# Scaled to -1..1 (throttle: 0..1)
+float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[20] function						# Functions mapping
+int8[21] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames

src/modules/commander/commander.cpp

@@ -2284,6 +2284,18 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "main state transition denied");
 			}
 
+			/* check throttle kill switch */
+			int prevLockdown = armed.lockdown;
+			if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+				/* set lockdown flag */
+				armed.lockdown = TRUE;
+			} else {
+				armed.lockdown = FALSE;
+			}
+			if (prevLockdown != armed.lockdown) {
+				warnx("armed.lockdown: %d\n", armed.lockdown);
+			}
+
 		} else {
 			if (!status.rc_input_blocked && !status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);

src/modules/sensors/sensor_params.c

@@ -2009,6 +2009,15 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
+/**
+ * Kill switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Switches
+ */
+PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0);
+
 /**
  * Flaps channel mapping.
  *
@@ -2243,6 +2252,25 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
  */
 PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
 
+
+/**
+ * Threshold for selecting offboard mode
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
+
 /**
  * PWM input channel that provides RSSI.
  *

src/modules/sensors/sensors.cpp

@@ -277,6 +277,7 @@ private:
 		int rc_map_loiter_sw;
 		int rc_map_acro_sw;
 		int rc_map_offboard_sw;
+		int rc_map_kill_sw;
 
 		int rc_map_flaps;
 
@@ -297,6 +298,7 @@ private:
 		float rc_loiter_th;
 		float rc_acro_th;
 		float rc_offboard_th;
+		float rc_killswitch_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_rattitude_inv;
@@ -305,6 +307,7 @@ private:
 		bool rc_loiter_inv;
 		bool rc_acro_inv;
 		bool rc_offboard_inv;
+		bool rc_killswitch_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -336,6 +339,7 @@ private:
 		param_t rc_map_loiter_sw;
 		param_t rc_map_acro_sw;
 		param_t rc_map_offboard_sw;
+		param_t rc_map_kill_sw;
 
 		param_t rc_map_flaps;
 
@@ -360,6 +364,7 @@ private:
 		param_t rc_loiter_th;
 		param_t rc_acro_th;
 		param_t rc_offboard_th;
+		param_t rc_killswitch_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -589,6 +594,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 	_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
+	_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -614,6 +620,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
 	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
 	_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
+	_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
 
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
@@ -774,6 +781,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_kill_sw, &(_parameters.rc_map_kill_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -813,6 +824,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
 	_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
 	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
+	param_get(_parameter_handles.rc_killswitch_th, &(_parameters.rc_killswitch_th));
+	_parameters.rc_killswitch_inv = (_parameters.rc_killswitch_th < 0);
+	_parameters.rc_killswitch_th = fabs(_parameters.rc_killswitch_th);
 
 	/* update RC function mappings */
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -827,6 +841,7 @@ Sensors::parameters_update()
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
 
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1955,6 +1970,8 @@ Sensors::rc_poll()
 					     _parameters.rc_acro_inv);
 			manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
 						 _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+			manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
+					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub != nullptr) {