From cb3a4f12670ce8d6cf608eb0ec97298a3bcb0919 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <hsteinhaus@gmx.de>
Date: Tue, 25 Mar 2014 15:23:40 +0100
Subject: px4io driver: publish input_rc even if RC connection has been lost

---
 src/drivers/px4io/px4io.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 82f3ba044..2c0078503 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1479,10 +1479,9 @@ 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))
-			return OK;
+		/* we do not know the RC input, but have to publish timestamp_published 
+		 * and rc_lost flag, so do not prematurely return here
+		 */
 	}
 
 	/* lazily advertise on first publication */
-- 
cgit v1.2.3


From 409fa565f48ffe164b7332c9186a876b2771922a Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <hsteinhaus@gmx.de>
Date: Tue, 25 Mar 2014 15:42:44 +0100
Subject: px4io: do not include failsafe condition into rc_lost flag

---
 src/modules/px4iofirmware/controls.c | 158 +++++++++++++++++------------------
 1 file changed, 77 insertions(+), 81 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 941500f0d..609dd3312 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -201,94 +201,90 @@ 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;
+
+					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 +312,8 @@ 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)) {
+	/* 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 |
-- 
cgit v1.2.3


From c6d98a32f83383e6204fd6cefbfcc1fd7e1cf159 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 10:27:43 +0200
Subject: Proper failsafe handling onboard, including throttle failsafe
 condition if enabled

---
 src/modules/px4iofirmware/controls.c | 36 ++++++++++++++++++++++++------------
 src/modules/px4iofirmware/protocol.h |  9 +++++----
 2 files changed, 29 insertions(+), 16 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 609dd3312..b860fc587 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);
 
@@ -261,8 +259,20 @@ controls_tick() {
 				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
 					/* 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 */
+					if (mapped == 1) {
+						/* roll, pitch, yaw, throttle, override is the standard order */
 						scaled = -scaled;
+					}
+
+					if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) {
+						/* throttle failsafe detection */
+						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) ||
+						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) {
+							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);
@@ -312,6 +322,11 @@ controls_tick() {
 	 * Handle losing RC input
 	 */
 
+	/* 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 */
@@ -322,27 +337,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;
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..b69f68107 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -164,10 +164,11 @@
 /* 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_FEATURES_RC_FAIL_DETECT	(1 << 4) /**< enable RC fail detection based on channel value */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
-- 
cgit v1.2.3


From 64ffafb48ee45452070dfb37be8d0de6098915a8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 10:28:01 +0200
Subject: Only publish RC inputs if we have seen some valid inputs at some
 point

---
 src/drivers/px4io/px4io.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 2c0078503..8eee1cbca 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-		/* we do not know the RC input, but have to publish timestamp_published 
-		 * and rc_lost flag, so do not prematurely return here
-		 */
+		/* 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 */
-- 
cgit v1.2.3


From fb44ad8e22f2a0862f1d7bda66643a6336247024 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 10:29:17 +0200
Subject: Simplify the failsafe handling, reduce 3 params to one

---
 src/modules/sensors/sensor_params.c | 28 +++-------------------------
 src/modules/sensors/sensors.cpp     | 26 +++++++-------------------
 2 files changed, 10 insertions(+), 44 deletions(-)

(limited to 'src')

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 288c6e00a..e4564aa25 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_FLOAT(RC_FS_THR, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f890c4c7f..2442acd6b 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -263,8 +263,6 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
-		int rc_fs_ch;
-		int rc_fs_mode;
 		float rc_fs_thr;
 
 		float battery_voltage_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,8 +527,6 @@ 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");
 
 	/* gyro offsets */
@@ -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.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
+			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (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;
-- 
cgit v1.2.3


From 797698a7a114032c7eea62c5f48f2b229ca973b7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 10:34:35 +0200
Subject: Trigger failsafe action also on failsafe flag

---
 src/modules/sensors/sensors.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 2442acd6b..4c34d853f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1309,7 +1309,7 @@ Sensors::rc_poll()
 		}
 
 		/* check for failsafe */
-		if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
+		if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
 			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) {
 			/* do not publish manual control setpoints when there are none */
 			return;
-- 
cgit v1.2.3


From 9123ebce8cbc618899ece31d31c97e022038beb2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 10:45:02 +0200
Subject: px4io: Allow RC failsafe detection as valid feature

---
 src/modules/px4iofirmware/registers.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 97d25bbfa..42b863b53 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -175,7 +175,8 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
 					 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
 					 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
-					 PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+					 PX4IO_P_SETUP_FEATURES_PWM_RSSI | \
+					 PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)
 #else
 #define PX4IO_P_SETUP_FEATURES_VALID	0
 #endif
-- 
cgit v1.2.3


From 9a0b2b7610d39f88b627046e0d90f66aada1e88f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 11:02:22 +0200
Subject: Make throttle failsafe depend on the failsafe threshold parameter.
 Make the parameter optional (no harm if not found).

---
 src/drivers/px4io/px4io.cpp           | 17 ++++++++++++++++-
 src/modules/px4iofirmware/controls.c  |  6 +++---
 src/modules/px4iofirmware/protocol.h  | 21 +++++++++++----------
 src/modules/px4iofirmware/px4io.h     |  1 +
 src/modules/px4iofirmware/registers.c |  4 ++--
 5 files changed, 33 insertions(+), 16 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 8eee1cbca..f6125d273 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 */
+				float failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FS_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");
+					}
+				}
+
 			}
 
 		}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index b860fc587..356fe44cd 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -264,10 +264,10 @@ controls_tick() {
 						scaled = -scaled;
 					}
 
