Merge pull request #80 from PX4/#61-px4io-spektrum-decoder

#61 px4io spektrum decoder

11 files changed, 665 insertions, 358 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index f2c87473c..296c541c9 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -53,6 +53,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <termios.h>
 #include <fcntl.h>
 
 #include <arch/board/board.h>
@@ -87,15 +88,13 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
-	void			set_rx_mode(unsigned mode);
-
 private:
 	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
 	int			_serial_fd;	///< serial interface to PX4IO
 	hx_stream_t		_io_stream;	///< HX protocol stream
 
-	int			_task;		///< worker task
+	volatile int		_task;		///< worker task
 	volatile bool		_task_should_exit;
 	volatile bool		_connected;	///< true once we have received a valid frame
 
@@ -121,8 +120,6 @@ private:
 	bool			_send_needed;	///< If true, we need to send a packet to IO
 	bool			_config_needed;	///< if true, we need to set a config update to IO
 
-	uint8_t			_rx_mode;	///< the current RX mode on IO
-
 	/**
 	 * Trampoline to the worker task
 	 */
@@ -190,8 +187,7 @@ PX4IO::PX4IO() :
 	_primary_pwm_device(false),
 	_switch_armed(false),
 	_send_needed(false),
-	_config_needed(false),
-	_rx_mode(RX_MODE_PPM_ONLY)
+	_config_needed(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -201,33 +197,17 @@ PX4IO::PX4IO() :
 
 PX4IO::~PX4IO()
 {
-	if (_task != -1) {
-		/* tell the task we want it to go away */
-		_task_should_exit = true;
-
-		/* spin waiting for the thread to stop */
-		unsigned i = 10;
-
-		do {
-			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(50000);
-
-			/* if we have given up, kill it */
-			if (--i == 0) {
-				task_delete(_task);
-				break;
-			}
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
 
-		} while (_task != -1);
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
 	}
-
-	/* clean up the alternate device node */
-	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
-
-	/* kill the HX stream */
-	if (_io_stream != nullptr)
-		hx_stream_free(_io_stream);
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
 
 	g_dev = nullptr;
 }
@@ -295,6 +275,16 @@ PX4IO::task_main()
 		goto out;
 	}
 
+	/* 115200bps, no parity, one stop bit */
+	{
+		struct termios t;
+
+		tcgetattr(_serial_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(_serial_fd, TCSANOW, &t);
+	}
+
 	/* protocol stream */
 	_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
 	if (_io_stream == nullptr) {
@@ -348,7 +338,6 @@ PX4IO::task_main()
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 		}
 
@@ -402,8 +391,16 @@ PX4IO::task_main()
 	}
 
 out:
+	debug("exiting");
+
+	/* kill the HX stream */
 	if (_io_stream != nullptr)
 		hx_stream_free(_io_stream);
+	::close(_serial_fd);
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	/* tell the dtor that we are exiting */
 	_task = -1;
@@ -464,15 +461,17 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 	}
 	_connected = true;
 
-	/* publish raw rc channel values from IO */
-	_input_rc.timestamp = hrt_absolute_time();
-	_input_rc.channel_count = rep->channel_count;
-	for (int i = 0; i < rep->channel_count; i++)
-	{
-		_input_rc.values[i] = rep->rc_channel[i];
-	}
+	/* publish raw rc channel values from IO if valid channels are present */
+	if (rep->channel_count > 0) {
+		_input_rc.timestamp = hrt_absolute_time();
+		_input_rc.channel_count = rep->channel_count;
+		for (int i = 0; i < rep->channel_count; i++)
+		{
+			_input_rc.values[i] = rep->rc_channel[i];
+		}
 
-	orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
+		orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
+	}
 
 	/* remember the latched arming switch state */
 	_switch_armed = rep->armed;
@@ -514,7 +513,6 @@ PX4IO::config_send()
 	int		ret;
 
 	cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
-	cfg.serial_rx_mode = _rx_mode;
 
 	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
 	if (ret)
@@ -634,15 +632,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	return ret;
 }
 
-void
-PX4IO::set_rx_mode(unsigned mode)
-{
-	if (mode != _rx_mode) {
-		_rx_mode = mode;
-		_config_needed = true;
-	}
-}
-
 extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 
 namespace
@@ -744,25 +733,14 @@ px4io_main(int argc, char *argv[])
 		return ret;
 	}
 
