Major workover of the PX4IO firmware for I2C operation.

11 files changed, 549 insertions, 308 deletions

diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile
index d97f963ab..0eb3ffcf7 100644
--- a/apps/px4io/Makefile
+++ b/apps/px4io/Makefile
@@ -39,11 +39,19 @@
 # Note that we pull a couple of specific files from the systemlib, since
 # we can't support it all.
 #
-CSRCS		= $(wildcard *.c) \
+CSRCS		= adc.c \
+		  controls.c \
+		  dsm.c \
+		  i2c.c \
+		  px4io.c \
+		  registers.c \
+		  safety.c \
+		  sbus.c \
 		  ../systemlib/hx_stream.c \
 		  ../systemlib/perf_counter.c \
 		  ../systemlib/up_cxxinitialize.c
-CXXSRCS		= $(wildcard *.cpp)
+
+CXXSRCS		= mixer.cpp
 
 INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
 
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 169d5bb81..072d296da 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -37,34 +37,22 @@
  * R/C inputs and servo outputs.
  */
 
-
 #include <nuttx/config.h>
-#include <stdio.h>
 #include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <termios.h>
-#include <string.h>
 #include <poll.h>
 
-#include <nuttx/clock.h>
-
 #include <drivers/drv_hrt.h>
-#include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 
-#define DEBUG
+//#define DEBUG
 #include "px4io.h"
 
 #define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
-#define RC_CHANNEL_HIGH_THRESH		1700
-#define RC_CHANNEL_LOW_THRESH		1300
+#define RC_CHANNEL_HIGH_THRESH		5000
+#define RC_CHANNEL_LOW_THRESH		-5000
 
-static void	ppm_input(void);
+static bool	ppm_input(uint16_t *values, uint16_t *num_values);
 
 void
 controls_main(void)
@@ -79,9 +67,26 @@ controls_main(void)
 	fds[1].fd = sbus_init("/dev/ttyS2");
 	fds[1].events = POLLIN;
 
+	ASSERT(fds[0].fd >= 0);
+	ASSERT(fds[1].fd >= 0);
+
+	/* default to a 1:1 input map */
+	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN]        = 1000;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER]     = 1500;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX]        = 2000;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE]   = 50;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
+	}
+
 	for (;;) {
 		/* run this loop at ~100Hz */
-		poll(fds, 2, 10);
+		int result = poll(fds, 2, 10);
+
+		ASSERT(result >= 0);
 
 		/*
 		 * Gather R/C control inputs from supported sources.
@@ -90,95 +95,212 @@ controls_main(void)
 		 * one control input source, they're going to fight each
 		 * other.  Don't do that.
 		 */
-		bool locked = false;
+
+		bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
+		if (dsm_updated)
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+
+		bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
+		if (sbus_updated)
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
 
 		/*
-		 * Store RC channel count to detect switch to RC loss sooner
-		 * than just by timeout
+		 * XXX each S.bus frame will cause a PPM decoder interrupt
+		 * storm (lots of edges).  It might be sensible to actually
+		 * disable the PPM decoder completely if we have S.bus signal.
 		 */
-		unsigned rc_channels = system_state.rc_channels;
+		bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
+		if (ppm_updated)
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 
-		if (fds[0].revents & POLLIN)
-			locked |= dsm_input();
+		ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
 
-		if (fds[1].revents & POLLIN)
-			locked |= sbus_input();
+		/*
+		 * In some cases we may have received a frame, but input has still
+		 * been lost.
+		 */
+		bool rc_input_lost = false;
 
 		/*
-		 * If we don't have lock from one of the serial receivers,
-		 * look for PPM. It shares an input with S.bus, so there's
-		 * a possibility it will mis-parse an S.bus frame.
-		 *
-		 * XXX each S.bus frame will cause a PPM decoder interrupt
-		 * storm (lots of edges).  It might be sensible to actually
-		 * disable the PPM decoder completely if we have an alternate
-		 * receiver lock.
+		 * If we received a new frame from any of the RC sources, process it.
 		 */
-		if (!locked)
-			ppm_input();
+		if (dsm_updated | sbus_updated | ppm_updated) {
+
+			/* update RC-received timestamp */
+			system_state.rc_channels_timestamp = hrt_absolute_time();
+
+			/* record a bitmask of channels assigned */
+			unsigned assigned_channels = 0;
+
+			/* 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];
+
+					/* 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];
+					}
+
+					/* constrain to min/max values */
+					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 (-500..500) */
+					scaled -= 1500;
+
+					/* scale to fixed-point representation (-10000..10000) */
+					scaled *= 20;
+
+					ASSERT(scaled >= -15000);
+					ASSERT(scaled <= 15000);
+
+					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+						scaled = -scaled;
 
-		/* check for manual override status */
-		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
-			/* force manual input override */
-			system_state.mixer_manual_override = true;
+					/* and update the scaled/mapped version */
+					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+					ASSERT(mapped < MAX_CONTROL_CHANNELS);
 
-		} else {
-			/* override not engaged, use FMU */
-			system_state.mixer_manual_override = false;
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+				}
+			}
+
+			/* set un-assigned controls to zero */
+			for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+				if (!(assigned_channels & (1 << i)))
+					r_rc_values[i] = 0;
+			}
+
+			/*
+			 * If we got an update with zero channels, treat it as 
+			 * a loss of input.
+			 */
+			if (assigned_channels == 0)
+				rc_input_lost = true;
+
+			/*
+			 * Export the valid channel bitmap
+			 */
+			r_rc_valid = assigned_channels;
 		}
 
 		/*
 		 * If we haven't seen any new control data in 200ms, assume we
-		 * have lost input and tell FMU.
+		 * have lost input.
 		 */
 		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+			rc_input_lost = true;
