Merged DSM fixes

22 files changed, 783 insertions, 122 deletions

diff --git a/Debug/NuttX b/Debug/NuttX
index 825733554..8e6544842 100644
--- a/Debug/NuttX
+++ b/Debug/NuttX
@@ -168,17 +168,29 @@ define _showsemaphore
 	printf "\n"
 end
 
-define showtask
+#
+# Print information about a task's stack usage
+#
+define showtaskstack
 	set $task = (struct _TCB *)$arg0
 
-	printf "%p %.2d ", $task, $task->pid
-	_showtaskstate $task
-	printf " %s\n", $task->name
 	set $stack_free = 0
 	while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
 		set $stack_free = $stack_free + 1
 	end
 	printf"    stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
+end
+
+#
+# Print details of a task
+#
+define showtask
+	set $task = (struct _TCB *)$arg0
+
+	printf "%p %.2d ", $task, $task->pid
+	_showtaskstate $task
+	printf " %s\n", $task->name
+	showtaskstack $task
 
 	if $task->task_state == TSTATE_WAIT_SEM
 		printf "    waiting on %p ", $task->waitsem
diff --git a/apps/drivers/blinkm/Makefile b/apps/drivers/blinkm/Makefile
new file mode 100644
index 000000000..5a623693d
--- /dev/null
+++ b/apps/drivers/blinkm/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# BlinkM I2C LED driver
+#
+
+APPNAME		 = blinkm
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
new file mode 100644
index 000000000..d589025cc
--- /dev/null
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -0,0 +1,465 @@
+/****************************************************************************
+ *
+ *   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 blinkm.cpp
+ *
+ * Driver for the BlinkM LED controller connected via I2C.
+ */
+
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <drivers/drv_blinkm.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+class BlinkM : public device::I2C
+{
+public:
+	BlinkM(int bus);
+	~BlinkM();
+
+	virtual int		init();
+	virtual int		probe();
+
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	static const char	*script_names[];
+
+private:
+	enum ScriptID {
+		USER		= 0,
+		RGB,
+		WHITE_FLASH,
+		RED_FLASH,
+		GREEN_FLASH,
+		BLUE_FLASH,
+		CYAN_FLASH,
+		MAGENTA_FLASH,
+		YELLOW_FLASH,
+		BLACK,
+		HUE_CYCLE,
+		MOOD_LIGHT,
+		VIRTUAL_CANDLE,
+		WATER_REFLECTIONS,
+		OLD_NEON,
+		THE_SEASONS,
+		THUNDERSTORM,
+		STOP_LIGHT,
+		MORSE_CODE
+	};
+
+	work_s			_work;
+	static const unsigned	_monitor_interval = 250;
+
+	static void		monitor_trampoline(void *arg);
+	void			monitor();
+
+	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
+
+	int			fade_rgb(uint8_t r, uint8_t g, uint8_t b);
+	int			fade_hsb(uint8_t h, uint8_t s, uint8_t b);
+
+	int			fade_rgb_random(uint8_t r, uint8_t g, uint8_t b);
+	int			fade_hsb_random(uint8_t h, uint8_t s, uint8_t b);
+
+	int			set_fade_speed(uint8_t s);
+
+	int			play_script(uint8_t script_id);
+	int			play_script(const char *script_name);
+	int			stop_script();
+
+	int			write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3);
+	int			read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]);
+	int			set_script(uint8_t length, uint8_t repeats);
+
+	int			get_rgb(uint8_t &r, uint8_t &g, uint8_t &b);
+
+	int			get_firmware_version(uint8_t version[2]);
+};
+
+/* for now, we only support one BlinkM */
+namespace
+{
+BlinkM *g_blinkm;
+
+}
+
+/* list of script names, must match script ID numbers */
+const char *BlinkM::script_names[] = {
+	"USER",
+	"RGB",
+	"WHITE_FLASH",
+	"RED_FLASH",
+	"GREEN_FLASH",
+	"BLUE_FLASH",
+	"CYAN_FLASH",
+	"MAGENTA_FLASH",
+	"YELLOW_FLASH",
+	"BLACK",
+	"HUE_CYCLE",
+	"MOOD_LIGHT",
+	"VIRTUAL_CANDLE",
+	"WATER_REFLECTIONS",
+	"OLD_NEON",
+	"THE_SEASONS",
+	"THUNDERSTORM",
+	"STOP_LIGHT",
+	"MORSE_CODE",
+	nullptr
+};
+
+extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
+
+BlinkM::BlinkM(int bus) :
+	I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000)
+{
+}
+
+BlinkM::~BlinkM()
+{
+}
+
+int
+BlinkM::init()
+{
+	int ret;
+
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	/* set some sensible defaults */
+	set_fade_speed(25);
+
+	/* turn off by default */
+	play_script(BLACK);
+
+	/* start the system monitor as a low-priority workqueue entry */
+	work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1);
+
+	return OK;
+}
+
+int
+BlinkM::probe()
+{
+	uint8_t version[2];
+	int ret;
+
+	ret = get_firmware_version(version);
+
+	if (ret == OK)
+		log("found BlinkM firmware version %c%c", version[1], version[0]);
+
+	return ret;
+}
+
+int
+BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case BLINKM_PLAY_SCRIPT_NAMED:
+		if (arg == 0) {
+			ret = EINVAL;
+			break;
+		}
+		ret = play_script((const char *)arg);
+		break;
+
+	case BLINKM_PLAY_SCRIPT:
+		ret = play_script(arg);
+		break;
+
+	case BLINKM_SET_USER_SCRIPT: {
+		if (arg == 0) {
+			ret = EINVAL;
+			break;
+		}
+
+		unsigned lines = 0;
+		const uint8_t *script = (const uint8_t *)arg;
+
+		while ((lines < 50) && (script[1] != 0)) {
+			ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
+			if (ret != OK)
+				break;
+			script += 5;
+		}
+		if (ret == OK)
+			ret = set_script(lines, 0);
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+void
+BlinkM::monitor_trampoline(void *arg)
+{
+	BlinkM *bm = (BlinkM *)arg;
+
+	bm->monitor();
+}
+
+void
+BlinkM::monitor()
+{
+	/* check system state, possibly update LED to suit */
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval);
+}
+
+int
+BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'n', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'c', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b)
+{
+	const uint8_t msg[4] = { 'h', h, s, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[4] = { 'C', r, g, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b)
+{
+	const uint8_t msg[4] = { 'H', h, s, b };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::set_fade_speed(uint8_t s)
+{
+	const uint8_t msg[2] = { 'f', s };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::play_script(uint8_t script_id)
+{
+	const uint8_t msg[4] = { 'p', script_id, 0, 0 };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::play_script(const char *script_name)
+{
+	/* handle HTML colour encoding */
+	if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) {
+		char code[3];
+		uint8_t r, g, b;
+
+		code[2] = '\0';
+
+		code[0] = script_name[1];
+		code[1] = script_name[2];
+		r = strtol(code, 0, 16);
+		code[0] = script_name[3];
+		code[1] = script_name[4];
+		g = strtol(code, 0, 16);
+		code[0] = script_name[5];
+		code[1] = script_name[6];
+		b = strtol(code, 0, 16);
+
+		stop_script();
+		return set_rgb(r, g, b);
+	}
+
+	for (unsigned i = 0; script_names[i] != nullptr; i++)
+		if (!strcasecmp(script_name, script_names[i]))
+			return play_script(i);
+
+	return -1;
+}
+
+int
+BlinkM::stop_script()
+{
+	const uint8_t msg[1] = { 'o' };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3)
+{
+	const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4])
+{
+	const uint8_t msg[3] = { 'R', 0, line };
+	uint8_t result[5];
+
+	int ret = transfer(msg, sizeof(msg), result, sizeof(result));
+
+	if (ret == OK) {
+		ticks = result[0];
+		cmd[0] = result[1];
+		cmd[1] = result[2];
+		cmd[2] = result[3];
+		cmd[3] = result[4];
+	}
+
+	return ret;
+}
+
+int
+BlinkM::set_script(uint8_t len, uint8_t repeats)
+{
+	const uint8_t msg[4] = { 'L', 0, len, repeats };
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	const uint8_t msg = 'g';
+	uint8_t result[3];
+
+	int ret = transfer(&msg, sizeof(msg), result, sizeof(result));
+
+	if (ret == OK) {
+		r = result[0];
+		g = result[1];
+		b = result[2];
+	}
+
+	return ret;
+}
+
+
+int
+BlinkM::get_firmware_version(uint8_t version[2])
+{
+	const uint8_t msg = 'Z';
+
+	return transfer(&msg, sizeof(msg), version, sizeof(version));
+}
+
+int
+blinkm_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "start")) {
+		if (g_blinkm != nullptr)
+			errx(1, "already started");
+
+		g_blinkm = new BlinkM(3);
+
+		if (g_blinkm == nullptr)
+			errx(1, "new failed");
+
+		if (OK != g_blinkm->init()) {
+			delete g_blinkm;
+			g_blinkm = nullptr;
+			errx(1, "init failed");
+		}
+
+		exit(0);
+	}
+
+	if (g_blinkm == nullptr)
+		errx(1, "not started");
+
+	if (!strcmp(argv[1], "list")) {
+		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
+			fprintf(stderr, "    %s\n", BlinkM::script_names[i]);
+		fprintf(stderr, "    <html color number>\n");
+		exit(0);
+	}
+
+	/* things that require access to the device */
+	int fd = open(BLINKM_DEVICE_PATH, 0);
+	if (fd < 0)
+		err(1, "can't open BlinkM device");
+
+	if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
+		exit(0);
+
+	errx(1, "missing command, try 'start', 'list' or a script name");
+}
\ No newline at end of file
diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp
index 56112d767..474190d83 100644
--- a/apps/drivers/device/i2c.cpp
+++ b/apps/drivers/device/i2c.cpp
@@ -115,7 +115,7 @@ I2C::probe()
 }
 
 int