-	if (!strcmp(argv[1], "rx_dsm_10bit")) {
-		if (g_dev == nullptr)
-			errx(1, "not started");
-		g_dev->set_rx_mode(RX_MODE_DSM_10BIT);
-	}
-	if (!strcmp(argv[1], "rx_dsm_11bit")) {
-		if (g_dev == nullptr)
-			errx(1, "not started");
-		g_dev->set_rx_mode(RX_MODE_DSM_11BIT);
-	}
-	if (!strcmp(argv[1], "rx_sbus")) {
-		if (g_dev == nullptr)
-			errx(1, "not started");
-		g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
-	}
-
+	if (!strcmp(argv[1], "rx_dsm") ||
+	    !strcmp(argv[1], "rx_dsm_10bit") ||
+	    !strcmp(argv[1], "rx_dsm_11bit") ||
+	    !strcmp(argv[1], "rx_sbus") ||
+	    !strcmp(argv[1], "rx_ppm"))
+		errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
 	if (!strcmp(argv[1], "test"))
 		test();
 
-
-	errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'");
+	errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
 }
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 5669aeb01..8b354ff60 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -189,8 +189,10 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		ret = recv(c, 10);
-		//log("discard 0x%02x", c);
+		ret = recv(c, 250);
+		if (ret == OK) {
+			//log("discard 0x%02x", c);
+		}
 	} while (ret == OK);
 }
 
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index f1dac433f..40ea38cf7 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -32,10 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file FMU communication for the PX4IO module.
+ * @file comms.c
+ *
+ * FMU communication for the PX4IO module.
  */
 
-
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdbool.h>
@@ -44,9 +45,9 @@
 #include <debug.h>
 #include <stdlib.h>
 #include <errno.h>
-#include <termios.h>
 #include <string.h>
 #include <poll.h>
+#include <termios.h>
 
 #include <nuttx/clock.h>
 
@@ -54,103 +55,59 @@
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 
+#define DEBUG
 #include "px4io.h"
 
 #define FMU_MIN_REPORT_INTERVAL	  5000	/*  5ms */
 #define FMU_MAX_REPORT_INTERVAL	100000	/* 100ms */
 
+int frame_rx;
+int frame_bad;
+
 static int			fmu_fd;
 static hx_stream_t		stream;
-static int			rx_fd;
 static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
-static struct pollfd pollfds[2];
-static int pollcount;
-
-void
+static void
 comms_init(void)
 {
 	/* initialise the FMU interface */
-	fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK);
-	if (fmu_fd < 0)
-		lib_lowprintf("COMMS: fmu open failed %d\n", errno);
+	fmu_fd = open("/dev/ttyS1", O_RDWR);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
-	pollfds[0].fd = fmu_fd;
-	pollfds[0].events = POLLIN;
-	pollcount = 1;
 
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
 
-}
-
-static void
-serial_rx_init(unsigned serial_mode)
-{
-	if (serial_mode == system_state.serial_rx_mode)
-		return;
-	system_state.serial_rx_mode = serial_mode;
-
-	if (serial_mode == RX_MODE_PPM_ONLY) {
-		if (rx_fd != -1) {
-			pollcount = 1;
-			close(rx_fd);
-			rx_fd = -1;
-		}
-		return;
-	}
-
-	/* open the serial port used for DSM and S.bus communication */
-	rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
-	pollfds[1].fd = rx_fd;
-	pollfds[1].events = POLLIN;
-	pollcount = 2;
-
 	struct termios t;
-	tcgetattr(rx_fd, &t);
-
-	switch (serial_mode) {
-	case RX_MODE_DSM_10BIT:
-	case RX_MODE_DSM_11BIT:
-
-		/* 115200, no parity, one stop bit */
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-
-		dsm_init(serial_mode);
-		break;
-
-	case RX_MODE_FUTABA_SBUS:
-		/* 100000, even parity, two stop bits */
-		cfsetspeed(&t, 100000);
-		t.c_cflag |= (CSTOPB | PARENB);
-
-		sbus_init(serial_mode);
-		break;
-
-	default:
-		break;
-	}
 
-	tcsetattr(rx_fd, TCSANOW, &t);
+	/* 115200bps, no parity, one stop bit */
+	tcgetattr(fmu_fd, &t);
+	cfsetspeed(&t, 115200);
+	t.c_cflag &= ~(CSTOPB | PARENB);
+	tcsetattr(fmu_fd, TCSANOW, &t);
 }
 
 void