+		}
 
-			if (system_state.rc_channels > 0) {
-				/*
-				 * If the RC signal status (lost / present) has
-				 * just changed, request an update immediately.
-				 */
-				system_state.fmu_report_due = true;
-			}
+		/*
+		 * Handle losing RC input
+		 */
+		if (rc_input_lost) {
+
+			/* Clear the RC input status flags, clear manual override control */
+			r_status_flags &= ~(
+				PX4IO_P_STATUS_FLAGS_OVERRIDE |
+				PX4IO_P_STATUS_FLAGS_RC_OK |
+				PX4IO_P_STATUS_FLAGS_RC_PPM |
+				PX4IO_P_STATUS_FLAGS_RC_DSM |
+				PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
+			/* Set the RC_LOST alarm */
+			r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 
-			/* set the number of channels to zero - no inputs */
-			system_state.rc_channels = 0;
+			/* Mark the arrays as empty */
+			r_raw_rc_count = 0;
+			r_rc_valid = 0;
 		}
 
 		/*
-		 * PWM output updates are performed in addition on each comm update.
-		 * the updates here are required to ensure operation if FMU is not started
-		 * or stopped responding.
+		 * Check for manual override.
+		 *
+		 * The OVERRIDE_OK feature must be set, and we must have R/C input.
+		 * Override is enabled if either the hardcoded channel / value combination
+		 * is selected, or the AP has requested it.
 		 */
-		mixer_tick();
+		if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && 
+			(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+			bool override = false;
+
+			/*
+			 * Check mapped channel 5; if the value is 'high' then the pilot has
+			 * requested override.
+			 */
+			if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
+				override = true;
+
+			/*
+			 * Check for an explicit manual override request from the AP.
+			 */
+			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+				(r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE))
+				override = true;
+
+			if (override) {
+
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
+
+				/* mix new RC input control values to servos */
+				if (dsm_updated | sbus_updated | ppm_updated)
+					mixer_tick();
+
+			} else {
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+			}
+		}
+
 	}
 }
 
-static void
-ppm_input(void)
+static bool
+ppm_input(uint16_t *values, uint16_t *num_values)
 {
+	bool result = false;
+
+	/* avoid racing with PPM updates */
+	irqstate_t state = irqsave();
+
 	/*
-	 * Look for new PPM input.
+	 * Look for recent PPM input.
 	 */
-	if (ppm_last_valid_decode != 0) {
-
-		/* avoid racing with PPM updates */
-		irqstate_t state = irqsave();
+	if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) {
 
 		/* PPM data exists, copy it */
-		system_state.rc_channels = ppm_decoded_channels;
+		result = true;
+		*num_values = ppm_decoded_channels;
+		if (*num_values > MAX_CONTROL_CHANNELS)
+			*num_values = MAX_CONTROL_CHANNELS;
 
-		for (unsigned i = 0; i < ppm_decoded_channels; i++) {
-			system_state.rc_channel_data[i] = ppm_buffer[i];
-		}
+		for (unsigned i = 0; i < *num_values; i++)
+			values[i] = ppm_buffer[i];
 
-		/* copy the timestamp and clear it */
-		system_state.rc_channels_timestamp = ppm_last_valid_decode;
+		/* clear validity */
 		ppm_last_valid_decode = 0;
+	}
 
-		irqrestore(state);
+	irqrestore(state);
 
-		/* trigger an immediate report to the FMU */
-		system_state.fmu_report_due = true;
-	}
+	return result;
 }
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 560ef47d9..f0e8e0f32 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -43,17 +43,13 @@
 
 #include <fcntl.h>
 #include <unistd.h>
-#include <string.h>
 #include <termios.h>
 
-#include <systemlib/ppm_decode.h>
-
 #include <drivers/drv_hrt.h>
 
 #define DEBUG
 
 #include "px4io.h"
-#include "protocol.h"
 
 #define DSM_FRAME_SIZE		16
 #define DSM_FRAME_CHANNELS	7
@@ -72,13 +68,13 @@ unsigned dsm_frame_drops;
 
 static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
 static void dsm_guess_format(bool reset);
