Merge branch 'master' into airspeed_scaling

8 files changed, 152 insertions, 156 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 82f3ba044..e318e206a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -944,8 +944,23 @@ PX4IO::task_main()
 				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
 
 				if (pret != OK) {
-					log("voltage scaling upload failed");
+					log("vscale upload failed");
 				}
+
+				/* send RC throttle failsafe value to IO */
+				int32_t failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FAILS_THR");
+
+				if (failsafe_param > 0) {
+
+					param_get(failsafe_param, &failsafe_param_val);
+					uint16_t failsafe_thr = failsafe_param_val;
+					pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+					if (pret != OK) {
+						log("failsafe upload failed");
+					}
+				}
+
 			}
 
 		}
@@ -1479,10 +1494,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-		/* we do not know the RC input, only publish if RC OK flag is set */
-		/* if no raw RC, just don't publish */
-		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		/* only keep publishing RC input if we ever got a valid input */
+		if (_rc_last_valid == 0) {
+			/* we have never seen valid RC signals, abort */
 			return OK;
+		}
 	}
 
 	/* lazily advertise on first publication */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c45cafc1b..5a94b6671 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
 
-			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 941500f0d..56c5aa005 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -134,8 +134,6 @@ controls_tick() {
 
 	perf_begin(c_gather_sbus);
 
-	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
-
 	bool sbus_failsafe, sbus_frame_drop;
 	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
 
@@ -201,94 +199,104 @@ controls_tick() {
 		/* update RC-received timestamp */
 		system_state.rc_channels_timestamp_received = hrt_absolute_time();
 
-		/* do not command anything in failsafe, kick in the RC loss counter */
-		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
-
-			/* update RC-received timestamp */
-			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
-
-			/* map raw inputs to mapped inputs */
-			/* XXX mapping should be atomic relative to protocol */
-			for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
-				/* map the input channel */
-				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
-
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
-
-					uint16_t raw = r_raw_rc_values[i];
-
-					int16_t scaled;
-
-					/*
-					 * 1) Constrain to min/max values, as later processing depends on bounds.
-					 */
-					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-						raw = conf[PX4IO_P_RC_CONFIG_MIN];
-					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-						raw = conf[PX4IO_P_RC_CONFIG_MAX];
-
-					/*
-					 * 2) Scale around the mid point differently for lower and upper range.
-					 *
-					 * This is necessary as they don't share the same endpoints and slope.
-					 *
-					 * First normalize to 0..1 range with correct sign (below or above center),
-					 * then scale to 20000 range (if center is an actual center, -10000..10000,
-					 * if parameters only support half range, scale to 10000 range, e.g. if
-					 * center == min 0..10000, if center == max -10000..0).
-					 *
-					 * As the min and max bounds were enforced in step 1), division by zero
-					 * cannot occur, as for the case of center == min or center == max the if
-					 * statement is mutually exclusive with the arithmetic NaN case.
-					 *
-					 * DO NOT REMOVE OR ALTER STEP 1!
-					 */
-					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
-
-					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
-
-					} else {
-						/* in the configured dead zone, output zero */
-						scaled = 0;
-					}
-
-					/* invert channel if requested */
-					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-						scaled = -scaled;
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+				uint16_t raw = r_raw_rc_values[i];
+
+				int16_t scaled;
+
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+					raw = conf[PX4IO_P_RC_CONFIG_MIN];
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}
 
-					/* and update the scaled/mapped version */
-					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-					if (mapped < PX4IO_CONTROL_CHANNELS) {
+				/* invert channel if requested */
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
+					scaled = -scaled;
+				}
 
-						/* invert channel if pitch - pulling the lever down means pitching up by convention */
-						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-							scaled = -scaled;
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-						assigned_channels |= (1 << mapped);
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) {
+						/* roll, pitch, yaw, throttle, override is the standard order */
+						scaled = -scaled;
+					}
 
+					if (mapped == 3 && r_setup_rc_thr_failsafe) {
+						/* throttle failsafe detection */
+						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
+						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
+							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+						} else {
+							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+						}
 					}
+
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+
 				}
 			}
+		}
 