-comms_check(void)
+comms_main(void)
 {
-	/*
-	 * Check for serial data
-	 */
-	int ret = poll(pollfds, pollcount, 0);
+	comms_init();
+
+	struct pollfd fds;
+	fds.fd = fmu_fd;
+	fds.events = POLLIN;
+	debug("FMU: ready");
+
+	for (;;) {
+		/* wait for serial data, but no more than 100ms */
+		poll(&fds, 1, 100);
 
-	if (ret > 0) {
 		/*
 		 * Pull bytes from FMU and feed them to the HX engine.
 		 * Limit the number of bytes we actually process on any one iteration.
 		 */
-		if (pollfds[0].revents & POLLIN) {
+		if (fds.revents & POLLIN) {
 			char buf[32];
 			ssize_t count = read(fmu_fd, buf, sizeof(buf));
 			for (int i = 0; i < count; i++)
@@ -158,55 +115,38 @@ comms_check(void)
 		}
 
 		/*
-		 * Pull bytes from the serial RX port and feed them to the decoder
-		 * if we care about serial RX data.
+		 * Decide if it's time to send an update to the FMU.
 		 */
-		if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) {
-			switch (system_state.serial_rx_mode) {
-			case RX_MODE_DSM_10BIT:
-			case RX_MODE_DSM_11BIT:
-				dsm_input(rx_fd);
-				break;
-
-			case RX_MODE_FUTABA_SBUS:
-				sbus_input(rx_fd);
-				break;
-
-			default:
-				break;
+		static hrt_abstime last_report_time;
+		hrt_abstime now, delta;
+
+		/* should we send a report to the FMU? */
+		now = hrt_absolute_time();
+		delta = now - last_report_time;
+		if ((delta > FMU_MIN_REPORT_INTERVAL) && 
+		    (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
+
+			system_state.fmu_report_due = false;
+			last_report_time = now;
+
+			/* populate the report */
+			for (int i = 0; i < system_state.rc_channels; i++)
+				report.rc_channel[i] = system_state.rc_channel_data[i];
+
+			if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) {
+				report.channel_count = system_state.rc_channels;
+			} else {
+				report.channel_count = 0;
 			}
-		}
-	}
+			
+			report.armed = system_state.armed;
 
-	/*
-	 * Decide if it's time to send an update to the FMU.
-	 */
-	static hrt_abstime last_report_time;
-	hrt_abstime now, delta;
-
-	/* should we send a report to the FMU? */
-	now = hrt_absolute_time();
-	delta = now - last_report_time;
-	if ((delta > FMU_MIN_REPORT_INTERVAL) && 
-	    (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
-
-		system_state.fmu_report_due = false;
-		last_report_time = now;
-
-		/* populate the report */
-		for (int i = 0; i < system_state.rc_channels; i++)
-			report.rc_channel[i] = system_state.rc_channel_data[i];
-		report.channel_count = system_state.rc_channels;
-		report.armed = system_state.armed;
-
-		/* and send it */
-		hx_stream_send(stream, &report, sizeof(report));
+			/* and send it */
+			hx_stream_send(stream, &report, sizeof(report));
+		}
 	}
 }
 
-int frame_rx;
-int frame_bad;
-
 static void
 comms_handle_config(const void *buffer, size_t length)
 {
@@ -218,8 +158,6 @@ comms_handle_config(const void *buffer, size_t length)
 	}
 
 	frame_rx++;
-
-	serial_rx_init(cfg->serial_rx_mode);
 }
 
 static void
@@ -277,9 +215,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 			comms_handle_config(buffer, length);
 			break;
 		default:
+		    	frame_bad++;
 			break;
 		}
 	}