-static void dsm_decode(hrt_abstime now);
+static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
 
 int
 dsm_init(const char *device)
 {
 	if (dsm_fd < 0)
-		dsm_fd = open(device, O_RDONLY);
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
 
 	if (dsm_fd >= 0) {
 		struct termios t;
@@ -106,7 +102,7 @@ dsm_init(const char *device)
 }
 
 bool
-dsm_input(void)
+dsm_input(uint16_t *values, uint16_t *num_values)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -143,7 +139,7 @@ dsm_input(void)
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
-		goto out;
+		return false;
 
 	last_rx_time = now;
 
@@ -156,20 +152,14 @@ dsm_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
-		goto out;
+		return false;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
 	 * decode it.
 	 */
-	dsm_decode(now);
 	partial_frame_count = 0;
-
-out:
-	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
-	 */
-	return (now - last_frame_time) < 200000;
+	return dsm_decode(now, values, num_values);
 }
 
 static bool
@@ -273,8 +263,8 @@ dsm_guess_format(bool reset)
 	dsm_guess_format(true);
 }
 
-static void
-dsm_decode(hrt_abstime frame_time)
+static bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
 
 	/*
@@ -295,7 +285,7 @@ dsm_decode(hrt_abstime frame_time)
 	/* if we don't know the frame format, update the guessing state machine */
 	if (channel_shift == 0) {
 		dsm_guess_format(false);
-		return;
+		return false;
 	}
 
 	/*
@@ -323,8 +313,8 @@ dsm_decode(hrt_abstime frame_time)
 			continue;
 
 		/* update the decoded channel count */
-		if (channel >= system_state.rc_channels)
-			system_state.rc_channels = channel + 1;
+		if (channel >= *num_values)
+			*num_values = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		if (channel_shift == 11)
@@ -355,13 +345,11 @@ dsm_decode(hrt_abstime frame_time)
 			break;
 		}
 
-		system_state.rc_channel_data[channel] = value;
+		values[channel] = value;
 	}
 
-	/* and note that we have received data from the R/C controller */
-	/* XXX failsafe will cause problems here - need a strategy for detecting it */
-	system_state.rc_channels_timestamp = frame_time;
-
-	/* trigger an immediate report to the FMU */
-	system_state.fmu_report_due = true;
+	/*
+	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
+	 */
+	return true;
 }
diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c
index 4e95dd583..d08da2ff1 100644
--- a/apps/px4io/i2c.c
+++ b/apps/px4io/i2c.c
@@ -259,7 +259,6 @@ i2c_tx_complete(void)
 	/* XXX handle transmit-done */
 
 	/* prepare for the next transaction */
-	memset(tx_buf, 0, sizeof(tx_buf));
 	i2c_tx_setup();
 }
 
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index ed7d684b6..7ccf915b0 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -38,22 +38,13 @@
  */
 
 #include <nuttx/config.h>
-#include <nuttx/arch.h>
 
 #include <sys/types.h>
 #include <stdbool.h>
 #include <string.h>
-#include <assert.h>
-#include <errno.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sched.h>
-
-#include <debug.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/device/device.h>
 
 #include <systemlib/mixer/mixer.h>
 
@@ -78,10 +69,12 @@ extern "C" {
 bool mixer_servos_armed = false;
 
 /* selected control values and count for mixing */
-static uint16_t *control_values;
-static int control_count;
-
-static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
+enum mixer_source {
+	MIX_FMU,
+	MIX_OVERRIDE,
+	MIX_FAILSAFE
+};
+static mixer_source source;
 
 static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_group,
@@ -93,23 +86,32 @@ static MixerGroup mixer_group(mixer_callback, 0);
 void
 mixer_tick(void)
 {
-	bool should_arm;
-
 	/* check that we are receiving fresh data from the FMU */
 	if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+
 		/* too many frames without FMU input, time to go to failsafe */
-		system_state.mixer_manual_override = true;
-		system_state.mixer_fmu_available = false;
-		lib_lowprintf("RX timeout\n");
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK;
+		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
+		debug("AP RX timeout");
 	}
 
 	/*
-	 * Decide which set of inputs we're using.
+	 * Decide which set of controls we're using.
 	 */
-	 
-	/* this is for planes, where manual override makes sense */
-	if (system_state.manual_override_ok) {
+	if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && 
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) {
+	 	/* this is for planes, where manual override makes sense */
+		source = MIX_OVERRIDE;
+	} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
+		source = MIX_FMU;
+	} else {
+		source = MIX_FAILSAFE;
+	}
+
+#if 0
 		/* if everything is ok */
+
 		if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
 			/* we have recent control data from the FMU */
 			control_count = PX4IO_CONTROL_CHANNELS;
@@ -170,43 +172,40 @@ mixer_tick(void)
 			control_count = 0;
 		}
 	}
-
+#endif
 	/*
-	 * Run the mixers if we have any control data at all.
+	 * Run the mixers.
 	 */