-					if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) {
+					if (mapped == 3 && r_setup_rc_thr_failsafe) {
 						/* throttle failsafe detection */
-						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) ||
-						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) {
+						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);
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index b69f68107..a978d483a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -168,7 +168,6 @@
 #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_RC_FAIL_DETECT	(1 << 4) /**< enable RC fail detection based on channel value */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -202,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 */
@@ -218,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..88ad79398 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 42b863b53..6752e7b4b 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -169,14 +169,14 @@ 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
 #define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
 					 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
 					 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
-					 PX4IO_P_SETUP_FEATURES_PWM_RSSI | \
-					 PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)
+					 PX4IO_P_SETUP_FEATURES_PWM_RSSI)
 #else
 #define PX4IO_P_SETUP_FEATURES_VALID	0
 #endif
-- 
cgit v1.2.3


From 5e0d687b566022c12270f68facbf7ca35f62306c Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <hsteinhaus@gmx.de>
Date: Tue, 25 Mar 2014 15:23:40 +0100
Subject: px4io driver: publish input_rc even if RC connection has been lost

---
 src/drivers/px4io/px4io.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 82f3ba044..2c0078503 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1479,10 +1479,9 @@ 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))
-			return OK;
+		/* we do not know the RC input, but have to publish timestamp_published 
+		 * and rc_lost flag, so do not prematurely return here
+		 */
 	}
 
 	/* lazily advertise on first publication */
-- 
cgit v1.2.3


From 745ef4f4856af60aca5625860e5e3f25ea189dc9 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <hsteinhaus@gmx.de>
Date: Tue, 25 Mar 2014 15:42:44 +0100
Subject: px4io: do not include failsafe condition into rc_lost flag

---
 src/modules/px4iofirmware/controls.c | 158 +++++++++++++++++------------------
 1 file changed, 77 insertions(+), 81 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 941500f0d..609dd3312 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -201,94 +201,90 @@ 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;