-    	frame_bad++;
 }
 
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
new file mode 100644
index 000000000..d4eace3df
--- /dev/null
+++ b/apps/px4io/controls.c
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   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 <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <debug.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <termios.h>
+#include <string.h>
+#include <poll.h>
+
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/hx_stream.h>
+#include <systemlib/perf_counter.h>
+
+#define DEBUG
+#include "px4io.h"
+
+void
+controls_main(void)
+{
+	struct pollfd fds[2];
+
+	fds[0].fd = dsm_init("/dev/ttyS0");
+	fds[0].events = POLLIN;
+
+
+	fds[1].fd = sbus_init("/dev/ttyS2");
+	fds[1].events = POLLIN;
+
+	for (;;) {
+		/* run this loop at ~100Hz */
+		poll(fds, 2, 10);
+
+		if (fds[0].revents & POLLIN)
+			dsm_input();
+		if (fds[1].revents & POLLIN)
+			sbus_input();
+
+		/* XXX do ppm processing, bypass mode, etc. here */
+
+		/* do PWM output updates */
+		mixer_tick();
+	}
+}
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 79b6301c7..744dac3d6 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -44,46 +44,69 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <string.h>
+#include <termios.h>
 
 #include <systemlib/ppm_decode.h>
  
 #include <drivers/drv_hrt.h>
 
+#define DEBUG
+
 #include "px4io.h"
 #include "protocol.h"
 
 #define DSM_FRAME_SIZE		16
 #define DSM_FRAME_CHANNELS	7
 
+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 bool	insync;
 static unsigned	channel_shift;
 
-static void dsm_decode(void);
+unsigned dsm_frame_drops;
 
-void
-dsm_init(unsigned mode)
+static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
+static void dsm_guess_format(bool reset);
+static void dsm_decode(hrt_abstime now);
+
+int
+dsm_init(const char *device)
 {
-	insync = false;
-	partial_frame_count = 0;
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY);
 
-	if (mode == RX_MODE_DSM_10BIT) {
-		channel_shift = 10;
+	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 {
-		channel_shift = 11;
+		debug("DSM: open failed");
 	}
 
-	last_frame_time = hrt_absolute_time();
+	return dsm_fd;
 }
 
 void
-dsm_input(int fd)
+dsm_input(void)
 {
-	uint8_t		buf[DSM_FRAME_SIZE];
 	ssize_t		ret;
 	hrt_abstime	now;
 
@@ -97,25 +120,33 @@ dsm_input(int fd)
 	 * 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_frame_time) > 5000)
-		partial_frame_count = 0;
+	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(fd, buf, DSM_FRAME_SIZE - partial_frame_count);
+	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;
+	last_rx_time = now;
 
 	/*
 	 * Add bytes to the current frame
 	 */
-	memcpy(&frame[partial_frame_count], buf, ret);
 	partial_frame_count += ret;
 
 	/*
@@ -123,30 +154,137 @@ dsm_input(int fd)
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
 	 	return;
-	last_frame_time = now;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
 	 * decode it.
 	 */
-	dsm_decode();
+	dsm_decode(now);
 	partial_frame_count = 0;
 }
 
+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_decode(void)
+dsm_guess_format(bool reset)
 {
-	uint16_t	data_mask = (1 << channel_shift) - 1;
+	static uint32_t	cs10;
+	static uint32_t	cs11;
+	static unsigned samples;
 
-	/*
-	 * The encoding of the first byte is uncertain, so we're going
-	 * to ignore it for now.
+	/* 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.
 	 *
-	 * The second byte may tell us about the protocol, but it's not
-	 * actually very interesting since what we really want to know
-	 * is how the channel data is formatted, and there doesn't seem
-	 * to be a reliable way to determine this from the protocol ID
-	 * alone.
+	 * 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) */
+		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: detected 11-bit format");
+		return;
+	}
+	if ((votes10 == 1) && (votes11 == 0)) {
+		channel_shift = 10;
+		debug("DSM: detected 10-bit format");
+		return;
+	}
+
+	/* call ourselves to reset our state ... we have to try again */
+	debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
+	dsm_guess_format(true);
+}
+
+static void
+dsm_decode(hrt_abstime frame_time)
+{
+
+/*
+	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);
+	last_frame_time = frame_time;
+	if (channel_shift == 0) {
+		dsm_guess_format(false);
+		system_state.dsm_input_ok = false;
+		return;
+	}
+
+	/*
+	 * 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
@@ -155,30 +293,49 @@ dsm_decode(void)
 	 * seven channels are being transmitted.
 	 */
 
+	const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS;
+
+	uint16_t dsm_channels[dsm_chancount];
+
 	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;
 
-		/* ignore pad channels */
-		if (raw == 0xffff)
+		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
 			continue;
 
-		unsigned channel = (raw >> channel_shift) & 0xf;
-
 		/* ignore channels out of range */
 		if (channel >= PX4IO_INPUT_CHANNELS)
 			continue;
 
+		/* update the decoded channel count */
 		if (channel > ppm_decoded_channels)
 			ppm_decoded_channels = channel;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		unsigned data = raw & data_mask;
 		if (channel_shift == 11)
-			data /= 2;
-		ppm_buffer[channel] = 988 + data;
+			value /= 2;
+
+		/* stuff the decoded channel into the PPM input buffer */
+		dsm_channels[channel] = 988 + value;
+	}
+
+	/* DSM input is valid */
+	system_state.dsm_input_ok = true;
+
+	/* check if no S.BUS data is available */
+	if (!system_state.sbus_input_ok) {
+
+		for (unsigned i = 0; i < dsm_chancount; i++) {
+			system_state.rc_channel_data[i] = dsm_channels[i];
+		}
 
+		/* and note that we have received data from the R/C controller */
+		/* XXX failsafe will cause problems here - need a strategy for detecting it */
+		system_state.rc_channels_timestamp = frame_time;
+		system_state.rc_channels = dsm_chancount;
+		system_state.fmu_report_due = true;
 	}