-	if (control_count > 0) {
-		float	outputs[IO_SERVO_COUNT];
-		unsigned mixed;
-
-		/* mix */
-		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
-
-		/* scale to PWM and update the servo outputs as required */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
-			if (i < mixed) {
-				/* scale to servo output */
-				system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
-
-			} else {
-				/* set to zero to inhibit PWM pulse output */
-				system_state.servos[i] = 0;
-			}
+	float	outputs[IO_SERVO_COUNT];
+	unsigned mixed;
+
+	/* mix */
+	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+	/* scale to PWM and update the servo outputs as required */
+	for (unsigned i = 0; i < mixed; i++) {
+
+		/* save actuator values for FMU readback */
+		r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+
+		/* scale to servo output */
+		r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
 
-			/*
-			 * If we are armed, update the servo output.
-			 */
-			if (system_state.armed && system_state.arm_ok) {
-				up_pwm_servo_set(i, system_state.servos[i]);
-			}
-		}
 	}
+	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		r_page_servos[i] = 0;
+
+	/*
+	 * Update the servo outputs.
+	 */
+	for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		up_pwm_servo_set(i, r_page_servos[i]);
 
 	/*
 	 * Decide whether the servos should be armed right now.
-	 * A sufficient reason is armed state and either FMU or RC control inputs
 	 */
 
-	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
+	bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED);
 
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
@@ -226,21 +225,33 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	/* if the control index refers to an input that's not valid, we can't return it */
-	if (control_index >= control_count)
+	if (control_group != 0)
 		return -1;
 
-	/* scale from current PWM units (1000-2000) to mixer input values */
-	if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) {
-		control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
-	} else {
-		control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
+	switch (source) {
+	case MIX_FMU:
+		if (control_index < PX4IO_CONTROL_CHANNELS) {
+			control = REG_TO_FLOAT(r_page_controls[control_index]);
+			break;
+		}
+		return -1;
+
+	case MIX_OVERRIDE:
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			break;
+		}
+		return -1;
+
+	case MIX_FAILSAFE:
+		/* XXX we could allow for configuration of per-output failsafe values */
+		return -1;
 	}
 
 	return 0;
 }
 
-static char mixer_text[256];
+static char mixer_text[256];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
 
 void
@@ -267,6 +278,7 @@ mixer_handle_text(const void *buffer, size_t length)
 		debug("append %d", length);
 
 		/* check for overflow - this is really fatal */
+		/* XXX could add just what will fit & try to parse, then repeat... */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
 			return;
 
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index aabc476dd..4cb8a54ef 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -55,7 +55,9 @@
  *
  * As convention, values that would be floating point in other parts of 
  * the PX4 system are expressed as signed integer values scaled by 10000,
- * e.g. control values range from -10000..10000.
+ * e.g. control values range from -10000..10000.  Use the REG_TO_SIGNED and
+ * SIGNED_TO_REG macros to convert between register representation and
+ * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
  *
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
@@ -66,6 +68,12 @@
 #define PX4IO_INPUT_CHANNELS	12
 #define PX4IO_RELAY_CHANNELS	4
 
+/* Per C, this is safe for all 2's complement systems */
+#define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
+#define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
+
+#define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
+#define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
@@ -91,13 +99,15 @@
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_DSM		(1 << 4) /* DSM input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 5) /* SBUS input is valid */
+#define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
 
 #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 */
 #define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
 #define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
 #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
-#define PX4IO_P_STATUS_ALARMS_AP_LOST		(1 << 4)
+#define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
+#define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5)
 
 #define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
 #define PX4IO_P_STATUS_TEMPERATURE		5	/* temperature in (units tbd) */
@@ -109,19 +119,26 @@
 #define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of raw RC input values, microseconds */
-#define PX4IO_PAGE_RAW_RC_INPUT		4		/* 0..CONFIG_RC_INPUT_COUNT-1 */
+#define PX4IO_PAGE_RAW_RC_INPUT		4
+#define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
+#define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
-#define PX4IO_PAGE_RC_INPUT		5		/* 0..CONFIG_RC_INPUT_COUNT-1 */
+#define PX4IO_PAGE_RC_INPUT		5
+#define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
+#define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
 
 /* array of raw ADC values */
 #define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
 
 /* setup page */
 #define PX4IO_PAGE_SETUP		100
+#define PX4IO_P_SETUP_FEATURES			0
+#define PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK	(1 << 0) /* OK to switch to manual override */
+
 #define PX4IO_P_SETUP_ARMING			1	/* arming controls */
 #define PX4IO_P_SETUP_ARMING_ARM_OK		(1 << 0) /* OK to arm */
-#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE	(1 << 1) /* request local override */
+#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE	(1 << 2) /* request switch to manual override */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 
@@ -136,7 +153,17 @@
 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD		102
 
