Correct RC config sanity checking and report back when RC config errors occur.

5 files changed, 55 insertions, 21 deletions

diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index ed74cb3d3..0fba2cbe5 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -170,9 +170,11 @@ mixer_tick(void)
 	 * XXX correct behaviour for failsafe may require an additional case
 	 * here.
 	 */
-	bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
-	 		   /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-	/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
+	bool should_arm = (
+	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
+		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK));
 
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 0ee6d2c4b..14d221b5b 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -103,6 +103,7 @@
 #define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
+#define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 589264661..a0e0002a6 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -153,6 +153,9 @@ user_start(int argc, char *argv[])
 	/* configure the first 8 PWM outputs (i.e. all of them) */
 	up_pwm_servo_init(0xff);
 
+	/* initialise the registry space */
+	registers_init();
+
 	/* initialise the control inputs */
 	controls_init();
 
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 5ec2f7258..dac09021d 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -180,6 +180,12 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
 uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
 
 void
+registers_init(void)
+{
+	r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+}
+
+void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
 
@@ -390,27 +396,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			/* should the channel be enabled? */
 			/* this option is normally set last */
 			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+				uint8_t count = 0;
+
 				/* assert min..center..max ordering */
-				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500)
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
-					break;
+				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
+					count++;
+				}
+
 				/* assert deadzone is sane */
-				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
-					break;
+				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
+					count++;
+				}
+				// The following check isn't needed as constraint checks in controls.c will catch this.
+				//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+				//	count++;
+				//}
+				//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+				//	count++;
+				//}
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+					count++;
+				}
 
 				/* sanity checks pass, enable channel */
-				conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+				if (count) {
+					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
+					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
+				} else {
+					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+				}
 			}
 			break;
 			/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 540636e19..5495d5ae1 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -176,12 +176,17 @@ heartbeat_blink(void *arg)
 static void
 failsafe_blink(void *arg)
 {
+	/* indicate that a serious initialisation error occured */
+	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
+		LED_AMBER(true);
+	    return;
+    }
+
 	static bool failsafe = false;
 
 	/* blink the failsafe LED if we don't have FMU input */
 	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 		failsafe = !failsafe;
-
 	} else {
 		failsafe = false;
 	}