-	ppm_last_valid_decode = hrt_absolute_time();
 }
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 572344d00..483e9fe4d 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -49,7 +49,6 @@
 #include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_hrt.h>
 
 #include <systemlib/ppm_decode.h>
 
@@ -60,17 +59,6 @@
  */
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
-
-/*
- * HRT periodic call used to check for control input data.
- */
-static struct hrt_call mixer_input_call;
-
-/*
- * Mixer periodic tick.
- */
-static void	mixer_tick(void *arg);
-
 /*
  * Collect RC input data from the controller source(s).
  */
@@ -92,20 +80,8 @@ struct mixer {
 	/* XXX more config here */
 } mixers[IO_SERVO_COUNT];
 
-int
-mixer_init(void)
-{
-
-
-	/* look for control data at 50Hz */
-	hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
-
-	return 0;
-}
-
-
-static void
-mixer_tick(void *arg)
+void
+mixer_tick(void)
 {
 	uint16_t *control_values;
 	int control_count;
@@ -148,12 +124,11 @@ mixer_tick(void *arg)
 		/* we have no control input */
 		control_count = 0;
 	}
-
 	/*
 	 * Tickle each mixer, if we have control data.
 	 */
 	if (control_count > 0) {
-		for (i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
+		for (i = 0; i < IO_SERVO_COUNT; i++) {
 			mixer_update(i, control_values, control_count);
 
 			/*
@@ -195,17 +170,26 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
 static void
 mixer_get_rc_input(void)
 {
-
 	/* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */
 	if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) {
-		system_state.rc_channels = 0;
-		system_state.fmu_report_due = true;
+
+		/* input was ok and timed out, mark as update */
+		if (system_state.ppm_input_ok) {
+			system_state.ppm_input_ok = false;
+			system_state.fmu_report_due = true;
+		}
 		return;
 	}
 
-	/* otherwise, copy channel data */
-	system_state.rc_channels = ppm_decoded_channels;
-	for (unsigned i = 0; i < ppm_decoded_channels; i++)
-		system_state.rc_channel_data[i] = ppm_buffer[i];
-	system_state.fmu_report_due = true;
+	/* mark PPM as valid */
+	system_state.ppm_input_ok = true;
+
+	/* check if no DSM and S.BUS data is available */
+	if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) {
+		/* otherwise, copy channel data */
+		system_state.rc_channels = ppm_decoded_channels;
+		for (unsigned i = 0; i < ppm_decoded_channels; i++)
+			system_state.rc_channel_data[i] = ppm_buffer[i];
+		system_state.fmu_report_due = true;
+	}
 }
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 660eed12b..c704b1201 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -62,11 +62,7 @@ struct px4io_config {
 	uint16_t f2i_config_magic;
 #define F2I_CONFIG_MAGIC 0x6366
 
-	uint8_t		serial_rx_mode;
-#define RX_MODE_PPM_ONLY	0
-#define RX_MODE_DSM_10BIT	1
-#define RX_MODE_DSM_11BIT	2
-#define RX_MODE_FUTABA_SBUS	3
+	/* XXX currently nothing here */
 };
 
 /* report from IO to FMU */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index dec2cdde6..77524797f 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -55,37 +55,17 @@
 __EXPORT int user_start(int argc, char *argv[]);
 
 struct sys_state_s 	system_state;