+
+					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 +312,8 @@ 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)) {
+	/* 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 |
-- 
cgit v1.2.3


From 09d106432749ec02866241863eb912f05b903e64 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 11:06:07 +0200
Subject: px4io: Remove unused variable

---
 src/modules/px4iofirmware/controls.c | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 609dd3312..f56f630bc 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);
 
-- 
cgit v1.2.3


From 3e4841b6fe2d8d6d06b167be49cbe76ab7e04a46 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 11:10:41 +0200
Subject: px4io: Guard against the RC failsafe value of channel 5 causing a
 manual override action if set to manual in failsafe

---
 src/modules/px4iofirmware/controls.c | 24 +++++++++++++-----------
 1 file changed, 13 insertions(+), 11 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index f56f630bc..aae7bb951 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -310,6 +310,11 @@ controls_tick() {
 	 * Handle losing RC input
 	 */
 
+	/* 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 */
@@ -320,27 +325,24 @@ controls_tick() {
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 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;
+
+		/* Set the RC_LOST alarm */
+		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 	}
 
 	/*
 	 * 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;
 
@@ -363,10 +365,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);
 	}
 }
 
-- 
cgit v1.2.3


From 73d04f7a37ba52ce1891e740db554557bd71940a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 11:15:22 +0200
Subject: px4io driver: Only publish RC signal if it was at least once valid.

---
 src/drivers/px4io/px4io.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 2c0078503..8eee1cbca 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-		/* we do not know the RC input, but have to publish timestamp_published 
-		 * and rc_lost flag, so do not prematurely return here
-		 */
+		/* 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 */
-- 
cgit v1.2.3


From 3b5e6f98338fded2cbe7be1c301dc2698f7239aa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 11:28:07 +0200
Subject: sensors and px4io driver: Guard against failsafe trigger for inverted
 remotes

---
 src/drivers/px4io/px4io.cpp         | 4 ++--
 src/modules/sensors/sensor_params.c | 2 +-
 src/modules/sensors/sensors.cpp     | 8 ++++----
 3 files changed, 7 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f6125d273..a30bfb2de 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -948,8 +948,8 @@ PX4IO::task_main()
 				}
 
 				/* send RC throttle failsafe value to IO */
-				float failsafe_param_val;
-				param_t failsafe_param = param_find("RC_FS_THR");
+				int32_t failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FAILS_THR");
 
 				if (failsafe_param > 0)
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index e4564aa25..14f7ac812 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -675,4 +675,4 @@ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
  * @max 2200
  * @group Radio Calibration
  */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 0);
+PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4c34d853f..44a91ca67 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -263,7 +263,7 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
-		float rc_fs_thr;
+		int32_t rc_fs_thr;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -527,7 +527,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
 	/* RC failsafe */
-	_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");
@@ -1309,8 +1309,8 @@ Sensors::rc_poll()
 		}
 
 		/* check for failsafe */
-		if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
-			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) {
+		if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
+			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) {
 			/* do not publish manual control setpoints when there are none */
 			return;
 		}
-- 
cgit v1.2.3


From 671d35f67ccae5c35ef4fcb573f5fa52395601c4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 16:05:58 +0200
Subject: Fix logic for S.Bus failsafe detection

---
 src/modules/sensors/sensors.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 44a91ca67..c91006262 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1309,8 +1309,8 @@ Sensors::rc_poll()
 		}
 
 		/* check for failsafe */
-		if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
-			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (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[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
+			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
 			/* do not publish manual control setpoints when there are none */
 			return;
 		}
-- 
cgit v1.2.3


From 7b95d36405cb63b53fd1fea2c25e29aedca5a3a2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 16:45:23 +0200
Subject: navigator hotfix: Increase acceptance range for yaw setpoints.

---
 src/modules/navigator/navigator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

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;
 			}
 
-- 
cgit v1.2.3


From 024de1fec431fbd065aeb31035245e7851450a0b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 17:04:36 +0200
Subject: Remove unwanted colon

---
 src/modules/px4iofirmware/px4io.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 88ad79398..4db948484 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -108,7 +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_setup_rc_thr_failsafe	r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
 
 #define r_control_values	(&r_page_controls[0])
 
-- 
cgit v1.2.3


From 262485a5e87ccdc8ff645ba45992bdbe13363fab Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 17:09:48 +0200
Subject: px4io: Typo fixed

---
 src/drivers/px4io/px4io.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index a30bfb2de..e318e206a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -951,7 +951,7 @@ PX4IO::task_main()
 				int32_t failsafe_param_val;
 				param_t failsafe_param = param_find("RC_FAILS_THR");
 
-				if (failsafe_param > 0)
+				if (failsafe_param > 0) {
 
 					param_get(failsafe_param, &failsafe_param_val);
 					uint16_t failsafe_thr = failsafe_param_val;
-- 
cgit v1.2.3


From e6d48c4f3276af252315118286556b3a000274b6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 17:12:21 +0200
Subject: Fix failsafe assignment in sensors app

---
 src/modules/sensors/sensors.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c91006262..b171b995d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1309,8 +1309,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[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
-			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (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[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;
 		}
-- 
cgit v1.2.3