-
+/* R/C channel config */
+#define PX4IO_PAGE_RC_CONFIG		103	/* R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
+#define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
+#define PX4IO_P_RC_CONFIG_DEADZONE		3	/* band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4	/* mapped input value */
+#define PX4IO_P_RC_CONFIG_OPTIONS		5	/* channel options bitmask */
+#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
+#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
+#define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /*
  * Old serial PX4FMU <-> PX4IO messaging protocol.
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index e51c1c73c..13f46939a 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -37,22 +37,22 @@
  */
 
 #include <nuttx/config.h>
-#include <stdio.h>
+
+#include <stdio.h>	// required for task_create
 #include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
 #include <stdlib.h>
 #include <errno.h>
 #include <string.h>
-
-#include <nuttx/clock.h>
+#include <poll.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
 
+#include <systemlib/perf_counter.h>
+
 #include <stm32_uart.h>
 
+#define DEBUG
 #include "px4io.h"
 
 __EXPORT int user_start(int argc, char *argv[]);
@@ -70,8 +70,6 @@ int user_start(int argc, char *argv[])
 
 	/* reset all to zero */
 	memset(&system_state, 0, sizeof(system_state));
-	/* default to 50 Hz PWM outputs */
-	system_state.servo_rate = 50;
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
@@ -83,7 +81,7 @@ int user_start(int argc, char *argv[])
 	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
 
 	/* print some startup info */
-	lib_lowprintf("\nPX4IO: starting\n");
+	debug("\nPX4IO: starting\n");
 
 	/* default all the LEDs to off while we start */
 	LED_AMBER(false);
@@ -108,11 +106,19 @@ int user_start(int argc, char *argv[])
 
 
 	struct mallinfo minfo = mallinfo();
-	lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
+	debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
 
-	/* start the i2c test code */
+	/* start the i2c handler */
 	i2c_init();
 
-	/* we're done here, go run the communications loop */
-	comms_main();
+	/* add a performance counter for mixing */
+	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
+
+	/* run the mixer at 100Hz (for now...) */
+	for (;;) {
+		poll(NULL, 0, 10);
+		perf_begin(mixer_perf);
+		mixer_tick();
+		perf_end(mixer_perf);
+	}
 }
\ No newline at end of file
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index fb0659602..2e2c50a3a 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -75,6 +75,29 @@ extern uint16_t			r_page_adc[];		/* PX4IO_PAGE_RAW_ADC_INPUT */
 
 extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
+extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
+
+/*
+ * Register aliases.
+ *
+ * Handy aliases for registers that are widely used.
+ */
+#define r_status_flags		r_page_status[PX4IO_P_STATUS_FLAGS]
+#define r_status_alarms		r_page_status[PX4IO_P_STATUS_ALARMS]
+
+#define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
+#define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
+#define r_rc_values		(&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+
+#define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
+#define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
+#define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
+#define r_setup_pwm_lowrate	r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE]
+#define r_setup_pwm_highrate	r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE]
+#define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+
+#define r_control_values	(&r_page_controls[0])
 
 /*
  * System state structure.
@@ -84,9 +107,9 @@ extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
  */
 struct sys_state_s {
 
-	bool		armed;			/* IO armed */
-	bool		arm_ok;			/* FMU says OK to arm */
-	uint16_t	servo_rate;		/* output rate of servos in Hz */
+//	bool		armed;			/* IO armed */
+//	bool		arm_ok;			/* FMU says OK to arm */
+//	uint16_t	servo_rate;		/* output rate of servos in Hz */
 
 	/**
 	 * Remote control input(s) channel mappings
@@ -105,39 +128,39 @@ struct sys_state_s {
 	/**
 	 * Data from the remote control input(s)
 	 */
-	unsigned	rc_channels;
-	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
+//	unsigned	rc_channels;
+//	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
 	uint64_t	rc_channels_timestamp;
 
 	/**
 	 * Control signals from FMU.
 	 */
-	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
+//	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
 
 	/**
 	 * Mixed servo outputs
 	 */
-	uint16_t	servos[IO_SERVO_COUNT];
+//	uint16_t	servos[IO_SERVO_COUNT];
 
 	/**
 	 * Relay controls
 	 */
-	bool		relays[PX4IO_RELAY_CHANNELS];
+//	bool		relays[PX4IO_RELAY_CHANNELS];
 
 	/**
 	 * If true, we are using the FMU controls, else RC input if available.
 	 */
-	bool		mixer_manual_override;
+//	bool		mixer_manual_override;
 
 	/**
 	 * If true, FMU input is available.
 	 */
-	bool		mixer_fmu_available;
+//	bool		mixer_fmu_available;
 
 	/**
 	 * If true, state that should be reported to FMU has been updated.
 	 */
-	bool		fmu_report_due;
+//	bool		fmu_report_due;
 
 	/**
 	 * Last FMU receive time, in microseconds since system boot
@@ -147,12 +170,12 @@ struct sys_state_s {
 	/**
 	 * If true, the RC signal has been lost for more than a timeout interval
 	 */
-	bool		rc_lost;
+//	bool		rc_lost;
 
 	/**
 	 * If true, the connection to FMU has been lost for more than a timeout interval
 	 */
-	bool		fmu_lost;
+//	bool		fmu_lost;
 
 	/**
 	 * If true, FMU is ready for autonomous position control. Only used for LED indication
@@ -162,41 +185,27 @@ struct sys_state_s {
 	/**
 	 * If true, IO performs an on-board manual override with the RC channel values
 	 */
-	bool manual_override_ok;
+//	bool manual_override_ok;
 
 	/*
 	 * Measured battery voltage in mV
 	 */
-	uint16_t	battery_mv;
+//	uint16_t	battery_mv;
 
 	/*
 	 * ADC IN5 measurement
 	 */
-	uint16_t	adc_in5;
+//	uint16_t	adc_in5;
 
 	/*
 	 * Power supply overcurrent status bits.
 	 */
-	uint8_t		overcurrent;
+//	uint8_t		overcurrent;
 
 };
 
 extern struct sys_state_s system_state;
 