-int			gpio_fd;
-
-static const char cursor[] = {'|', '/', '-', '\\'};
-
-static struct hrt_call timer_tick_call;
-volatile int timers[TIMER_NUM_TIMERS];
-static void timer_tick(void *arg);
 
 int user_start(int argc, char *argv[])
 {
-	int     cycle = 0;
-	bool	heartbeat = false;
-	bool	failsafe = false;
+	/* reset all to zero */
+	memset(&system_state, 0, sizeof(system_state));
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
-	/* init the FMU and receiver links */
-	comms_init();
-
-	/* configure the first 8 PWM outputs (i.e. all of them) */
-	/* note, must do this after comms init to steal back PA0, which is CTS otherwise */
-	up_pwm_servo_init(0xff);
-
 	/* print some startup info */
 	lib_lowprintf("\nPX4IO: starting\n");
-	struct mallinfo minfo = mallinfo();
-	lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
-
-	/* start the software timer service */
-	hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL);
 
 	/* default all the LEDs to off while we start */
 	LED_AMBER(false);
@@ -95,64 +75,23 @@ int user_start(int argc, char *argv[])
 	/* turn on servo power */
 	POWER_SERVO(true);
 
-	/* start the mixer */
-	mixer_init();
-
 	/* start the safety switch handler */
 	safety_init();
 
-	/* set up some timers for the main loop */
-	timers[TIMER_BLINK_BLUE] = 250;		/* heartbeat blink @ 2Hz */
-	timers[TIMER_STATUS_PRINT] = 1000;	/* print status message @ 1Hz */
-
-	/*
-	 * Main loop servicing communication with FMU
-	 */
-	while(true) {
-
-		/* check for communication from FMU, send updates */
-		comms_check();
-
-		/* blink the heartbeat LED */
-		if (timers[TIMER_BLINK_BLUE] == 0) {
-			timers[TIMER_BLINK_BLUE] = 250;
-			LED_BLUE(heartbeat = !heartbeat);
-		}
-
-		/* blink the failsafe LED if we don't have FMU input */
-		if (!system_state.mixer_use_fmu) {
-			if (timers[TIMER_BLINK_AMBER] == 0) {
-				timers[TIMER_BLINK_AMBER] = 125;
-				failsafe = !failsafe;
-			}
-		} else {
-			failsafe = false;
-		}
-		LED_AMBER(failsafe);
-		
-		/* print some simple status */
-		if (timers[TIMER_STATUS_PRINT] == 0) {
-			timers[TIMER_STATUS_PRINT] = 1000;
-			lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d   \r",
-				cursor[cycle++ & 3],
-				(system_state.arm_ok        ? "FMU_ARMED"  : "FMU_SAFE"),
-				(system_state.armed         ? "ARMED"  : "SAFE"),
-				(system_state.rc_channels   ? "RC OK"  : "NO RC"),
-				(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
-				system_state.rc_channels,
-				frame_rx, frame_bad
-			);
-		}
-	}
-
-	/* Should never reach here */
-	return ERROR;
-}
-
-static void
-timer_tick(void *arg)
-{
-	for (unsigned i = 0; i < TIMER_NUM_TIMERS; i++)
-		if (timers[i] > 0)
-			timers[i]--;
-}
+	/* configure the first 8 PWM outputs (i.e. all of them) */
+	up_pwm_servo_init(0xff);
+
+	/* start the flight control signal handler */
+	task_create("FCon", 
+		    SCHED_PRIORITY_DEFAULT,
+		    1024,
+		    (main_t)controls_main,
+		    NULL);
+
+
+	struct mallinfo minfo = mallinfo();
+	lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
+
+	/* we're done here, go run the communications loop */
+	comms_main();
+}
\ No newline at end of file
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index f50e29252..483b9bcc8 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -56,10 +56,11 @@
  * Debug logging
  */
 
-#if 1
-# define debug(fmt, ...)	lib_lowprintf(fmt "\n", ##args)
+#ifdef DEBUG
+# include <debug.h>
+# define debug(fmt, args...)	lib_lowprintf(fmt "\n", ##args)
 #else