-			/* set un-assigned controls to zero */
-			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-				if (!(assigned_channels & (1 << i)))
-					r_rc_values[i] = 0;
+		/* set un-assigned controls to zero */
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+			if (!(assigned_channels & (1 << i))) {
+				r_rc_values[i] = 0;
 			}
+		}
 
-			/* set RC OK flag, as we got an update */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		/* set RC OK flag, as we got an update */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
 
-			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
-			if (assigned_channels > 4) {
-				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
-			} else {
-				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
-			}
+		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+		if (assigned_channels > 4) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}
 
 		/*
@@ -316,8 +324,13 @@ controls_tick() {
 	 * Handle losing RC input
 	 */
 
-	/* this kicks in if the receiver is gone or the system went to failsafe */
-	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	/* if we are in failsafe, clear the override flag */
+	if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
+	}
+
+	/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
+	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
@@ -326,27 +339,24 @@ controls_tick() {
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;
 
+		/* Set raw channel count to zero */
+		r_raw_rc_count = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 	}
 
-	/* this kicks in if the receiver is completely gone */
-	if (rc_input_lost) {
-
-		/* Set channel count to zero */
-		r_raw_rc_count = 0;
-	}
-
 	/*
 	 * Check for manual override.
 	 *
 	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
-	 * must have R/C input.
+	 * must have R/C input (NO FAILSAFE!).
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
 		bool override = false;
 
@@ -369,10 +379,10 @@ controls_tick() {
 				mixer_tick();
 
 		} else {
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
 	} else {
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
 }
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..a978d483a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -164,10 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
-#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
-#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
-#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
-#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -201,13 +201,15 @@ enum {							/* DSM bind states */
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
- 					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
+						/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
-#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
-#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */
 
-#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+						/* 12 occupied by CRC */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
@@ -217,10 +219,10 @@ enum {							/* DSM bind states */
 #define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 
 #define PX4IO_P_CONTROLS_GROUP_VALID		64
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */
 
 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD			52
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 54c5663a5..4db948484 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -108,6 +108,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 #endif
+#define r_setup_rc_thr_failsafe	r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
 
 #define r_control_values	(&r_page_controls[0])
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 97d25bbfa..6752e7b4b 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -169,6 +169,7 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
+	[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 288c6e00a..14f7ac812 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
  */
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
 
-
-/**
- * Failsafe channel mapping.
- *
- * @min 0
- * @max 18
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_CH, 0);
-
-/**
- * Failsafe channel mode.
- *
- * 0 = too low means signal loss,
- * 1 = too high means signal loss
- *
- * @min 0
- * @max 1
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);
-
 /**
  * Failsafe channel PWM threshold.
  *
- * @min 0
- * @max 1
+ * @min 800
+ * @max 2200
  * @group Radio Calibration
  */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
+PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f890c4c7f..b171b995d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -263,9 +263,7 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
-		int rc_fs_ch;
-		int rc_fs_mode;
-		float rc_fs_thr;
+		int32_t rc_fs_thr;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -313,8 +311,6 @@ private:
 		param_t rc_scale_yaw;
 		param_t rc_scale_flaps;
 
-		param_t rc_fs_ch;
-		param_t rc_fs_mode;
 		param_t rc_fs_thr;
 
 		param_t battery_voltage_scaling;
@@ -531,9 +527,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
 	/* RC failsafe */
-	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
-	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
-	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -689,8 +683,6 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
 	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
-	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
-	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
 	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
 
 	/* update RC function mappings */
@@ -1312,19 +1304,15 @@ Sensors::rc_poll()
 		manual_control.aux5 = NAN;
 
 		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4)
+		if (rc_input.channel_count < 4) {
 			return;
+		}
 
-		/* failsafe check */
-		if (_parameters.rc_fs_ch != 0) {
-			if (_parameters.rc_fs_mode == 0) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
-					return;
-
-			} else if (_parameters.rc_fs_mode == 1) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
-					return;
-			}
+		/* 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))))) {
+			/* do not publish manual control setpoints when there are none */
+			return;
 		}
 
 		unsigned channel_limit = rc_input.channel_count;