-#if 0
-/*
- * Software countdown timers.
- *
- * Each timer counts down to zero at one tick per ms.
- */
-#define TIMER_BLINK_AMBER	0
-#define TIMER_BLINK_BLUE	1
-#define TIMER_STATUS_PRINT	2
-#define TIMER_SANITY		7
-#define TIMER_NUM_TIMERS	8
-extern volatile int	timers[TIMER_NUM_TIMERS];
-#endif
-
 /*
  * GPIO handling.
  */
@@ -249,9 +258,11 @@ extern uint16_t	adc_measure(unsigned channel);
 
 /*
  * R/C receiver handling.
+ *
+ * Input functions return true when they receive an update from the RC controller.
  */
 extern void	controls_main(void);
 extern int	dsm_init(const char *device);
-extern bool	dsm_input(void);
+extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(void);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 1397dd2a4..59684f1ee 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -37,6 +37,11 @@
  * Implementation of the PX4IO register space.
  */
 
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdlib.h>
+
 #include "px4io.h"
 #include "protocol.h"
 
@@ -45,8 +50,9 @@ static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
 /**
  * Setup registers
  */
-uint16_t		r_page_setup[] =
+volatile uint16_t	r_page_setup[] =
 {
+	[PX4IO_P_SETUP_FEATURES]		= 0,
 	[PX4IO_P_SETUP_ARMING]			= 0,
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_LOWRATE]		= 50,
@@ -54,14 +60,16 @@ uint16_t		r_page_setup[] =
 	[PX4IO_P_SETUP_RELAYS]			= 0,
 };
 
-#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
+#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
-#define PX4IO_P_SETUP_RELAYS_VALID	((1 << PXIO_RELAY_CHANNELS) - 1)
+#define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
 /**
  * Control values from the FMU.
  */
-uint16_t		r_page_controls[PX4IO_CONTROL_CHANNELS];
+volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
 
 /**
  * Static configuration parameters.
@@ -108,13 +116,26 @@ uint16_t		r_page_servos[IO_SERVO_COUNT];
 /**
  * Scaled/routed RC input
  */