-# define debug(fmt, ...)	do {} while(0)
+# define debug(fmt, args...)	do {} while(0)
 #endif
 
 /*
@@ -71,11 +72,16 @@ struct sys_state_s
 	bool		armed;		/* IO armed */
 	bool		arm_ok;		/* FMU says OK to arm */
 
+	bool		ppm_input_ok;	/* valid PPM input data */
+	bool		dsm_input_ok;	/* valid Spektrum DSM data */
+	bool		sbus_input_ok;	/* valid Futaba S.Bus data */
+
 	/*
 	 * Data from the remote control input(s)
 	 */
 	int		rc_channels;
 	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
+	uint64_t	rc_channels_timestamp;
 
 	/*
 	 * Control signals from FMU.
@@ -146,7 +152,7 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
 /*
  * Mixer
  */
-extern int	mixer_init(void);
+extern void	mixer_tick(void);
 
 /*
  * Safety switch/LED.
@@ -156,16 +162,16 @@ extern void	safety_init(void);
 /*
  * FMU communications
  */
-extern void	comms_init(void);
-extern void	comms_check(void);
+extern void	comms_main(void) __attribute__((noreturn));
 
 /*
- * Serial receiver decoders.
+ * R/C receiver handling.
  */
-extern void	dsm_init(unsigned mode);
-extern void	dsm_input(int fd);
-extern void	sbus_init(unsigned mode);
-extern void	sbus_input(int fd);
+extern void	controls_main(void);
+extern int	dsm_init(const char *device);
+extern void	dsm_input(void);
+extern int	sbus_init(const char *device);
+extern void	sbus_input(void);
 
 /*
  * Assertion codes
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 24fc9951a..d5bd103c1 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -51,6 +51,8 @@
 #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
@@ -63,13 +65,22 @@ static unsigned counter;
 
 static bool safety_led_state;
 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
@@ -117,3 +128,28 @@ safety_check_button(void *arg)
 	}
 	LED_SAFETY(safety_led_state);
 }
+
+
+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)
+{
+	static bool failsafe = false;
+
+	/* blink the failsafe LED if we don't have FMU input */
+	if (!system_state.mixer_use_fmu) {
+		failsafe = !failsafe;
+	} else {
+		failsafe = false;
+	}
+	LED_AMBER(failsafe);
+}
\ No newline at end of file
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index e363a0a78..c3949f2b0 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -41,18 +41,201 @@
 
 #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"
 
-void
-sbus_init(unsigned mode)
+#define SBUS_FRAME_SIZE		25
+#define SBUS_INPUT_CHANNELS	16
+
+static int sbus_fd = -1;
+
+static hrt_abstime last_rx_time;
+
+static uint8_t	frame[SBUS_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+
+unsigned sbus_frame_drops;
+
+static void sbus_decode(hrt_abstime frame_time);
+
+int
+sbus_init(const char *device)
 {
+	if (sbus_fd < 0)
+		sbus_fd = open(device, O_RDONLY);
+
+	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("Sbus: ready");
+	} else {
+		debug("Sbus: open failed");
+	}
+
+	return sbus_fd;
 }
 
 void
-sbus_input(int fd)
+sbus_input(void)
+{
+	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;
+	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;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	sbus_decode(now);
+	partial_frame_count = 0;
+}
+
+/*
+ * 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 void
+sbus_decode(hrt_abstime frame_time)
 {
+	/* check frame boundary markers to avoid out-of-sync cases */
+	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
+		sbus_frame_drops++;
+		system_state.sbus_input_ok = false;
+		return;
+	}
+
+	/* if the failsafe bit is set, we consider that a loss of RX signal */
+	if (frame[23] & (1 << 4)) {
+		system_state.sbus_input_ok = false;
+		return;
+	}
+
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : 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 */
+		system_state.rc_channel_data[channel] = (value / 2) + 998;
+	}
+
+	if (PX4IO_INPUT_CHANNELS >= 18) {
+		/* decode two switch channels */
+		chancount = 18;
+	}
+
+	system_state.rc_channels = chancount;
+	system_state.sbus_input_ok = true;
+	system_state.fmu_report_due = true;
+
+	/* and note that we have received data from the R/C controller */
+	system_state.rc_channels_timestamp = frame_time;	
 }