From 86a0862af6412906611ed295cae4604e7111b1e9 Mon Sep 17 00:00:00 2001
From: TickTock- <lukecell@safe-mail.net>
Date: Sat, 19 Apr 2014 13:07:09 -0700
Subject: [PATCH] Added rc_map_failsafe to enable use of channels other than
 throttle for failsafe.

---
 src/modules/sensors/sensor_params.c | 29 +++++++++++++++++++++++++++++
 src/modules/sensors/sensors.cpp     | 19 +++++++++++--------
 2 files changed, 40 insertions(+), 8 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 14f7ac812f..ff121c51ef 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -535,6 +535,35 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
  */
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 
+/**
+ * Failsafe channel mapping.
+ *
+ * The RC mapping index indicates which rc function
+ * should be used for detecting the failsafe condition
+ *
+ * @min 0
+ * @max 14
+ *
+ * mapping (from rc_channels.h)
+ * 	THROTTLE = 0,
+	ROLL     = 1,
+	PITCH    = 2,
+	YAW      = 3,
+	MODE = 4,
+	RETURN = 5,
+	ASSISTED = 6,
+	MISSION = 7,
+	OFFBOARD_MODE = 8,
+	FLAPS   = 9,
+	AUX_1   = 10,
+	AUX_2   = 11,
+	AUX_3   = 12,
+	AUX_4   = 13,
+	AUX_5   = 14,
+ *
+ */
+PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0);  //Default to throttle function
+
 /**
  * Throttle control channel mapping.
  *
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4083b8b4f5..fa3905f608 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -242,6 +242,7 @@ private:
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
+		int rc_map_failsafe;
 
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
@@ -290,6 +291,7 @@ private:
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
+		param_t rc_map_failsafe;
 
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
@@ -512,6 +514,7 @@ Sensors::Sensors() :
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
 	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+	_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
@@ -650,6 +653,10 @@ Sensors::parameters_update()
 		warnx(paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
+		warnx(paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
 		warnx(paramerr);
 	}
@@ -704,7 +711,6 @@ Sensors::parameters_update()
 	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
 	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
 	_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
-	_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
 
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
@@ -1310,8 +1316,8 @@ Sensors::rc_poll()
 		}
 
 		/* check for failsafe */
-		if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
-			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
+		if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.min[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.rc_fs_thr))
+			|| ((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.max[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.rc_fs_thr))))) {
 			/* do not publish manual control setpoints when there are none */
 			return;
 		}
@@ -1434,7 +1440,8 @@ Sensors::rc_poll()
 			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
 		}
 
-//		if (_rc.function[OFFBOARD_MODE] >= 0) {
+
+		//		if (_rc.function[OFFBOARD_MODE] >= 0) {
 //			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
 //		}
 
@@ -1455,10 +1462,6 @@ Sensors::rc_poll()
 			manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
 		}
 
-		if (_rc.function[AUX_5] >= 0) {
-			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
-		}
-
 		/* copy from mapped manual control to control group 3 */
 		actuator_group_3.control[0] = manual_control.roll;
 		actuator_group_3.control[1] = manual_control.pitch;