-uint16_t		r_page_rc_input[MAX_CONTROL_CHANNELS];
+uint16_t		r_page_rc_input[] = {
+	[PX4IO_P_RC_VALID]			= 0,
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
 
 /**
  * Raw RC input
  */
-uint16_t		r_page_raw_rc_input[MAX_CONTROL_CHANNELS];
+uint16_t		r_page_raw_rc_input[] =
+{
+	[PX4IO_P_RAW_RC_COUNT]			= 0,
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
 
+/**
+ * R/C channel input configuration.
+ */
+uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+
+#define PX4IO_P_RC_CONFIG_OPTIONS_VALID	PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
 
 void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -136,7 +157,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 
 		/* XXX we should cause a mixer tick ASAP */
-		system_state.mixer_fmu_available = true;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 		break;
 
 		/* handle text going to the mixer parser */
@@ -168,7 +189,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		switch (offset) {
 		case PX4IO_P_STATUS_ALARMS:
 			/* clear bits being written */
-			r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value;
+			r_status_alarms &= ~value;
 			break;
 
 		default:
@@ -179,20 +200,30 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_SETUP:
 		switch (offset) {
+		case PX4IO_P_SETUP_FEATURES:
+
+			value &= PX4IO_P_SETUP_FEATURES_VALID;
+			r_setup_features = value;
+
+			/* update manual override state - disable if no longer OK */
+			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK))
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+
+			break;
+
 		case PX4IO_P_SETUP_ARMING:
 
 			value &= PX4IO_P_SETUP_ARMING_VALID;
-			r_page_setup[PX4IO_P_SETUP_ARMING] = value;
+			r_setup_arming = value;
 
 			/* update arming state - disarm if no longer OK */
-			if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
-				system_state.armed = false;
-
+			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			break;
 
 		case PX4IO_P_SETUP_PWM_RATES:
 			value &= PX4IO_P_SETUP_RATES_VALID;
-			r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value;
+			r_setup_pwm_rates = value;
 			/* XXX re-configure timers */
 			break;
 
@@ -201,7 +232,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				value = 50;
 			if (value > 400)
 				value = 400;
-			r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value;
+			r_setup_pwm_lowrate = value;
 			/* XXX re-configure timers */
 			break;
 
@@ -210,13 +241,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				value = 50;
 			if (value > 400)
 				value = 400;
-			r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value;
+			r_setup_pwm_highrate = value;
 			/* XXX re-configure timers */
 			break;
 
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
-			r_page_setup[PX4IO_P_SETUP_RELAYS] = value;
+			r_setup_relays = value;
 			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
 			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
 			POWER_ACC1(value & (1 << 2) ? 1 : 0);
@@ -228,6 +259,63 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		}
 		break;
 
+	case PX4IO_PAGE_RC_CONFIG: {
+		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
+		unsigned index = offset % PX4IO_P_RC_CONFIG_STRIDE;
+		uint16_t *conf = &r_page_rc_input_config[offset * PX4IO_P_RC_CONFIG_STRIDE];
+
+		if (channel >= MAX_CONTROL_CHANNELS)
+			return -1;
+
+		/* disable the channel until we have a chance to sanity-check it */
+		conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+
+		switch (index) {
+
+		case PX4IO_P_RC_CONFIG_MIN:
+		case PX4IO_P_RC_CONFIG_CENTER:
+		case PX4IO_P_RC_CONFIG_MAX:
+		case PX4IO_P_RC_CONFIG_DEADZONE:
+		case PX4IO_P_RC_CONFIG_ASSIGNMENT:
+			conf[index] = value;
+			break;
+
+		case PX4IO_P_RC_CONFIG_OPTIONS:
+			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
+
+			/* set all options except the enabled option */
+			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+
+			/* should the channel be enabled? */
+			/* this option is normally set last */
+			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+				/* 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;
+				/* 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;
+
+				/* sanity checks pass, enable channel */
+				conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+			}
+			break;
+
+		}
+	}
+
 	default:
 		return -1;
 	}
@@ -237,27 +325,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 int
 registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
 {
+#define SELECT_PAGE(_page_name)	{ *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); }
 
 	switch (page) {
-	case PX4IO_PAGE_CONFIG:
-		*values = r_page_config;
-		*num_values = sizeof(r_page_config) / sizeof(r_page_config[0]);
-		break;
 
+	/*
+	 * Handle pages that are updated dynamically at read time.
+	 */
 	case PX4IO_PAGE_STATUS:
+		/* PX4IO_P_STATUS_FREEMEM */
 		{
 			struct mallinfo minfo = mallinfo();
 			r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
 		}
+
 		/* XXX PX4IO_P_STATUS_CPULOAD */
-		r_page_status[PX4IO_P_STATUS_FLAGS] = 
-			(system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) |
-			(system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) |
-			((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0))
-			/* XXX specific receiver status */
 
-		/* XXX PX4IO_P_STATUS_ALARMS] */
+		/* PX4IO_P_STATUS_FLAGS maintained externally */
+
+		/* PX4IO_P_STATUS_ALARMS maintained externally */
 
+		/* PX4IO_P_STATUS_VBATT */
 		{
 			/*
 			 * Coefficients here derived by measurement of the 5-16V
@@ -285,43 +373,42 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			unsigned counts = adc_measure(ADC_VBATT);
 			r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
 		}
+
 		/* XXX PX4IO_P_STATUS_TEMPERATURE */
 
-		*values = r_page_status;
-		*num_values = sizeof(r_page_status) / sizeof(r_page_status[0]);
+		SELECT_PAGE(r_page_status);
 		break;
 
-	case PX4IO_PAGE_ACTUATORS:
-		*values = r_page_actuators;
-		*num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]);
-		break;
+	case PX4IO_PAGE_RAW_ADC_INPUT:
+		r_page_adc[0] = adc_measure(ADC_VBATT);
+		r_page_adc[1] = adc_measure(ADC_IN5);
 
-	case PX4IO_PAGE_SERVOS:
-		*values = system_state.servos;
-		*num_values = IO_SERVO_COUNT;
+		SELECT_PAGE(r_page_adc);
 		break;
 
-	case PX4IO_PAGE_RAW_RC_INPUT:
-		*values = r_page_raw_rc_input;
-		*num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]);
-		break;
+	/*
+	 * Pages that are just a straight read of the register state.
+	 */
+#define COPY_PAGE(_page_name, _page)	case _page_name: SELECT_PAGE(_page); break;
 
