Merge branch 'px4io-i2c-throttle' of github.com:PX4/Firmware into px4io-i2c-throttle

4 files changed, 43 insertions, 31 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 2611c4e9c..8fb53295f 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -83,6 +83,7 @@
 
 #include <px4io/protocol.h>
 #include "uploader.h"
+#include <debug.h>
 
 
 class PX4IO : public device::I2C
@@ -771,9 +772,16 @@ PX4IO::io_set_rc_config()
 		/* send channel config to IO */
 		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
 		if (ret != OK) {
-			log("RC config update failed");
+			log("rc config upload failed");
 			break;
 		}
+
+		/* check the IO initialisation flag */
+		if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
+			log("config for RC%d rejected by IO", i + 1);
+			break;
+		}
+
 		offset += PX4IO_P_RC_CONFIG_STRIDE;
 	}
 
@@ -1186,7 +1194,7 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
 		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@@ -1197,7 +1205,8 @@ PX4IO::print_status()
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"));
+		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
+		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
 	printf("alarms 0x%04x%s%s%s%s%s%s\n",
 		alarms,
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index dabdde0af..908334c47 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -147,32 +147,43 @@ controls_tick() {
 
 				uint16_t raw = r_raw_rc_values[i];
 
-				/* implement the deadzone */
-				if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) {
-					raw += conf[PX4IO_P_RC_CONFIG_DEADZONE];
-					if (raw > conf[PX4IO_P_RC_CONFIG_CENTER])
-						raw = conf[PX4IO_P_RC_CONFIG_CENTER];
-				}
-				if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) {
-					raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE];
-					if (raw < conf[PX4IO_P_RC_CONFIG_CENTER])
-						raw = conf[PX4IO_P_RC_CONFIG_CENTER];
-				}
+				int16_t scaled;
 
-				/* constrain to min/max values */
+				/*
+				 * 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];
 
-				int16_t scaled = raw;
-
-				/* adjust to zero-relative around center (nominal -500..500) */
-				scaled -= conf[PX4IO_P_RC_CONFIG_CENTER];
-
-				/* scale to fixed-point representation (-10000..10000) */
-				scaled *= 20;
+				/*
+				 * 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 center is min 0..20000, if center is max -20000..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 = 20.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 = 20.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;
 
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index a0e0002a6..589264661 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -153,9 +153,6 @@ 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 dac09021d..d97fd8d86 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -180,12 +180,6 @@ 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)
 {
 
@@ -389,6 +383,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		case PX4IO_P_RC_CONFIG_OPTIONS:
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;