-I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
 {
 	struct i2c_msg_s msgv[2];
 	unsigned msgs;
@@ -130,7 +130,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
 		if (send_len > 0) {
 			msgv[msgs].addr = _address;
 			msgv[msgs].flags = 0;
-			msgv[msgs].buffer = send;
+			msgv[msgs].buffer = const_cast<uint8_t *>(send);
 			msgv[msgs].length = send_len;
 			msgs++;
 		}
diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h
index 3112e8e37..4d630b8a8 100644
--- a/apps/drivers/device/i2c.h
+++ b/apps/drivers/device/i2c.h
@@ -97,7 +97,7 @@ protected:
 	 * @return		OK if the transfer was successful, -errno
 	 *			otherwise.
 	 */
-	int		transfer(uint8_t *send, unsigned send_len,
+	int		transfer(const uint8_t *send, unsigned send_len,
 				 uint8_t *recv, unsigned recv_len);
 
 	/**
diff --git a/apps/drivers/drv_blinkm.h b/apps/drivers/drv_blinkm.h
new file mode 100644
index 000000000..9c278f6c5
--- /dev/null
+++ b/apps/drivers/drv_blinkm.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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 drv_blinkm.h
+ *
+ * BlinkM driver API
+ *
+ * This could probably become a more generalised API for multi-colour LED
+ * driver systems, or be merged with the generic LED driver.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define BLINKM_DEVICE_PATH	"/dev/blinkm"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _BLINKMIOCBASE		(0x2900)
+#define _BLINKMIOC(_n)		(_IOC(_BLINKMIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define BLINKM_PLAY_SCRIPT_NAMED	_BLINKMIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define BLINKM_PLAY_SCRIPT		_BLINKMIOC(2)
+
+/** 
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define BLINKM_SET_USER_SCRIPT		_BLINKMIOC(3)
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index efd396fa6..31ed89ecc 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -89,6 +89,8 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
+	bool			dump_one;
+
 private:
 	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
@@ -179,6 +181,7 @@ PX4IO	*g_dev;
 
 PX4IO::PX4IO() :
 	CDev("px4io", "/dev/px4io"),
+	dump_one(false),
 	_serial_fd(-1),
 	_io_stream(nullptr),
 	_task(-1),
@@ -494,6 +497,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 
 	_send_needed = true;
 
+	/* if monitoring, dump the received info */
+	if (dump_one) {
+		dump_one = false;
+
+		printf("IO: %s armed ", rep->armed ? "" : "not");
+		for (unsigned i = 0; i < rep->channel_count; i++)
+			printf("%d: %d ", i, rep->rc_channel[i]);
+		printf("\n");
+	}
+
 out:
 	unlock();
 }
@@ -681,6 +694,30 @@ test(void)
 	exit(0);
 }
 