-	case PX4IO_PAGE_RC_INPUT:
-		*values = system_state.rc_channel_data;
-		*num_values = system_state.rc_channels;
-		return -1;
+	/* status pages */
+	COPY_PAGE(PX4IO_PAGE_CONFIG,		r_page_config);
+	COPY_PAGE(PX4IO_PAGE_ACTUATORS,		r_page_actuators);
+	COPY_PAGE(PX4IO_PAGE_SERVOS,		r_page_servos);
+	COPY_PAGE(PX4IO_PAGE_RAW_RC_INPUT,	r_page_raw_rc_input);
+	COPY_PAGE(PX4IO_PAGE_RC_INPUT,		r_page_rc_input);
 
-	case PX4IO_PAGE_RAW_ADC_INPUT:
-		r_page_adc[0] = adc_measure(ADC_VBATT);
-		r_page_adc[1] = adc_measure(ADC_IN5);
-		*values = r_page_adc;
-		*num_values = ADC_CHANNEL_COUNT;
-		break;
+	/* readback of input pages */
+	COPY_PAGE(PX4IO_PAGE_SETUP,		r_page_setup);
+	COPY_PAGE(PX4IO_PAGE_CONTROLS,		r_page_controls);
+	COPY_PAGE(PX4IO_PAGE_RC_CONFIG,		r_page_rc_input_config);
 
 	default:
 		return -1;
 	}
 
+#undef SELECT_PAGE
+
 	/* if the offset is beyond the end of the page, we have no data */
 	if (*num_values <= offset)
 		return -1;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 0cae29ac9..a14051f76 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -36,15 +36,8 @@
  */
 
 #include <nuttx/config.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
-#include <stdlib.h>
-#include <errno.h>
 
-#include <nuttx/clock.h>
+#include <stdbool.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -116,30 +109,28 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !system_state.armed) {
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
-			/* change to armed state and notify the FMU */
-			system_state.armed = true;
+			/* switch to armed state */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
 			counter++;
-			system_state.fmu_report_due = true;
 		}
 
 		/* Disarm quickly */
 
-	} else if (safety_button_pressed && system_state.armed) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			system_state.armed = false;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			counter++;
-			system_state.fmu_report_due = true;
 		}
 
 	} else {
@@ -149,15 +140,15 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_SAFE;
 
-	if (system_state.armed) {
-		if (system_state.arm_ok) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+		if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
 		} else {
 			pattern = LED_PATTERN_IO_ARMED;
 		}
 
-	} else if (system_state.arm_ok) {
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
 		pattern = LED_PATTERN_FMU_ARMED;
 
 	} else if (system_state.vector_flight_mode_ok) {
@@ -188,7 +179,7 @@ failsafe_blink(void *arg)
 	static bool failsafe = false;
 
 	/* blink the failsafe LED if we don't have FMU input */
-	if (!system_state.mixer_fmu_available) {
+	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 		failsafe = !failsafe;
 
 	} else {
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index 568ef8091..d199a9361 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -66,13 +66,13 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static void sbus_decode(hrt_abstime frame_time);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
 
 int
 sbus_init(const char *device)
 {
 	if (sbus_fd < 0)
-		sbus_fd = open(device, O_RDONLY);
+		sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -97,7 +97,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(void)
+sbus_input(uint16_t *values, uint16_t *num_values)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -134,7 +134,7 @@ sbus_input(void)
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
-		goto out;
+		return false;
 
 	last_rx_time = now;
 
@@ -147,20 +147,14 @@ sbus_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < SBUS_FRAME_SIZE)
-		goto out;
+		return false;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
 	 * decode it.
 	 */
-	sbus_decode(now);
 	partial_frame_count = 0;
-
-out:
-	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
-	 */
-	return (now - last_frame_time) < 200000;
+	return sbus_decode(now, values, num_values);
 }
 
 /*
@@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 	/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00,  0} }
 };
 
-static void
-sbus_decode(hrt_abstime frame_time)
+static bool
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
 		sbus_frame_drops++;
-		return;
+		return false;
 	}
 
 	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
@@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time)
 	    (frame[23] & (1 << 3))) { /* failsafe */
 
 		/* actively announce signal loss */
-		system_state.rc_channels = 0;
-		return 1;
+		*values = 0;
+		return false;
 	}
 
 	/* we have received something we think is a frame */
@@ -241,23 +235,19 @@ sbus_decode(hrt_abstime frame_time)
 		}
 
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		system_state.rc_channel_data[channel] = (value / 2) + 998;
+		values[channel] = (value / 2) + 998;
 	}
 
 	/* decode switch channels if data fields are wide enough */
 	if (chancount > 17) {
 		/* channel 17 (index 16) */
-		system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
 		/* channel 18 (index 17) */
-		system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+		values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */
-	system_state.rc_channels = chancount;
-
-	/* and note that we have received data from the R/C controller */
-	system_state.rc_channels_timestamp = frame_time;
+	*num_values = chancount;
 
-	/* trigger an immediate report to the FMU */
-	system_state.fmu_report_due = true;
+	return true;
 }