Moved all drivers to new world, PX4IO completely in new world

17 files changed, 4195 insertions, 0 deletions

diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
new file mode 100644
index 000000000..670b8d635
--- /dev/null
+++ b/src/modules/px4iofirmware/adc.c
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file adc.c
+ *
+ * Simple ADC support for PX4IO on STM32.
+ */
+#include <nuttx/config.h>
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+
+#define DEBUG
+#include "px4io.h"
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
+
+#define rSR		REG(STM32_ADC_SR_OFFSET)
+#define rCR1		REG(STM32_ADC_CR1_OFFSET)
+#define rCR2		REG(STM32_ADC_CR2_OFFSET)
+#define rSMPR1		REG(STM32_ADC_SMPR1_OFFSET)
+#define rSMPR2		REG(STM32_ADC_SMPR2_OFFSET)
+#define rJOFR1		REG(STM32_ADC_JOFR1_OFFSET)
+#define rJOFR2		REG(STM32_ADC_JOFR2_OFFSET)
+#define rJOFR3		REG(STM32_ADC_JOFR3_OFFSET)
+#define rJOFR4		REG(STM32_ADC_JOFR4_OFFSET)
+#define rHTR		REG(STM32_ADC_HTR_OFFSET)
+#define rLTR		REG(STM32_ADC_LTR_OFFSET)
+#define rSQR1		REG(STM32_ADC_SQR1_OFFSET)
+#define rSQR2		REG(STM32_ADC_SQR2_OFFSET)
+#define rSQR3		REG(STM32_ADC_SQR3_OFFSET)
+#define rJSQR		REG(STM32_ADC_JSQR_OFFSET)
+#define rJDR1		REG(STM32_ADC_JDR1_OFFSET)
+#define rJDR2		REG(STM32_ADC_JDR2_OFFSET)
+#define rJDR3		REG(STM32_ADC_JDR3_OFFSET)
+#define rJDR4		REG(STM32_ADC_JDR4_OFFSET)
+#define rDR		REG(STM32_ADC_DR_OFFSET)
+
+perf_counter_t		adc_perf;
+
+int
+adc_init(void)
+{
+	adc_perf = perf_alloc(PC_ELAPSED, "adc");
+
+	/* do calibration if supported */
+#ifdef ADC_CR2_CAL
+	rCR2 |= ADC_CR2_RSTCAL;
+	up_udelay(1);
+
+	if (rCR2 & ADC_CR2_RSTCAL)
+		return -1;
+
+	rCR2 |= ADC_CR2_CAL;
+	up_udelay(100);
+
+	if (rCR2 & ADC_CR2_CAL)
+		return -1;
+
+#endif
+
+	/* arbitrarily configure all channels for 55 cycle sample time */
+	rSMPR1 = 0b00000011011011011011011011011011;
+	rSMPR2 = 0b00011011011011011011011011011011;
+
+	/* XXX for F2/4, might want to select 12-bit mode? */
+	rCR1 = 0;
+
+	/* enable the temperature sensor / Vrefint channel if supported*/
+	rCR2 =
+#ifdef ADC_CR2_TSVREFE
+		/* enable the temperature sensor in CR2 */
+		ADC_CR2_TSVREFE |
+#endif
+		0;
+
+#ifdef ADC_CCR_TSVREFE
+	/* enable temperature sensor in CCR */
+	rCCR = ADC_CCR_TSVREFE;
+#endif
+
+	/* configure for a single-channel sequence */
+	rSQR1 = 0;
+	rSQR2 = 0;
+	rSQR3 = 0;	/* will be updated with the channel each tick */
+
+	/* power-cycle the ADC and turn it on */
+	rCR2 &= ~ADC_CR2_ADON;
+	up_udelay(10);
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+
+	return 0;
+}
+
+uint16_t
+adc_measure(unsigned channel)
+{
+	perf_begin(adc_perf);
+
+	/* clear any previous EOC */
+	if (rSR & ADC_SR_EOC)
+		rSR &= ~ADC_SR_EOC;
+
+	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
+	rSQR3 = channel;
+	rCR2 |= ADC_CR2_ADON;
+
+	/* wait for the conversion to complete */
+	hrt_abstime now = hrt_absolute_time();
+
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* never spin forever - this will give a bogus result though */
+		if (hrt_elapsed_time(&now) > 1000) {
+			debug("adc timeout");
+			break;
+		}
+	}
+
+	/* read the result and clear EOC */
+	uint16_t result = rDR;
+
+	perf_end(adc_perf);
+	return result;
+}
\ No newline at end of file
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
new file mode 100644
index 000000000..dc36f6c93
--- /dev/null
+++ b/src/modules/px4iofirmware/controls.c
@@ -0,0 +1,332 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file controls.c
+ *
+ * R/C inputs and servo outputs.
+ */
+
+#include <nuttx/config.h>
+#include <stdbool.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/ppm_decode.h>
+
+#include "px4io.h"
+
+#define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
+#define RC_CHANNEL_HIGH_THRESH		5000
+#define RC_CHANNEL_LOW_THRESH		-5000
+
+static bool	ppm_input(uint16_t *values, uint16_t *num_values);
+
+static perf_counter_t c_gather_dsm;
+static perf_counter_t c_gather_sbus;
+static perf_counter_t c_gather_ppm;
+
+void
+controls_init(void)
+{
+	/* DSM input */
+	dsm_init("/dev/ttyS0");
+
+	/* S.bus input */
+	sbus_init("/dev/ttyS2");
+
+	/* default to a 1:1 input map, all enabled */
+	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]   = 30;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+	}
+
+	c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
+	c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
+	c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
+}
+
+void
+controls_tick() {
+
+	/*
+	 * Gather R/C control inputs from supported sources.
+	 *
+	 * Note that if you're silly enough to connect more than
+	 * one control input source, they're going to fight each
+	 * other.  Don't do that.
+	 */
+
+	perf_begin(c_gather_dsm);
+	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;
+	perf_end(c_gather_dsm);
+
+	perf_begin(c_gather_sbus);
+	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;
+	perf_end(c_gather_sbus);
+
+	/*
+	 * 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.
+	 */
+	perf_begin(c_gather_ppm);
+	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;
+	perf_end(c_gather_ppm);
+
+	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+
+	/*
+	 * In some cases we may have received a frame, but input has still
+	 * been lost.
+	 */
+	bool rc_input_lost = false;
+
+	/*
+	 * If we received a new frame from any of the RC sources, process it.
+	 */
+	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];
+
+				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;
+
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				ASSERT(mapped < MAX_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 */
+					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 < 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.
+		 *
+		 * This might happen if a protocol-based receiver returns an update
+		 * that contains no channels that we have mapped.
+		 */
+		if (assigned_channels == 0) {
+			rc_input_lost = true;
+		} else {
+			/* set RC OK flag and clear RC lost alarm */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+			r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
+		}
+
+		/*
+		 * 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.
+	 */
+	if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+		rc_input_lost = true;
+
+		/* clear the input-kind flags here */
+		r_status_flags &= ~(
+			PX4IO_P_STATUS_FLAGS_RC_PPM |
+			PX4IO_P_STATUS_FLAGS_RC_DSM |
+			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+	}
+
+	/*
+	 * Handle losing RC input
+	 */
+	if (rc_input_lost) {
+
+		/* Clear the RC input status flag, clear manual override flag */
+		r_status_flags &= ~(
+			PX4IO_P_STATUS_FLAGS_OVERRIDE |
+			PX4IO_P_STATUS_FLAGS_RC_OK);
+
+		/* Set the RC_LOST alarm */
+		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+
+		/* Mark the arrays as empty */
+		r_raw_rc_count = 0;
+		r_rc_valid = 0;
+	}
+
+	/*
+	 * Check for manual override.
+	 *
+	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag 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.
+	 */
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+		bool override = false;
+
+		/*
+		 * Check mapped channel 5 (can be any remote channel,
+		 * depends on RC_MAP_OVER parameter);
+		 * If the value is 'high' then the pilot has
+		 * requested override.
+		 *
+		 */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+			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 bool
+ppm_input(uint16_t *values, uint16_t *num_values)
+{
+	bool result = false;
+
+	/* avoid racing with PPM updates */
+	irqstate_t state = irqsave();
+
+	/*
+	 * If we have received a new PPM frame within the last 200ms, accept it
+	 * and then invalidate it.
+	 */
+	if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
+
+		/* PPM data exists, copy it */
+		*num_values = ppm_decoded_channels;
+		if (*num_values > MAX_CONTROL_CHANNELS)
+			*num_values = MAX_CONTROL_CHANNELS;
+
+		for (unsigned i = 0; i < *num_values; i++)
+			values[i] = ppm_buffer[i];
+
+		/* clear validity */
+		ppm_last_valid_decode = 0;
+
+		/* good if we got any channels */
+		result = (*num_values > 0);
+	}
+
+	irqrestore(state);
+
+	return result;
+}
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
new file mode 100644
index 000000000..ea35e5513
--- /dev/null
+++ b/src/modules/px4iofirmware/dsm.c
@@ -0,0 +1,356 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file dsm.c
+ *
+ * Serial protocol decoder for the Spektrum DSM* family of protocols.
+ *
+ * Decodes into the global PPM buffer and updates accordingly.
+ */
+
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <drivers/drv_hrt.h>
+
+#define DEBUG
+
+#include "px4io.h"
+
+#define DSM_FRAME_SIZE		16
+#define DSM_FRAME_CHANNELS	7
+
+static int dsm_fd = -1;
+
+static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
+
+static uint8_t	frame[DSM_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+static unsigned	channel_shift;
+
+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 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 | O_NONBLOCK);
+
+	if (dsm_fd >= 0) {
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		partial_frame_count = 0;
+		last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+		debug("DSM: open failed");
+	}
+
+	return dsm_fd;
+}
+
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 * The DSM* protocol doesn't provide any explicit framing,
+	 * so we detect frame boundaries by the inter-frame delay.
+	 *
+	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
+	 * frame transmission time is ~1.4ms.
+	 *
+	 * We expect to only be called when bytes arrive for processing,
+	 * and if an interval of more than 5ms passes between calls,
+	 * the first byte we read will be the first byte of a frame.
+	 *
+	 * In the case where byte(s) are dropped from a frame, this also
+	 * provides a degree of protection. Of course, it would be better
+	 * if we didn't drop bytes...
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - last_rx_time) > 5000) {
+		if (partial_frame_count > 0) {
+			dsm_frame_drops++;
+			partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current frame.
+	 */
+	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	last_rx_time = now;
+
+	/*
+	 * Add bytes to the current frame
+	 */
+	partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full frame, return
+	 */
+	if (partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}
+
+static bool
+dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
+{
+
+	if (raw == 0xffff)
+		return false;
+
+	*channel = (raw >> shift) & 0xf;
+
+	uint16_t data_mask = (1 << shift) - 1;
+	*value = raw & data_mask;
+
+	//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
+
+	return true;
+}
+
+static void
+dsm_guess_format(bool reset)
+{
+	static uint32_t	cs10;
+	static uint32_t	cs11;
+	static unsigned samples;
+
+	/* reset the 10/11 bit sniffed channel masks */
+	if (reset) {
+		cs10 = 0;
+		cs11 = 0;
+		samples = 0;
+		channel_shift = 0;
+		return;
+	}
+
+	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
+
+		uint8_t *dp = &frame[2 + (2 * i)];
+		uint16_t raw = (dp[0] << 8) | dp[1];
+		unsigned channel, value;
+
+		/* if the channel decodes, remember the assigned number */
+		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
+			cs10 |= (1 << channel);
+
+		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
+			cs11 |= (1 << channel);
+
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+	}
+
+	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	if (samples++ < 5)
+		return;
+
+	/*
+	 * Iterate the set of sensible sniffed channel sets and see whether
+	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 *
+	 * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
+	 *     stream, we may want to sniff for longer in some cases when we think we
+	 *     are talking to a DSM2 receiver in high-resolution mode (so that we can
+	 *     reject it, ideally).
+	 *     See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
+	 *     of this issue.
+	 */
+	static uint32_t masks[] = {
+		0x3f,	/* 6 channels (DX6) */
+		0x7f,	/* 7 channels (DX7) */
+		0xff,	/* 8 channels (DX8) */
+		0x1ff,	/* 9 channels (DX9, etc.) */
+		0x3ff,	/* 10 channels (DX10) */
+		0x3fff	/* 18 channels (DX10) */
+	};
+	unsigned votes10 = 0;
+	unsigned votes11 = 0;
+
+	for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {
+
+		if (cs10 == masks[i])
+			votes10++;
+
+		if (cs11 == masks[i])
+			votes11++;
+	}
+
+	if ((votes11 == 1) && (votes10 == 0)) {
+		channel_shift = 11;
+		debug("DSM: 11-bit format");
+		return;
+	}
+
+	if ((votes10 == 1) && (votes11 == 0)) {
+		channel_shift = 10;
+		debug("DSM: 10-bit format");
+		return;
+	}
+
+	/* call ourselves to reset our state ... we have to try again */
+	debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
+	dsm_guess_format(true);
+}
+
+static bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+{
+
+	/*
+		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	*/
+	/*
+	 * If we have lost signal for at least a second, reset the
+	 * format guessing heuristic.
+	 */
+	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+		dsm_guess_format(true);
+
+	/* we have received something we think is a frame */
+	last_frame_time = frame_time;
+
+	/* if we don't know the frame format, update the guessing state machine */
+	if (channel_shift == 0) {
+		dsm_guess_format(false);
+		return false;
+	}
+
+	/*
+	 * The encoding of the first two bytes is uncertain, so we're
+	 * going to ignore them for now.
+	 *
+	 * Each channel is a 16-bit unsigned value containing either a 10-
+	 * or 11-bit channel value and a 4-bit channel number, shifted
+	 * either 10 or 11 bits. The MSB may also be set to indicate the
+	 * second frame in variants of the protocol where more than
+	 * seven channels are being transmitted.
+	 */
+
+	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
+
+		uint8_t *dp = &frame[2 + (2 * i)];
+		uint16_t raw = (dp[0] << 8) | dp[1];
+		unsigned channel, value;
+
+		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+			continue;
+
+		/* ignore channels out of range */
+		if (channel >= PX4IO_INPUT_CHANNELS)
+			continue;
+
+		/* update the decoded channel count */
+		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)
+			value /= 2;
+
+		value += 998;
+
+		/*
+		 * Store the decoded channel into the R/C input buffer, taking into
+		 * account the different ideas about channel assignement that we have.
+		 *
+		 * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
+		 * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
+		 */
+		switch (channel) {
+		case 0:
+			channel = 2;
+			break;
+
+		case 1:
+			channel = 0;
+			break;
+
+		case 2:
+			channel = 1;
+
+		default:
+			break;
+		}
+
+		values[channel] = value;
+	}
+
+	/*
+	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
+	 */
+	return true;
+}
diff --git a/src/modules/px4iofirmware/hx_stream.c b/src/modules/px4iofirmware/hx_stream.c
new file mode 100644
index 000000000..8d77f14a8
--- /dev/null
+++ b/src/modules/px4iofirmware/hx_stream.c
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hx_stream.c
+ *
+ * A simple serial line framing protocol based on HDLC
+ * with 32-bit CRC protection.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <crc32.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "perf_counter.h"
+
+#include "hx_stream.h"
+
+
+struct hx_stream {
+	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		frame_bytes;
+	bool			escaped;
+	bool			txerror;
+
+	int			fd;
+	hx_stream_rx_callback	callback;
+	void			*callback_arg;
+
+	perf_counter_t		pc_tx_frames;
+	perf_counter_t		pc_rx_frames;
+	perf_counter_t		pc_rx_errors;
+};
+
+/*
+ * Protocol magic numbers, straight out of HDLC.
+ */
+#define FBO	0x7e	/**< Frame Boundary Octet */
+#define CEO	0x7c	/**< Control Escape Octet */
+
+static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
+static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
+static int	hx_rx_frame(hx_stream_t stream);
+
+static void
+hx_tx_raw(hx_stream_t stream, uint8_t c)
+{
+	if (write(stream->fd, &c, 1) != 1)
+		stream->txerror = true;
+}
+
+static void
+hx_tx_byte(hx_stream_t stream, uint8_t c)
+{
+	switch (c) {
+	case FBO:
+	case CEO:
+		hx_tx_raw(stream, CEO);
+		c ^= 0x20;
+		break;
+	}
+
+	hx_tx_raw(stream, c);
+}
+
+static int
+hx_rx_frame(hx_stream_t stream)
+{
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} u;
+	unsigned length = stream->frame_bytes;
+
+	/* reset the stream */
+	stream->frame_bytes = 0;
+	stream->escaped = false;
+
+	/* not a real frame - too short */
+	if (length < 4) {
+		if (length > 1)
+			perf_count(stream->pc_rx_errors);
+
+		return 0;
+	}
+
+	length -= 4;
+
+	/* compute expected CRC */
+	u.w = crc32(&stream->buf[0], length);
+
+	/* compare computed and actual CRC */
+	for (unsigned i = 0; i < 4; i++) {
+		if (u.b[i] != stream->buf[length + i]) {
+			perf_count(stream->pc_rx_errors);
+			return 0;
+		}
+	}
+
+	/* frame is good */
+	perf_count(stream->pc_rx_frames);
+	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	return 1;
+}
+
+hx_stream_t
+hx_stream_init(int fd,
+	       hx_stream_rx_callback callback,
+	       void *arg)
+{
+	hx_stream_t stream;
+
+	stream = (hx_stream_t)malloc(sizeof(struct hx_stream));
+
+	if (stream != NULL) {
+		memset(stream, 0, sizeof(struct hx_stream));
+		stream->fd = fd;
+		stream->callback = callback;
+		stream->callback_arg = arg;
+	}
+
+	return stream;
+}
+
+void
+hx_stream_free(hx_stream_t stream)
+{
+	/* free perf counters (OK if they are NULL) */
+	perf_free(stream->pc_tx_frames);
+	perf_free(stream->pc_rx_frames);
+	perf_free(stream->pc_rx_errors);
+
+	free(stream);
+}
+
+void
+hx_stream_set_counters(hx_stream_t stream,
+		       perf_counter_t tx_frames,
+		       perf_counter_t rx_frames,
+		       perf_counter_t rx_errors)
+{
+	stream->pc_tx_frames = tx_frames;
+	stream->pc_rx_frames = rx_frames;
+	stream->pc_rx_errors = rx_errors;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} u;
+	const uint8_t *p = (const uint8_t *)data;
+	unsigned resid = count;
+
+	if (resid > HX_STREAM_MAX_FRAME)
+		return -EINVAL;
+
+	/* start the frame */
+	hx_tx_raw(stream, FBO);
+
+	/* transmit the data */
+	while (resid--)
+		hx_tx_byte(stream, *p++);
+
+	/* compute the CRC */
+	u.w = crc32(data, count);
+
+	/* send the CRC */
+	p = &u.b[0];
+	resid = 4;
+
+	while (resid--)
+		hx_tx_byte(stream, *p++);
+
+	/* and the trailing frame separator */
+	hx_tx_raw(stream, FBO);
+
+	/* check for transmit error */
+	if (stream->txerror) {
+		stream->txerror = false;
+		return -EIO;
+	}
+
+	perf_count(stream->pc_tx_frames);
+	return 0;
+}
+
+void
+hx_stream_rx(hx_stream_t stream, uint8_t c)
+{
+	/* frame end? */
+	if (c == FBO) {
+		hx_rx_frame(stream);
+		return;
+	}
+
+	/* escaped? */
+	if (stream->escaped) {
+		stream->escaped = false;
+		c ^= 0x20;
+
+	} else if (c == CEO) {
+		/* now escaped, ignore the byte */
+		stream->escaped = true;
+		return;
+	}
+
+	/* save for later */
+	if (stream->frame_bytes < sizeof(stream->buf))
+		stream->buf[stream->frame_bytes++] = c;
+}
diff --git a/src/modules/px4iofirmware/hx_stream.h b/src/modules/px4iofirmware/hx_stream.h
new file mode 100644
index 000000000..128689953
--- /dev/null
+++ b/src/modules/px4iofirmware/hx_stream.h
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hx_stream.h
+ *
+ * A simple serial line framing protocol based on HDLC
+ * with 32-bit CRC protection.
+ */
+
+#ifndef _SYSTEMLIB_HX_STREAM_H
+#define _SYSTEMLIB_HX_STREAM_H
+
+#include <sys/types.h>
+
+#include "perf_counter.h"
+
+struct hx_stream;
+typedef struct hx_stream *hx_stream_t;
+
+#define HX_STREAM_MAX_FRAME	64
+
+typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length);
+
+__BEGIN_DECLS
+
+/**
+ * Allocate a new hx_stream object.
+ *
+ * @param fd		The file handle over which the protocol will
+ *			communicate.
+ * @param callback	Called when a frame is received.
+ * @param callback_arg	Passed to the callback.
+ * @return		A handle to the stream, or NULL if memory could
+ *			not be allocated.
+ */
+__EXPORT extern hx_stream_t	hx_stream_init(int fd,
+		hx_stream_rx_callback callback,
+		void *arg);
+
+/**
+ * Free a hx_stream object.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_free(hx_stream_t stream);
+
+/**
+ * Set performance counters for the stream.
+ *
+ * Any counter may be set to NULL to disable counting that datum.
+ *
+ * @param tx_frames	Counter for transmitted frames.
+ * @param rx_frames	Counter for received frames.
+ * @param rx_errors	Counter for short and corrupt received frames.
+ */
+__EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
+		perf_counter_t tx_frames,
+		perf_counter_t rx_frames,
+		perf_counter_t rx_errors);
+
+/**
+ * Send a frame.
+ *
+ * This function will block until all frame bytes are sent if
+ * the descriptor passed to hx_stream_init is marked blocking,
+ * otherwise it will return -1 (but may transmit a
+ * runt frame at the same time).
+ *
+ * @todo Handling of non-blocking streams needs to be better.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_send(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Handle a byte from the stream.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param c		The character to process.
+ */
+__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
+		uint8_t c);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
new file mode 100644
index 000000000..4485daa5b
--- /dev/null
+++ b/src/modules/px4iofirmware/i2c.c
@@ -0,0 +1,340 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.c
+ *
+ * I2C communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <stm32_i2c.h>
+#include <stm32_dma.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+/*
+ * I2C register definitions.
+ */
+#define I2C_BASE	STM32_I2C1_BASE
+
+#define REG(_reg)	(*(volatile uint32_t *)(I2C_BASE + _reg))
+
+#define rCR1		REG(STM32_I2C_CR1_OFFSET)
+#define rCR2		REG(STM32_I2C_CR2_OFFSET)
+#define rOAR1		REG(STM32_I2C_OAR1_OFFSET)
+#define rOAR2		REG(STM32_I2C_OAR2_OFFSET)
+#define rDR		REG(STM32_I2C_DR_OFFSET)
+#define rSR1		REG(STM32_I2C_SR1_OFFSET)
+#define rSR2		REG(STM32_I2C_SR2_OFFSET)
+#define rCCR		REG(STM32_I2C_CCR_OFFSET)
+#define rTRISE		REG(STM32_I2C_TRISE_OFFSET)
+
+static int		i2c_interrupt(int irq, void *context);
+static void		i2c_rx_setup(void);
+static void		i2c_tx_setup(void);
+static void		i2c_rx_complete(void);
+static void		i2c_tx_complete(void);
+
+static DMA_HANDLE	rx_dma;
+static DMA_HANDLE	tx_dma;
+
+static uint8_t		rx_buf[68];
+static unsigned		rx_len;
+
+static const uint8_t	junk_buf[] = { 0xff, 0xff, 0xff, 0xff };
+
+static const uint8_t	*tx_buf = junk_buf;
+static unsigned		tx_len = sizeof(junk_buf);
+unsigned		tx_count;
+
+static uint8_t		selected_page;
+static uint8_t		selected_offset;
+
+enum {
+	DIR_NONE = 0,
+	DIR_TX = 1,
+	DIR_RX = 2
+} direction;
+
+void
+i2c_init(void)
+{
+	debug("i2c init");
+
+	/* allocate DMA handles and initialise DMA */
+	rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX);
+	i2c_rx_setup();
+	tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX);
+	i2c_tx_setup();
+
+	/* enable the i2c block clock and reset it */
+	modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN);
+	modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
+	modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
+
+	/* configure the i2c GPIOs */
+	stm32_configgpio(GPIO_I2C1_SCL);
+	stm32_configgpio(GPIO_I2C1_SDA);
+
+	/* soft-reset the block */
+	rCR1 |= I2C_CR1_SWRST;
+	rCR1 = 0;
+
+	/* set for DMA operation */
+	rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+	/* set the frequency value in CR2 */
+	rCR2 &= ~I2C_CR2_FREQ_MASK;
+	rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+	/* set divisor and risetime for fast mode */
+	uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+	if (result < 1)
+		result = 1;
+	result = 3;
+	rCCR &= ~I2C_CCR_CCR_MASK;
+	rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+	rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+	/* set our device address */
+	rOAR1 = 0x1a << 1;
+
+	/* enable event interrupts */
+	irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt);
+	irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt);
+	up_enable_irq(STM32_IRQ_I2C1EV);
+	up_enable_irq(STM32_IRQ_I2C1ER);
+
+	/* and enable the I2C port */
+	rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+
+#ifdef DEBUG
+	i2c_dump();
+#endif
+}
+
+
+/*
+  reset the I2C bus
+  used to recover from lockups
+ */
+void i2c_reset(void)
+{
+	rCR1 |= I2C_CR1_SWRST;
+	rCR1 = 0;
+
+	/* set for DMA operation */
+	rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+	/* set the frequency value in CR2 */
+	rCR2 &= ~I2C_CR2_FREQ_MASK;
+	rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+	/* set divisor and risetime for fast mode */
+	uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+	if (result < 1)
+		result = 1;
+	result = 3;
+	rCCR &= ~I2C_CCR_CCR_MASK;
+	rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+	rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+	/* set our device address */
+	rOAR1 = 0x1a << 1;
+
+	/* and enable the I2C port */
+	rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+}
+
+static int
+i2c_interrupt(int irq, FAR void *context)
+{
+	uint16_t sr1 = rSR1;
+
+	if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) {
+
+		if (sr1 & I2C_SR1_STOPF) {
+			/* write to CR1 to clear STOPF */
+			(void)rSR1;		/* as recommended, re-read SR1 */
+			rCR1 |= I2C_CR1_PE;
+		}
+
+		/* DMA never stops, so we should do that now */
+		switch (direction) {
+		case DIR_TX:
+			i2c_tx_complete();
+			break;
+		case DIR_RX:
+			i2c_rx_complete();
+			break;
+		default:
+			/* not currently transferring - must be a new txn */
+			break;
+		}
+		direction = DIR_NONE;
+	}
+
+	if (sr1 & I2C_SR1_ADDR) {
+
+		/* clear ADDR to ack our selection and get direction */
+		(void)rSR1;		/* as recommended, re-read SR1 */
+		uint16_t sr2 = rSR2;
+
+		if (sr2 & I2C_SR2_TRA) {
+			/* we are the transmitter */
+
+			direction = DIR_TX;
+
+		} else {
+			/* we are the receiver */
+
+			direction = DIR_RX;
+		}
+	}
+
+	/* clear any errors that might need it (this handles AF as well */
+	if (sr1 & I2C_SR1_ERRORMASK)
+		rSR1 = 0;
+
+	return 0;
+}
+
+static void
+i2c_rx_setup(void)
+{
+	/*
+	 * Note that we configure DMA in circular mode; this means that a too-long
+	 * transfer will overwrite the buffer, but that avoids us having to deal with
+	 * bailing out of a transaction while the master is still babbling at us.
+	 */
+	rx_len = 0;
+	stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf),
+		DMA_CCR_CIRC |
+		DMA_CCR_MINC |
+		DMA_CCR_PSIZE_32BITS |
+		DMA_CCR_MSIZE_8BITS |
+		DMA_CCR_PRIMED);
+
+	stm32_dmastart(rx_dma, NULL, NULL, false);			
+}
+
+static void
+i2c_rx_complete(void)
+{
+	rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma);
+	stm32_dmastop(rx_dma);
+
+	if (rx_len >= 2) {
+		selected_page = rx_buf[0];
+		selected_offset = rx_buf[1];
+
+		/* work out how many registers are being written */
+		unsigned count = (rx_len - 2) / 2;
+		if (count > 0) {
+			registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
+		} else {
+			/* no registers written, must be an address cycle */
+			uint16_t *regs;
+			unsigned reg_count;
+
+			/* work out which registers are being addressed */
+			int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
+			if (ret == 0) {
+				tx_buf = (uint8_t *)regs;
+				tx_len = reg_count * 2;
+			} else {
+				tx_buf = junk_buf;
+				tx_len = sizeof(junk_buf);
+			}
+
+			/* disable interrupts while reconfiguring DMA for the selected registers */
+			irqstate_t flags = irqsave();
+
+			stm32_dmastop(tx_dma);
+			i2c_tx_setup();
+
+			irqrestore(flags);
+		}
+	}
+
+	/* prepare for the next transaction */
+	i2c_rx_setup();
+}
+
+static void
+i2c_tx_setup(void)
+{
+	/*
+	 * Note that we configure DMA in circular mode; this means that a too-long
+	 * transfer will copy the buffer more than once, but that avoids us having
+	 * to deal with bailing out of a transaction while the master is still 
+	 * babbling at us.
+	 */
+	stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len,
+		DMA_CCR_DIR |
+		DMA_CCR_CIRC |
+		DMA_CCR_MINC |
+		DMA_CCR_PSIZE_8BITS |
+		DMA_CCR_MSIZE_8BITS |
+		DMA_CCR_PRIMED);
+
+	stm32_dmastart(tx_dma, NULL, NULL, false);
+}
+
+static void
+i2c_tx_complete(void)
+{
+	tx_count = tx_len - stm32_dmaresidual(tx_dma);
+	stm32_dmastop(tx_dma);
+
+	/* for debug purposes, save the length of the last transmit as seen by the DMA */
+
+	/* leave tx_buf/tx_len alone, so that a retry will see the same data */
+
+	/* prepare for the next transaction */
+	i2c_tx_setup();
+}
+
+void
+i2c_dump(void)
+{
+	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
+	debug("OAR1  0x%08x  OAR2  0x%08x", rOAR1, rOAR2);
+	debug("CCR   0x%08x  TRISE 0x%08x", rCCR,  rTRISE);
+	debug("SR1   0x%08x  SR2   0x%08x", rSR1,  rSR2);
+}
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
new file mode 100644
index 000000000..f38593d2a
--- /dev/null
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -0,0 +1,311 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer.cpp
+ *
+ * Control channel input/output mixer and failsafe.
+ */
+
+#include <nuttx/config.h>
+#include <syslog.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/mixer/mixer.h>
+
+extern "C" {
+//#define DEBUG
+#include "px4io.h"
+}
+
+/*
+ * Maximum interval in us before FMU signal is considered lost
+ */
+#define FMU_INPUT_DROP_LIMIT_US		200000
+
+/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
+#define ROLL     0
+#define PITCH    1
+#define YAW      2
+#define THROTTLE 3
+#define OVERRIDE 4
+
+/* current servo arm/disarm state */
+static bool mixer_servos_armed = false;
+
+/* selected control values and count for mixing */
+enum mixer_source {
+	MIX_NONE,
+	MIX_FMU,
+	MIX_OVERRIDE,
+	MIX_FAILSAFE
+};
+static mixer_source source;
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+static MixerGroup mixer_group(mixer_callback, 0);
+
+void
+mixer_tick(void)
+{
+	/* check that we are receiving fresh data from the FMU */
+	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+
+		/* too long without FMU input, time to go to failsafe */
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
+			isr_debug(1, "AP RX timeout");
+		}
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
+
+	} else {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+		r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
+	}
+
+	source = MIX_FAILSAFE;
+
+	/*
+	 * Decide which set of controls we're using.
+	 */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+		/* don't actually mix anything - we already have raw PWM values or
+		 not a valid mixer. */
+		source = MIX_NONE;
+
+	} else {
+
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+			/* mix from FMU controls */
+			source = MIX_FMU;
+		}
+
+		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+		 	/* if allowed, mix from RC inputs directly */
+			source = MIX_OVERRIDE;
+		}
+	}
+
+	/*
+	 * Run the mixers.
+	 */
+	if (source == MIX_FAILSAFE) {
+
+		/* copy failsafe values to the servo outputs */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			r_page_servos[i] = r_page_servo_failsafe[i];
+
+	} else if (source != MIX_NONE) {
+
+		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;
+
+		}
+		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+			r_page_servos[i] = 0;
+	}
+
+	/*
+	 * Decide whether the servos should be armed right now.
+	 *
+	 * We must be armed, and we must have a PWM source; either raw from
+	 * FMU or from the mixer.
+	 *
+	 * 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)) &&
+		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
+		/* FMU is available or FMU is not available but override is an option */
+		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+	);
+
+	if (should_arm && !mixer_servos_armed) {
+		/* need to arm, but not armed */
+		up_pwm_servo_arm(true);
+		mixer_servos_armed = true;
+
+	} else if (!should_arm && mixer_servos_armed) {
+		/* armed but need to disarm */
+		up_pwm_servo_arm(false);
+		mixer_servos_armed = false;
+	}
+
+	if (mixer_servos_armed) {
+		/* update the servo outputs. */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servos[i]);
+	}
+}
+
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
+{
+	if (control_group != 0)
+		return -1;
+
+	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:
+	case MIX_NONE:
+		/* XXX we could allow for configuration of per-output failsafe values */
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * XXX error handling here should be more aggressive; currently it is
+ * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
+ * not loaded faithfully.
+ */
+
+static char mixer_text[256];		/* large enough for one mixer */
+static unsigned mixer_text_length = 0;
+
+void
+mixer_handle_text(const void *buffer, size_t length)
+{
+	/* do not allow a mixer change while fully armed */
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		return;
+	}
+
+	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
+
+	isr_debug(2, "mix txt %u", length);
+
+	if (length < sizeof(px4io_mixdata))
+		return;
+
+	unsigned	text_length = length - sizeof(px4io_mixdata);
+
+	switch (msg->action) {
+	case F2I_MIXER_ACTION_RESET:
+		isr_debug(2, "reset");
+
+		/* FIRST mark the mixer as invalid */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+		/* THEN actually delete it */
+		mixer_group.reset();
+		mixer_text_length = 0;
+
+		/* FALLTHROUGH */
+	case F2I_MIXER_ACTION_APPEND:
+		isr_debug(2, "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)) {
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+			return;
+		}
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		isr_debug(2, "buflen %u", mixer_text_length);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+
+			/* ideally, this should test resid == 0 ? */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
+			isr_debug(2, "used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		break;
+	}
+}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
new file mode 100644
index 000000000..085697fbb
--- /dev/null
+++ b/src/modules/px4iofirmware/module.mk
@@ -0,0 +1,4 @@
+
+
+SRCS = adc.c controls.c dsm.c i2c.c mixer.cpp px4io.c registers.c safety.c sbus.c \
+       up_cxxinitialize.c hx_stream.c perf_counter.c
diff --git a/src/modules/px4iofirmware/perf_counter.c b/src/modules/px4iofirmware/perf_counter.c
new file mode 100644
index 000000000..879f4715a
--- /dev/null
+++ b/src/modules/px4iofirmware/perf_counter.c
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file perf_counter.c
+ *
+ * @brief Performance measuring tools.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/queue.h>
+#include <drivers/drv_hrt.h>
+
+#include "perf_counter.h"
+
+/**
+ * Header common to all counters.
+ */
+struct perf_ctr_header {
+	sq_entry_t		link;	/**< list linkage */
+	enum perf_counter_type	type;	/**< counter type */
+	const char		*name;	/**< counter name */
+};
+
+/**
+ * PC_EVENT counter.
+ */
+struct perf_ctr_count {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+};
+
+/**
+ * PC_ELAPSED counter.
+ */
+struct perf_ctr_elapsed {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_start;
+	uint64_t		time_total;
+	uint64_t		time_least;
+	uint64_t		time_most;
+};
+
+/**
+ * PC_INTERVAL counter.
+ */
+struct perf_ctr_interval {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_event;
+	uint64_t		time_first;
+	uint64_t		time_last;
+	uint64_t		time_least;
+	uint64_t		time_most;
+
+};
+
+/**
+ * List of all known counters.
+ */
+static sq_queue_t	perf_counters;
+
+
+perf_counter_t
+perf_alloc(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t ctr = NULL;
+
+	switch (type) {
+	case PC_COUNT:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1);
+		break;
+
+	case PC_ELAPSED:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
+		break;
+
+	case PC_INTERVAL:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctr != NULL) {
+		ctr->type = type;
+		ctr->name = name;
+		sq_addfirst(&ctr->link, &perf_counters);
+	}
+
+	return ctr;
+}
+
+void
+perf_free(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	sq_rem(&handle->link, &perf_counters);
+	free(handle);
+}
+
+void
+perf_count(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count++;
+		break;
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		hrt_abstime now = hrt_absolute_time();
+
+		switch (pci->event_count) {
+		case 0:
+			pci->time_first = now;
+			break;
+		case 1:
+			pci->time_least = now - pci->time_last;
+			pci->time_most = now - pci->time_last;
+			break;
+		default: {
+			hrt_abstime interval = now - pci->time_last;
+			if (interval < pci->time_least)
+				pci->time_least = interval;
+			if (interval > pci->time_most)
+				pci->time_most = interval;
+			break;
+		}
+		}
+		pci->time_last = now;
+		pci->event_count++;
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_begin(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED:
+		((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_end(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+
+			pce->event_count++;
+			pce->time_total += elapsed;
+
+			if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				pce->time_least = elapsed;
+
+			if (pce->time_most < elapsed)
+				pce->time_most = elapsed;
+		}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_reset(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count = 0;
+		break;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		pce->event_count = 0;
+		pce->time_start = 0;
+		pce->time_total = 0;
+		pce->time_least = 0;
+		pce->time_most = 0;
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		pci->event_count = 0;
+		pci->time_event = 0;
+		pci->time_first = 0;
+		pci->time_last = 0;
+		pci->time_least = 0;
+		pci->time_most = 0;
+		break;
+	}
+	}
+}
+
+void
+perf_print_counter(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		printf("%s: %llu events\n",
+		       handle->name,
+		       ((struct perf_ctr_count *)handle)->event_count);
+		break;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
+		       handle->name,
+		       pce->event_count,
+		       pce->time_total,
+		       pce->time_least,
+		       pce->time_most);
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+
+		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		       handle->name,
+		       pci->event_count,
+		       (pci->time_last - pci->time_first) / pci->event_count,
+		       pci->time_least,
+		       pci->time_most);
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_print_all(void)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_print_counter(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+}
+
+void
+perf_reset_all(void)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_reset(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+}
diff --git a/src/modules/px4iofirmware/perf_counter.h b/src/modules/px4iofirmware/perf_counter.h
new file mode 100644
index 000000000..5c2cb15b2
--- /dev/null
+++ b/src/modules/px4iofirmware/perf_counter.h
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file perf_counter.h
+ * Performance measuring tools.
+ */
+
+#ifndef _SYSTEMLIB_PERF_COUNTER_H
+#define _SYSTEMLIB_PERF_COUNTER_H value
+
+/**
+ * Counter types.
+ */
+enum perf_counter_type {
+	PC_COUNT,		/**< count the number of times an event occurs */
+	PC_ELAPSED,		/**< measure the time elapsed performing an event */
+	PC_INTERVAL		/**< measure the interval between instances of an event */
+};
+
+struct perf_ctr_header;
+typedef struct perf_ctr_header	*perf_counter_t;
+
+__BEGIN_DECLS
+
+/**
+ * Create a new counter.
+ *
+ * @param type			The type of the new counter.
+ * @param name			The counter name.
+ * @return			Handle for the new counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+__EXPORT extern void		perf_free(perf_counter_t handle);
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_count(perf_counter_t handle);
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_begin(perf_counter_t handle);
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_end(perf_counter_t handle);
+
+/**
+ * Reset a performance event.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_reset(perf_counter_t handle);
+
+/**
+ * Print one performance counter.
+ *
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter(perf_counter_t handle);
+
+/**
+ * Print all of the performance counters.
+ */
+__EXPORT extern void		perf_print_all(void);
+
+/**
+ * Reset all of the performance counters.
+ */
+__EXPORT extern void		perf_reset_all(void);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
new file mode 100644
index 000000000..8d8b7e941
--- /dev/null
+++ b/src/modules/px4iofirmware/protocol.h
@@ -0,0 +1,204 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file protocol.h
+ *
+ * PX4IO I2C interface protocol.
+ *
+ * Communication is performed via writes to and reads from 16-bit virtual
+ * registers organised into pages of 255 registers each.
+ *
+ * The first two bytes of each write select a page and offset address
+ * respectively. Subsequent reads and writes increment the offset within
+ * the page. 
+ *
+ * Most pages are readable or writable but not both.
+ *
+ * Note that some pages may permit offset values greater than 255, which
+ * can only be achieved by long writes. The offset does not wrap.
+ *
+ * Writes to unimplemented registers are ignored. Reads from unimplemented
+ * registers return undefined values.
+ *
+ * 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.  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
+ * packed.
+ */
+
+#define PX4IO_CONTROL_CHANNELS			8
+#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
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
+#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
+#define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
+#define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
+#define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
+#define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
+
+/* dynamic status page */
+#define PX4IO_PAGE_STATUS		1
+#define PX4IO_P_STATUS_FREEMEM			0
+#define PX4IO_P_STATUS_CPULOAD			1
+
+#define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
+#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
+#define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
+#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_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 */
+#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_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
+#define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
+#define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+
+#define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
+
+/* array of post-mix actuator outputs, -10000..10000 */
+#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of PWM servo output values, microseconds */
+#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of raw RC input values, microseconds */
+#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
+#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 */
+
+/* PWM servo information */
+#define PX4IO_PAGE_PWM_INFO		7
+#define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
+
+/* setup page */
+#define PX4IO_PAGE_SETUP			100
+#define PX4IO_P_SETUP_FEATURES			0
+
+#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_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3) /* OK to perform position / vector control (= position lock) */
+#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+
+#define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
+#define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_IBATT_SCALE		7	/* battery current scaling factor (float) */
+#define PX4IO_P_SETUP_IBATT_BIAS		8	/* battery current bias value */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+
+/* autopilot control values, -10000..10000 */
+#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+
+/* 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 */
+
+/* PWM output - overrides mixer */
+#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+#pragma pack(push, 1)
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET			0
+#define F2I_MIXER_ACTION_APPEND			1
+
+	char		text[0];	/* actual text size may vary */
+};
+#pragma pack(pop)
+
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
new file mode 100644
index 000000000..9de37e118
--- /dev/null
+++ b/src/modules/px4iofirmware/px4io.c
@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4io.c
+ * Top-level logic for the PX4IO module.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>	// required for task_create
+#include <stdbool.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+#include <poll.h>
+#include <signal.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[]);
+
+extern void up_cxxinitialize(void);
+
+struct sys_state_s 	system_state;
+
+static struct hrt_call serial_dma_call;
+
+/* store i2c reset count XXX this should be a register, together with other error counters */
+volatile uint32_t i2c_loop_resets = 0;
+
+/*
+ * a set of debug buffers to allow us to send debug information from ISRs
+ */
+
+static volatile uint32_t msg_counter;
+static volatile uint32_t last_msg_counter;
+static volatile uint8_t msg_next_out, msg_next_in;
+
+/*
+ * WARNING: too large buffers here consume the memory required
+ * for mixer handling. Do not allocate more than 80 bytes for
+ * output.
+ */
+#define NUM_MSG 2
+static char msg[NUM_MSG][40];
+
+/*
+ * add a debug message to be printed on the console
+ */
+void
+isr_debug(uint8_t level, const char *fmt, ...)
+{
+	if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) {
+		return;
+	}
+	va_list ap;
+	va_start(ap, fmt);
+	vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
+	va_end(ap);
+	msg_next_in = (msg_next_in+1) % NUM_MSG;
+	msg_counter++;
+}
+
+/*
+ * show all pending debug messages
+ */
+static void
+show_debug_messages(void)
+{
+	if (msg_counter != last_msg_counter) {
+		uint32_t n = msg_counter - last_msg_counter;
+		if (n > NUM_MSG) n = NUM_MSG;
+		last_msg_counter = msg_counter;
+		while (n--) {
+			debug("%s", msg[msg_next_out]);
+			msg_next_out = (msg_next_out+1) % NUM_MSG;
+		}
+	}
+}
+
+int
+user_start(int argc, char *argv[])
+{
+	/* run C++ ctors before we go any further */
+	up_cxxinitialize();
+
+	/* reset all to zero */
+	memset(&system_state, 0, sizeof(system_state));
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+
+	/* print some startup info */
+	lowsyslog("\nPX4IO: starting\n");
+
+	/* default all the LEDs to off while we start */
+	LED_AMBER(false);
+	LED_BLUE(false);
+	LED_SAFETY(false);
+
+	/* turn on servo power */
+	POWER_SERVO(true);
+
+	/* start the safety switch handler */
+	safety_init();
+
+	/* configure the first 8 PWM outputs (i.e. all of them) */
+	up_pwm_servo_init(0xff);
+
+	/* initialise the control inputs */
+	controls_init();
+
+	/* start the i2c handler */
+	i2c_init();
+
+	/* add a performance counter for mixing */
+	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
+
+	/* add a performance counter for controls */
+	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
+
+	/* and one for measuring the loop rate */
+	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
+
+	struct mallinfo minfo = mallinfo();
+	lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
+
+#if 0
+	/* not enough memory, lock down */
+	if (minfo.mxordblk < 500) {
+		lowsyslog("ERR: not enough MEM");
+		bool phase = false;
+
+		if (phase) {
+			LED_AMBER(true);
+			LED_BLUE(false);
+		} else {
+			LED_AMBER(false);
+			LED_BLUE(true);
+		}
+
+		phase = !phase;
+		usleep(300000);
+	}
+#endif
+
+	/*
+	 * Run everything in a tight loop.
+	 */
+
+	uint64_t last_debug_time = 0;
+	for (;;) {
+
+		/* track the rate at which the loop is running */
+		perf_count(loop_perf);
+
+		/* kick the mixer */
+		perf_begin(mixer_perf);
+		mixer_tick();
+		perf_end(mixer_perf);
+
+		/* kick the control inputs */
+		perf_begin(controls_perf);
+		controls_tick();
+		perf_end(controls_perf);
+
+		/* check for debug activity */
+		show_debug_messages();
+
+		/* post debug state at ~1Hz */
+		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
+
+			struct mallinfo minfo = mallinfo();
+
+			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", 
+				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
+				  (unsigned)r_status_flags,
+				  (unsigned)r_setup_arming,
+				  (unsigned)r_setup_features,
+				  (unsigned)i2c_loop_resets,
+				  (unsigned)minfo.mxordblk);
+			last_debug_time = hrt_absolute_time();
+		}
+	}
+}
+
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
new file mode 100644
index 000000000..202e9d9d9
--- /dev/null
+++ b/src/modules/px4iofirmware/px4io.h
@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4io.h
+ *
+ * General defines and structures for the PX4IO module firmware.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <drivers/boards/px4io/px4io_internal.h>
+
+#include "protocol.h"
+
+/*
+ * Constants and limits.
+ */
+#define MAX_CONTROL_CHANNELS	12
+#define IO_SERVO_COUNT		8
+
+/*
+ * Debug logging
+ */
+
+#ifdef DEBUG
+# include <debug.h>
+# define debug(fmt, args...)	lowsyslog(fmt "\n", ##args)
+#else
+# define debug(fmt, args...)	do {} while(0)
+#endif
+
+/*
+ * Registers.
+ */
+extern uint16_t			r_page_status[];	/* PX4IO_PAGE_STATUS */
+extern uint16_t			r_page_actuators[];	/* PX4IO_PAGE_ACTUATORS */
+extern uint16_t			r_page_servos[];	/* PX4IO_PAGE_SERVOS */
+extern uint16_t			r_page_raw_rc_input[];	/* PX4IO_PAGE_RAW_RC_INPUT */
+extern uint16_t			r_page_rc_input[];	/* PX4IO_PAGE_RC_INPUT */
+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 */
+extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+
+/*
+ * 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_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
+#define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
+#define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+
+#define r_control_values	(&r_page_controls[0])
+
+/*
+ * System state structure.
+ */
+struct sys_state_s {
+
+	volatile uint64_t	rc_channels_timestamp;
+
+	/**
+	 * Last FMU receive time, in microseconds since system boot
+	 */
+	volatile uint64_t	fmu_data_received_time;
+
+};
+
+extern struct sys_state_s system_state;
+
+/*
+ * GPIO handling.
+ */
+#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+#define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+#define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+#define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+#define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+#define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
+
+#define ADC_VBATT		4
+#define ADC_IN5			5
+#define ADC_CHANNEL_COUNT	2
+
+/*
+ * Mixer
+ */
+extern void	mixer_tick(void);
+extern void	mixer_handle_text(const void *buffer, size_t length);
+
+/**
+ * Safety switch/LED.
+ */
+extern void	safety_init(void);
+
+/**
+ * FMU communications
+ */
+extern void	i2c_init(void);
+
+/**
+ * Register space
+ */
+extern void	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int	registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
+
+/**
+ * Sensors/misc inputs
+ */
+extern int	adc_init(void);
+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_init(void);
+extern void	controls_tick(void);
+extern int	dsm_init(const char *device);
+extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern int	sbus_init(const char *device);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
+
+/** global debug level for isr_debug() */
+extern volatile uint8_t debug_level;
+
+/* send a debug message to the console */
+extern void isr_debug(uint8_t level, const char *fmt, ...);
+
+void i2c_dump(void);
+void i2c_reset(void);
+
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
new file mode 100644
index 000000000..6c09def9e
--- /dev/null
+++ b/src/modules/px4iofirmware/registers.c
@@ -0,0 +1,644 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file registers.c
+ *
+ * Implementation of the PX4IO register space.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+#include "protocol.h"
+
+static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
+static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
+
+/**
+ * PAGE 0
+ *
+ * Static configuration parameters.
+ */
+static const uint16_t	r_page_config[] = {
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 3,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
+};
+
+/**
+ * PAGE 1
+ *
+ * Status values.
+ */
+uint16_t		r_page_status[] = {
+	[PX4IO_P_STATUS_FREEMEM]		= 0,
+	[PX4IO_P_STATUS_CPULOAD]		= 0,
+	[PX4IO_P_STATUS_FLAGS]			= 0,
+	[PX4IO_P_STATUS_ALARMS]			= 0,
+	[PX4IO_P_STATUS_VBATT]			= 0,
+	[PX4IO_P_STATUS_IBATT]			= 0
+};
+
+/**
+ * PAGE 2
+ *
+ * Post-mixed actuator values.
+ */
+uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+
+/**
+ * PAGE 3
+ *
+ * Servo PWM values
+ */
+uint16_t		r_page_servos[IO_SERVO_COUNT];
+
+/**
+ * PAGE 4
+ *
+ * Raw RC input
+ */
+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
+};
+
+/**
+ * PAGE 5
+ *
+ * Scaled/routed RC input
+ */
+uint16_t		r_page_rc_input[] = {
+	[PX4IO_P_RC_VALID]			= 0,
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
+
+/**
+ * Scratch page; used for registers that are constructed as-read.
+ *
+ * PAGE 6 Raw ADC input.
+ * PAGE 7 PWM rate maps.
+ */
+uint16_t		r_page_scratch[32];
+
+/**
+ * PAGE 100
+ *
+ * Setup registers
+ */
+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_DEFAULTRATE]		= 50,
+	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
+	[PX4IO_P_SETUP_RELAYS]			= 0,
+	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
+	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
+	[PX4IO_P_SETUP_IBATT_BIAS]		= 0,
+	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
+};
+
+#define PX4IO_P_SETUP_FEATURES_VALID	(0)
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
+					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+#define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
+
+/**
+ * PAGE 101
+ *
+ * Control values from the FMU.
+ */
+volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
+
+/*
+ * PAGE 102 does not have a buffer.
+ */
+
+/**
+ * PAGE 103
+ *
+ * R/C channel input configuration.
+ */
+uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+
+/* valid options */
+#define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
+
+/*
+ * PAGE 104 uses r_page_servos.
+ */
+
+/**
+ * PAGE 105
+ *
+ * Failsafe servo PWM values
+ */
+uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
+
+void
+registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+
+	switch (page) {
+
+		/* handle bulk controls input */
+	case PX4IO_PAGE_CONTROLS:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_controls[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+
+		system_state.fmu_data_received_time = hrt_absolute_time();
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+        	r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
+		
+		break;
+
+		/* handle raw PWM input */
+	case PX4IO_PAGE_DIRECT_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_servos[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+
+		system_state.fmu_data_received_time = hrt_absolute_time();
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
+
+		break;
+
+		/* handle setup for servo failsafe values */
+	case PX4IO_PAGE_FAILSAFE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_servo_failsafe[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+		/* handle text going to the mixer parser */
+	case PX4IO_PAGE_MIXERLOAD:
+		mixer_handle_text(values, num_values * sizeof(*values));
+		break;
+
+	default:
+		/* avoid offset wrap */
+		if ((offset + num_values) > 255)
+			num_values = 255 - offset;
+
+		/* iterate individual registers, set each in turn */
+		while (num_values--) {
+			if (registers_set_one(page, offset, *values))
+				break;
+			offset++;
+			values++;
+		}
+	}
+}
+
+static int
+registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
+{
+	switch (page) {
+
+	case PX4IO_PAGE_STATUS:
+		switch (offset) {
+		case PX4IO_P_STATUS_ALARMS:
+			/* clear bits being written */
+			r_status_alarms &= ~value;
+			break;
+
+		case PX4IO_P_STATUS_FLAGS:
+			/* 
+			 * Allow FMU override of arming state (to allow in-air restores),
+			 * but only if the arming state is not in sync on the IO side.
+			 */
+			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+				r_status_flags = value;
+			}
+			break;
+
+		default:
+			/* just ignore writes to other registers in this page */
+			break;
+		}
+		break;
+
+	case PX4IO_PAGE_SETUP:
+		switch (offset) {
+		case PX4IO_P_SETUP_FEATURES:
+
+			value &= PX4IO_P_SETUP_FEATURES_VALID;
+			r_setup_features = value;
+
+			/* no implemented feature selection at this point */
+
+			break;
+
+		case PX4IO_P_SETUP_ARMING:
+
+			value &= PX4IO_P_SETUP_ARMING_VALID;
+
+			/*
+			 * Update arming state - disarm if no longer OK.
+			 * This builds on the requirement that the FMU driver
+			 * asks about the FMU arming state on initialization,
+			 * so that an in-air reset of FMU can not lead to a
+			 * lockup of the IO arming state.
+			 */
+			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			}
+
+			r_setup_arming = value;
+
+			break;
+
+		case PX4IO_P_SETUP_PWM_RATES:
+			value &= PX4IO_P_SETUP_RATES_VALID;
+			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
+			break;
+
+		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
+			if (value < 50)
+				value = 50;
+			if (value > 400)
+				value = 400;
+			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
+			break;
+
+		case PX4IO_P_SETUP_PWM_ALTRATE:
+			if (value < 50)
+				value = 50;
+			if (value > 400)
+				value = 400;
+			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
+			break;
+
+		case PX4IO_P_SETUP_RELAYS:
+			value &= PX4IO_P_SETUP_RELAYS_VALID;
+			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);
+			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			break;
+
+		case PX4IO_P_SETUP_SET_DEBUG:
+			r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
+			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
+			break;
+
+		default:
+			return -1;
+		}
+		break;
+
+	case PX4IO_PAGE_RC_CONFIG: {
+
+		/* do not allow a RC config change while fully armed */
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+			break;
+		}
+
+		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
+		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
+		uint16_t *conf = &r_page_rc_input_config[channel * 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;
+			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;
+
+			/* 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) {
+					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) {
+					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 */
+				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 */
+
+		}
+		break;
+		/* case PX4IO_RC_PAGE_CONFIG */
+	}
+
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+uint8_t last_page;
+uint8_t last_offset;
+
+int
+registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
+{
+#define SELECT_PAGE(_page_name)							\
+	do {									\
+		*values = &_page_name[0];					\
+		*num_values = sizeof(_page_name) / sizeof(_page_name[0]);	\
+	} while(0)
+
+	switch (page) {
+
+	/*
+	 * 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 */
+
+		/* 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
+			 * range on one unit:
+			 *
+			 * V   counts
+			 *  5  1001
+			 *  6  1219
+			 *  7  1436
+			 *  8  1653
+			 *  9  1870
+			 * 10  2086
+			 * 11  2303
+			 * 12  2522
+			 * 13  2738
+			 * 14  2956
+			 * 15  3172
+			 * 16  3389
+			 *
+			 * slope = 0.0046067
+			 * intercept = 0.3863
+			 *
+			 * Intercept corrected for best results @ 12V.
+			 */
+			unsigned counts = adc_measure(ADC_VBATT);
+			unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+			unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+
+			r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+		}
+
+		/* PX4IO_P_STATUS_IBATT */
+		{
+			unsigned counts = adc_measure(ADC_VBATT);
+			unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
+			int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
+			if (corrected < 0)
+				corrected = 0;
+			r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+		}
+
+		SELECT_PAGE(r_page_status);
+		break;
+
+	case PX4IO_PAGE_RAW_ADC_INPUT:
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		r_page_scratch[0] = adc_measure(ADC_VBATT);
+		r_page_scratch[1] = adc_measure(ADC_IN5);
+
+		SELECT_PAGE(r_page_scratch);
+		break;
+
+	case PX4IO_PAGE_PWM_INFO:
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
+
+		SELECT_PAGE(r_page_scratch);
+		break;
+
+	/*
+	 * Pages that are just a straight read of the register state.
+	 */
+
+	/* status pages */
+	case PX4IO_PAGE_CONFIG:
+		SELECT_PAGE(r_page_config);
+		break;
+	case PX4IO_PAGE_ACTUATORS:
+		SELECT_PAGE(r_page_actuators);
+		break;
+	case PX4IO_PAGE_SERVOS:
+		SELECT_PAGE(r_page_servos);
+		break;
+	case PX4IO_PAGE_RAW_RC_INPUT:
+		SELECT_PAGE(r_page_raw_rc_input);
+		break;
+	case PX4IO_PAGE_RC_INPUT:
+		SELECT_PAGE(r_page_rc_input);
+		break;
+
+	/* readback of input pages */
+	case PX4IO_PAGE_SETUP:
+		SELECT_PAGE(r_page_setup);
+		break;
+	case PX4IO_PAGE_CONTROLS:
+		SELECT_PAGE(r_page_controls);
+		break;
+	case PX4IO_PAGE_RC_CONFIG:
+		SELECT_PAGE(r_page_rc_input_config);
+		break;
+	case PX4IO_PAGE_DIRECT_PWM:
+		SELECT_PAGE(r_page_servos);
+		break;
+	case PX4IO_PAGE_FAILSAFE_PWM:
+		SELECT_PAGE(r_page_servo_failsafe);
+		break;
+
+	default:
+		return -1;
+	}
+
+#undef SELECT_PAGE
+#undef COPY_PAGE
+
+last_page = page;
+last_offset = offset;
+
+	/* if the offset is at or beyond the end of the page, we have no data */
+	if (offset >= *num_values)
+		return -1;
+
+	/* correct the data pointer and count for the offset */
+	*values += offset;
+	*num_values -= offset;
+
+	return 0;
+}
+
+/*
+ * Helper function to handle changes to the PWM rate control registers.
+ */
+static void
+pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
+{
+	for (unsigned pass = 0; pass < 2; pass++) {
+		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+
+			/* get the channel mask for this rate group */
+			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			if (mask == 0)
+				continue;
+
+			/* all channels in the group must be either default or alt-rate */
+			uint32_t alt = map & mask;
+
+			if (pass == 0) {
+				/* preflight */
+				if ((alt != 0) && (alt != mask)) {
+					/* not a legal map, bail with an alarm */
+					r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+					return;
+				}
+			} else {
+				/* set it - errors here are unexpected */
+				if (alt != 0) {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				}
+			}
+		}
+	}
+	r_setup_pwm_rates = map;
+	r_setup_pwm_defaultrate = defaultrate;
+	r_setup_pwm_altrate = altrate;
+}
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
new file mode 100644
index 000000000..5495d5ae1
--- /dev/null
+++ b/src/modules/px4iofirmware/safety.c
@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Safety button logic.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+
+static struct hrt_call arming_call;
+static struct hrt_call heartbeat_call;
+static struct hrt_call failsafe_call;
+
+/*
+ * Count the number of times in a row that we see the arming button
+ * held down.
+ */
+static unsigned counter = 0;
+
+/*
+ * Define the various LED flash sequences for each system state.
+ */
+#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
+#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0xFFFE		/**< always on with short break 	*/
+#define LED_PATTERN_FMU_ARMED 			0x4444		/**< slow blinking			*/
+#define LED_PATTERN_IO_ARMED 			0x5555		/**< fast blinking 			*/
+#define LED_PATTERN_IO_FMU_ARMED 		0x5050		/**< long off then double blink 	*/
+
+static unsigned blink_counter = 0;
+
+/*
+ * IMPORTANT: The arming state machine critically
+ * 	      depends on using the same threshold
+ *            for arming and disarming. Since disarming
+ *            is quite deadly for the system, a similar
+ *            length can be justified.
+ */
+#define ARM_COUNTER_THRESHOLD	10
+
+static bool safety_button_pressed;
+
+static void safety_check_button(void *arg);
+static void heartbeat_blink(void *arg);
+static void failsafe_blink(void *arg);
+
+void
+safety_init(void)
+{
+	/* arrange for the button handler to be called at 10Hz */
+	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+
+	/* arrange for the heartbeat handler to be called at 4Hz */
+	hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
+
+	/* arrange for the failsafe blinker to be called at 8Hz */
+	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
+}
+
+static void
+safety_check_button(void *arg)
+{
+	/*
+	 * Debounce the safety button, change state if it has been held for long enough.
+	 *
+	 */
+	safety_button_pressed = BUTTON_SAFETY;
+
+	/*
+	 * Keep pressed for a while to arm.
+	 *
+	 * Note that the counting sequence has to be same length
+	 * for arming / disarming in order to end up as proper
+	 * state machine, keep ARM_COUNTER_THRESHOLD the same
+	 * length in all cases of the if/else struct below.
+	 */
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+
+		if (counter < ARM_COUNTER_THRESHOLD) {
+			counter++;
+
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
+			/* switch to armed state */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			counter++;
+		}
+
+		/* Disarm quickly */
+
+	} 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 */
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			counter++;
+		}
+
+	} else {
+		counter = 0;
+	}
+
+	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
+	uint16_t pattern = LED_PATTERN_SAFE;
+
+	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 (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+		pattern = LED_PATTERN_FMU_ARMED;
+
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
+		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
+	}
+
+	/* Turn the LED on if we have a 1 at the current bit position */
+	LED_SAFETY(pattern & (1 << blink_counter++));
+
+	if (blink_counter > 15) {
+		blink_counter = 0;
+	}
+}
+
+static void
+heartbeat_blink(void *arg)
+{
+	static bool heartbeat = false;
+
+	/* XXX add flags here that need to be frobbed by various loops */
+
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
+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;
+	}
+
+	LED_AMBER(failsafe);
+}
\ No newline at end of file
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
new file mode 100644
index 000000000..073ddeaba
--- /dev/null
+++ b/src/modules/px4iofirmware/sbus.c
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sbus.c
+ *
+ * Serial protocol decoder for the Futaba S.bus protocol.
+ */
+
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <systemlib/ppm_decode.h>
+
+#include <drivers/drv_hrt.h>
+
+#define DEBUG
+#include "px4io.h"
+#include "protocol.h"
+#include "debug.h"
+
+#define SBUS_FRAME_SIZE		25
+#define SBUS_INPUT_CHANNELS	16
+
+static int sbus_fd = -1;
+
+static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
+
+static uint8_t	frame[SBUS_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+
+unsigned sbus_frame_drops;
+
+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 | O_NONBLOCK);
+
+	if (sbus_fd >= 0) {
+		struct termios t;
+
+		/* 100000bps, even parity, two stop bits */
+		tcgetattr(sbus_fd, &t);
+		cfsetspeed(&t, 100000);
+		t.c_cflag |= (CSTOPB | PARENB);
+		tcsetattr(sbus_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		partial_frame_count = 0;
+		last_rx_time = hrt_absolute_time();
+
+		debug("S.Bus: ready");
+
+	} else {
+		debug("S.Bus: open failed");
+	}
+
+	return sbus_fd;
+}
+
+bool
+sbus_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 * The S.bus protocol doesn't provide reliable framing,
+	 * so we detect frame boundaries by the inter-frame delay.
+	 *
+	 * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
+	 * frame transmission time is ~2ms.
+	 *
+	 * We expect to only be called when bytes arrive for processing,
+	 * and if an interval of more than 3ms passes between calls,
+	 * the first byte we read will be the first byte of a frame.
+	 *
+	 * In the case where byte(s) are dropped from a frame, this also
+	 * provides a degree of protection. Of course, it would be better
+	 * if we didn't drop bytes...
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - last_rx_time) > 3000) {
+		if (partial_frame_count > 0) {
+			sbus_frame_drops++;
+			partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current frame.
+	 */
+	ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	last_rx_time = now;
+
+	/*
+	 * Add bytes to the current frame
+	 */
+	partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full frame, return
+	 */
+	if (partial_frame_count < SBUS_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	partial_frame_count = 0;
+	return sbus_decode(now, values, num_values);
+}
+
+/*
+ * S.bus decoder matrix.
+ *
+ * Each channel value can come from up to 3 input bytes. Each row in the
+ * matrix describes up to three bytes, and each entry gives:
+ *
+ * - byte offset in the data portion of the frame
+ * - right shift applied to the data byte
+ * - mask for the data byte
+ * - left shift applied to the result into the channel value
+ */
+struct sbus_bit_pick {
+	uint8_t byte;
+	uint8_t rshift;
+	uint8_t mask;
+	uint8_t lshift;
+};
+static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
+	/*  0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
+	/*  1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
+	/*  2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
+	/*  3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
+	/*  4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
+	/*  5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03,  9} },
+	/*  6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
+	/*  7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00,  0} },
+	/*  8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
+	/*  9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
+	/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
+	/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
+	/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
+	/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03,  9} },
+	/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
+	/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00,  0} }
+};
+
+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 false;
+	}
+
+	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
+	if ((frame[23] & (1 << 2)) && /* signal lost */
+	    (frame[23] & (1 << 3))) { /* failsafe */
+
+		/* actively announce signal loss */
+		*values = 0;
+		return false;
+	}
+
+	/* we have received something we think is a frame */
+	last_frame_time = frame_time;
+
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
+			     SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+
+	/* use the decoder matrix to extract channel data */
+	for (unsigned channel = 0; channel < chancount; channel++) {
+		unsigned value = 0;
+
+		for (unsigned pick = 0; pick < 3; pick++) {
+			const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
+
+			if (decode->mask != 0) {
+				unsigned piece = frame[1 + decode->byte];
+				piece >>= decode->rshift;
+				piece &= decode->mask;
+				piece <<= decode->lshift;
+
+				value |= piece;
+			}
+		}
+
+		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
+		values[channel] = (value / 2) + 998;
+	}
+
+	/* decode switch channels if data fields are wide enough */
+	if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
+		chancount = 18;
+
+		/* channel 17 (index 16) */
+		values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		/* channel 18 (index 17) */
+		values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+	}
+
+	/* note the number of channels decoded */
+	*num_values = chancount;
+
+	return true;
+}
diff --git a/src/modules/px4iofirmware/up_cxxinitialize.c b/src/modules/px4iofirmware/up_cxxinitialize.c
new file mode 100644
index 000000000..c78f29570
--- /dev/null
+++ b/src/modules/px4iofirmware/up_cxxinitialize.c
@@ -0,0 +1,150 @@
+/************************************************************************************
+ * configs/stm32f4discovery/src/up_cxxinitialize.c
+ * arch/arm/src/board/up_cxxinitialize.c
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <debug.h>
+
+#include <nuttx/arch.h>
+
+//#include <arch/stm32/chip.h>
+//#include "chip.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+/* Debug ****************************************************************************/
+/* Non-standard debug that may be enabled just for testing the static constructors */
+
+#ifndef CONFIG_DEBUG
+#  undef CONFIG_DEBUG_CXX
+#endif
+
+#ifdef CONFIG_DEBUG_CXX
+#  define cxxdbg              dbg
+#  define cxxlldbg            lldbg
+#  ifdef CONFIG_DEBUG_VERBOSE
+#    define cxxvdbg           vdbg
+#    define cxxllvdbg         llvdbg
+#  else
+#    define cxxvdbg(x...)
+#    define cxxllvdbg(x...)
+#  endif
+#else
+#  define cxxdbg(x...)
+#  define cxxlldbg(x...)
+#  define cxxvdbg(x...)
+#  define cxxllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Types
+ ************************************************************************************/
+/* This type defines one entry in initialization array */
+
+typedef void (*initializer_t)(void);
+
+/************************************************************************************
+ * External references
+ ************************************************************************************/
+/* _sinit and _einit are symbols exported by the linker script that mark the
+ * beginning and the end of the C++ initialization section.
+ */
+
+extern initializer_t _sinit;
+extern initializer_t _einit;
+
+/* _stext and _etext are symbols exported by the linker script that mark the
+ * beginning and the end of text.
+ */
+
+extern uint32_t _stext;
+extern uint32_t _etext;
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/****************************************************************************
+ * Name: up_cxxinitialize
+ *
+ * Description:
+ *   If C++ and C++ static constructors are supported, then this function
+ *   must be provided by board-specific logic in order to perform
+ *   initialization of the static C++ class instances.
+ *
+ *   This function should then be called in the application-specific
+ *   user_start logic in order to perform the C++ initialization.  NOTE
+ *   that no component of the core NuttX RTOS logic is involved; This
+ *   function defintion only provides the 'contract' between application
+ *   specific C++ code and platform-specific toolchain support
+ *
+ ***************************************************************************/
+
+__EXPORT void up_cxxinitialize(void)
+{
+  initializer_t *initp;
+
+  cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n",
+         &_sinit, &_einit, &_stext, &_etext);
+
+  /* Visit each entry in the initialzation table */
+
+  for (initp = &_sinit; initp != &_einit; initp++)
+    {
+      initializer_t initializer = *initp;
+      cxxdbg("initp: %p initializer: %p\n", initp, initializer);
+
+      /* Make sure that the address is non-NULL and lies in the text region
+       * defined by the linker script.  Some toolchains may put NULL values
+       * or counts in the initialization table
+       */
+
+      if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext)
+        {
+          cxxdbg("Calling %p\n", initializer);
+          initializer();
+        }
+    }
+}