+void
+monitor(void)
+{
+	unsigned cancels = 4;
+	printf("Hit <enter> three times to exit monitor mode\n");
+
+	for (;;) {
+		pollfd fds[1];
+
+		fds[0].fd = 0;
+		fds[0].events = POLLIN;
+		poll(fds, 1, 500);
+
+		if (fds[0].revents == POLLIN) {
+			int c;
+			read(0, &c, 1);
+			if (cancels-- == 0)
+				exit(0);
+		}
+
+		if (g_dev != nullptr)
+			g_dev->dump_one = true;
+	}
+}
 }
 
 int
@@ -756,8 +793,11 @@ px4io_main(int argc, char *argv[])
 	    !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();
+	if (!strcmp(argv[1], "monitor"))
+		monitor();
 
-	errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
+	errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'");
 }
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index 7ba4f52b0..3b0ee4565 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -128,7 +128,7 @@ int mtk_parse(uint8_t b,  char *gps_rx_buffer)
 				mtk_gps->eph = packet->hdop;
 				mtk_gps->epv = 65535; //unknown in mtk custom mode
 				mtk_gps->vel = packet->ground_speed;
-				mtk_gps->cog = 65535; //unknown in mtk custom mode
+				mtk_gps->cog = (uint16_t)packet->heading; //mtk: degrees *1e2, mavlink/ubx: degrees *1e2
 				mtk_gps->satellites_visible = packet->satellites;
 
 				/* convert time and date information to unix timestamp */
diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c
index 577a3a01c..1a50371c1 100644
--- a/apps/gps/nmea_helper.c
+++ b/apps/gps/nmea_helper.c
@@ -214,7 +214,7 @@ void *nmea_loop(void *args)
 			nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
 			nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling
 			nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100
-			nmea_gps->cog = 65535;
+			nmea_gps->cog = (uint16_t)info->direction*100; //nmea: degrees float, ubx/mavlink: degrees*1e2
 			nmea_gps->satellites_visible = (uint8_t)info->satinfo.inview;
 
 			int i = 0;
diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c
index d03a8d332..e2a10ec29 100644
--- a/apps/px4/tests/test_bson.c
+++ b/apps/px4/tests/test_bson.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/apps/px4/tests/tests_param.c b/apps/px4/tests/tests_param.c
index 88eff30f1..13f17bc43 100644
--- a/apps/px4/tests/tests_param.c
+++ b/apps/px4/tests/tests_param.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index f4eddbdd3..5033aeb98 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -100,8 +100,8 @@ comms_main(void)
 	debug("FMU: ready");
 
 	for (;;) {
-		/* wait for serial data, but no more than 100ms */
-		poll(&fds, 1, 100);
+		/* wait for serial data, but no more than 10ms */
+		poll(&fds, 1, 10);
 
 		/*
 		 * Pull bytes from FMU and feed them to the HX engine.
@@ -130,15 +130,9 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (int i = 0; i < system_state.rc_channels; i++)
+			for (unsigned 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.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
 			/* and send it */
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 0e0210373..e64f8b204 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -55,6 +55,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/ppm_decode.h>
 
 #define DEBUG
 #include "px4io.h"
@@ -63,15 +64,18 @@
 #define RC_CHANNEL_HIGH_THRESH		1600
 #define RC_CHANNEL_LOW_THRESH		1400
 
+static void	ppm_input(void);
+
 void
 controls_main(void)
 {
 	struct pollfd fds[2];
 
+	/* DSM input */
 	fds[0].fd = dsm_init("/dev/ttyS0");
 	fds[0].events = POLLIN;
 
-
+	/* S.bus input */
 	fds[1].fd = sbus_init("/dev/ttyS2");
 	fds[1].events = POLLIN;
 
@@ -79,10 +83,32 @@ controls_main(void)
 		/* run this loop at ~100Hz */
 		poll(fds, 2, 10);
 
+		/*
+		 * 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.
+		 */
+		bool locked = false;
+
 		if (fds[0].revents & POLLIN)
-			dsm_input();
+			locked |= dsm_input();
 		if (fds[1].revents & POLLIN)
-			sbus_input();
+			locked |= sbus_input();
+
+		/*
+		 * If we don't have lock from one of the serial receivers,
+		 * look for PPM. It shares an input with S.bus, so there's
+		 * a possibility it will mis-parse an S.bus frame.
+		 *
+		 * XXX each S.bus frame will cause a PPM decoder interrupt
+		 * storm (lots of edges).  It might be sensible to actually
+		 * disable the PPM decoder completely if we have an alternate
+		 * receiver lock.
+		 */
+		if (!locked)
+			ppm_input();
 
 		/* force manual input override */
 		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
@@ -92,16 +118,48 @@ controls_main(void)
 			system_state.mixer_use_fmu = true;
 		}
 
-		/* detect rc loss event */
-		if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) {
-			system_state.rc_lost = true;
-		}
+		/*
+		 * If we haven't seen any new control data in 200ms, assume we
+		 * have lost input and tell FMU.
+		 */
+		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
 
-		/* XXX detect fmu loss event */
+			/* set the number of channels to zero - no inputs */
+			system_state.rc_channels = 0;
+			system_state.rc_lost = true;
 
-		/* XXX handle failsave events - RC loss and FMU loss - here */
+			/* trigger an immediate report to the FMU */
+			system_state.fmu_report_due = true;
+		}
 
 		/* do PWM output updates */
 		mixer_tick();
 	}
 }
+
+static void
+ppm_input(void)
+{
+	/*
+	 * Look for new PPM input.
+	 */
+	if (ppm_last_valid_decode != 0) {
+
+		/* avoid racing with PPM updates */
+		irqstate_t state = irqsave();
+
+		/* PPM data exists, copy it */
+		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];
+
+		/* copy the timestamp and clear it */
+		system_state.rc_channels_timestamp = ppm_last_valid_decode;
+		ppm_last_valid_decode = 0;
+
+		irqrestore(state);
+
+		/* trigger an immediate report to the FMU */
+		system_state.fmu_report_due = true;
+	}	
+}
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 744dac3d6..2611f3a03 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -104,7 +104,7 @@ dsm_init(const char *device)
 	return dsm_fd;
 }
 
-void
+bool
 dsm_input(void)
 {
 	ssize_t		ret;
@@ -141,7 +141,7 @@ dsm_input(void)
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
-		return;
+		goto out;
 	last_rx_time = now;
 
 	/*
@@ -153,7 +153,7 @@ dsm_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
-	 	return;
+	 	goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -161,6 +161,12 @@ dsm_input(void)
 	 */
 	dsm_decode(now);
 	partial_frame_count = 0;
+
+out:
+	/*
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 */
+	return (now - last_frame_time) < 200000;
 }
 
 static bool
@@ -275,10 +281,13 @@ dsm_decode(hrt_abstime frame_time)
 	 */
 	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);
-		system_state.dsm_input_ok = false;
 		return;
 	}
 
@@ -293,10 +302,6 @@ dsm_decode(hrt_abstime frame_time)
 	 * 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)];
@@ -311,31 +316,40 @@ dsm_decode(hrt_abstime frame_time)
 			continue;
 
 		/* update the decoded channel count */
-		if (channel > ppm_decoded_channels)
-			ppm_decoded_channels = channel;
+		if (channel >= system_state.rc_channels)
+			system_state.rc_channels = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		if (channel_shift == 11)
 			value /= 2;
-
-		/* stuff the decoded channel into the PPM input buffer */
-		dsm_channels[channel] = 988 + value;
+		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;
+		}
+		system_state.rc_channel_data[channel] = 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) {
+	/* 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;
 
-		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;
-	}
+	/* trigger an immediate report to the FMU */
+	system_state.fmu_report_due = true;
 }
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index d9626c1e3..4fa3e1d5d 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -50,8 +50,6 @@
 
 #include <drivers/drv_pwm_output.h>
 
-#include <systemlib/ppm_decode.h>
-
 #include "px4io.h"
 
 /*
@@ -59,10 +57,6 @@
  */
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
-/*
- * Collect RC input data from the controller source(s).
- */
-static void	mixer_get_rc_input(void);
 
 /*
  * Update a mixer based on the current control signals.
@@ -89,12 +83,6 @@ mixer_tick(void)
 	bool should_arm;
 
 	/*
-	 * Start by looking for R/C control inputs.
-	 * This updates system_state with any control inputs received.
-	 */
-	mixer_get_rc_input();
-
-	/*
 	 * Decide which set of inputs we're using.
 	 */
 	if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
@@ -122,8 +110,10 @@ mixer_tick(void)
 
 	} else {
 		/* we have no control input */
+		/* XXX builtin failsafe would activate here */
 		control_count = 0;
 	}
+
 	/*
 	 * Tickle each mixer, if we have control data.
 	 */
@@ -167,30 +157,3 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
 		mixers[mixer].current_value = 0;
 	}
 }
-
-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) {
-
-		/* 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;
-	}
-
-	/* 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/px4io.c b/apps/px4io/px4io.c
index 77524797f..a3ac9e3e7 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -44,6 +44,7 @@
 #include <debug.h>
 #include <stdlib.h>
 #include <errno.h>
+#include <string.h>
 
 #include <nuttx/clock.h>
 
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 9bfe3b1e4..e0b7cdda1 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -69,17 +69,13 @@
 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 */
+	bool		armed;			/* IO armed */
+	bool		arm_ok;			/* FMU says OK to arm */
 
 	/**
 	 * Data from the remote control input(s)
 	 */
-	int		rc_channels;
+	unsigned	rc_channels;
 	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
 	uint64_t	rc_channels_timestamp;
 
@@ -184,9 +180,9 @@ extern void	comms_main(void) __attribute__((noreturn));
  */
 extern void	controls_main(void);
 extern int	dsm_init(const char *device);
-extern void	dsm_input(void);
+extern bool	dsm_input(void);
 extern int	sbus_init(const char *device);
-extern void	sbus_input(void);
+extern bool	sbus_input(void);
 
 /*
  * Assertion codes
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index c3949f2b0..a8f628a84 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -58,6 +58,7 @@
 static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
 
@@ -94,7 +95,7 @@ sbus_init(const char *device)
 	return sbus_fd;
 }
 
-void
+bool
 sbus_input(void)
 {
 	ssize_t		ret;
@@ -131,7 +132,7 @@ sbus_input(void)
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
-		return;
+		goto out;
 	last_rx_time = now;
 
 	/*
@@ -143,7 +144,7 @@ sbus_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < SBUS_FRAME_SIZE)
-	 	return;
+	 	goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -151,6 +152,12 @@ sbus_input(void)
 	 */
 	sbus_decode(now);
 	partial_frame_count = 0;
+
+out:
+	/*
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 */
+	return (now - last_frame_time) < 200000;
 }
 
 /*
@@ -195,17 +202,19 @@ 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 the failsafe bit is set, we consider the frame invalid */
 	if (frame[23] & (1 << 4)) {
-		system_state.sbus_input_ok = false;
 		return;
 	}
 
-	unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS;
+	/* 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++) {
@@ -228,14 +237,16 @@ sbus_decode(hrt_abstime frame_time)
 	}
 
 	if (PX4IO_INPUT_CHANNELS >= 18) {
-		/* decode two switch channels */
 		chancount = 18;
+		/* XXX decode the two switch channels */
 	}
 
+	/* note the number of channels decoded */
 	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;	
+
+	/* trigger an immediate report to the FMU */
+	system_state.fmu_report_due = true;
 }
diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c
index 745f76852..45715b791 100644
--- a/apps/systemcmds/bl_update/bl_update.c
+++ b/apps/systemcmds/bl_update/bl_update.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c
index 2328ebdb2..fb8b07ef4 100644
--- a/apps/systemcmds/boardinfo/boardinfo.c
+++ b/apps/systemcmds/boardinfo/boardinfo.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/apps/systemcmds/perf/perf.c b/apps/systemcmds/perf/perf.c
index 72406e9c7..ced942fd6 100644
--- a/apps/systemcmds/perf/perf.c
+++ b/apps/systemcmds/perf/perf.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 Lorenz Meier. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -13,7 +12,7 @@
  *    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
+ * 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.
  *
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index ca9f3f4ee..5e2a32a46 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -97,6 +97,7 @@ CONFIGURED_APPS += drivers/l3gd20
 CONFIGURED_APPS	+= drivers/px4io
 CONFIGURED_APPS += drivers/stm32
 CONFIGURED_APPS += drivers/led
+CONFIGURED_APPS += drivers/blinkm
 CONFIGURED_APPS += drivers/stm32/tone_alarm
 CONFIGURED_APPS += drivers/px4fmu
 CONFIGURED_APPS += drivers/hil