Merge remote-tracking branch 'upstream/master' into new_state_machine

Conflicts:
	src/drivers/px4io/px4io.cpp
	src/modules/commander/commander.c
	src/modules/commander/state_machine_helper.c

95 files changed, 27840 insertions, 0 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
new file mode 100644
index 000000000..264041e65
--- /dev/null
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -0,0 +1,400 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ardrone_interface.c
+ * Implementation of AR.Drone 1.0 / 2.0 motor control interface.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <math.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <systemlib/err.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+
+#include <systemlib/systemlib.h>
+
+#include "ardrone_motor_control.h"
+
+__EXPORT int ardrone_interface_main(int argc, char *argv[]);
+
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int ardrone_interface_task;		/**< Handle of deamon task / thread */
+static int ardrone_write;			/**< UART to write AR.Drone commands to */
+
+/**
+ * Mainloop of ardrone_interface.
+ */
+int ardrone_interface_thread_main(int argc, char *argv[]);
+
+/**
+ * Open the UART connected to the motor controllers
+ */
+static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int ardrone_interface_main(int argc, char *argv[])
+{
+		if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("ardrone_interface already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		ardrone_interface_task = task_spawn("ardrone_interface",
+						    SCHED_DEFAULT,
+						    SCHED_PRIORITY_MAX - 15,
+						    2048,
+						    ardrone_interface_thread_main,
+						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tardrone_interface is running\n");
+		} else {
+			printf("\tardrone_interface not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original)
+{
+	/* baud rate */
+	int speed = B115200;
+	int uart;
+
+	/* open uart */
+	uart = open(uart_name, O_RDWR | O_NOCTTY);
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+		fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+		close(uart);
+		return -1;
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		close(uart);
+		return -1;
+	}
+
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		close(uart);
+		return -1;
+	}
+
+	return uart;
+}
+
+int ardrone_interface_thread_main(int argc, char *argv[])
+{
+	thread_running = true;
+
+	char *device = "/dev/ttyS1";
+
+	/* welcome user */
+	printf("[ardrone_interface] Control started, taking over motors\n");
+
+	/* File descriptors */
+	int gpios;
+
+	char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
+
+	bool motor_test_mode = false;
+	int test_motor = -1;
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
+			motor_test_mode = true;
+		}
+
+		if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
+			if (i+1 < argc) {
+				int motor = atoi(argv[i+1]);
+				if (motor > 0 && motor < 5) {
+					test_motor = motor;
+				} else {
+					thread_running = false;
+					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
+				}
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
+	}
+
+	struct termios uart_config_original;
+
+	if (motor_test_mode) {
+		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
+	}
+
+	/* Led animation */
+	int counter = 0;
+	int led_counter = 0;
+
+	/* declare and safely initialize all structs */
+	struct vehicle_status_s state;
+	memset(&state, 0, sizeof(state));
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));
+	struct actuator_armed_s armed;
+	armed.armed = false;
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	printf("[ardrone_interface] Motors initialized - ready.\n");
+	fflush(stdout);
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	ardrone_write = ardrone_open_uart(device, &uart_config_original);
+
+	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
+	gpios = ar_multiplexing_init();
+
+	if (ardrone_write < 0) {
+		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	/* initialize motors */
+	if (OK != ar_init_motors(ardrone_write, gpios)) {
+		close(ardrone_write);
+		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
+
+
+	// XXX Re-done initialization to make sure it is accepted by the motors
+	// XXX should be removed after more testing, but no harm
+
+	/* close uarts */
+	close(ardrone_write);
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	ardrone_write = ardrone_open_uart(device, &uart_config_original);
+
+	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
+	gpios = ar_multiplexing_init();
+
+	if (ardrone_write < 0) {
+		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	/* initialize motors */
+	if (OK != ar_init_motors(ardrone_write, gpios)) {
+		close(ardrone_write);
+		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	while (!thread_should_exit) {
+
+		if (motor_test_mode) {
+			/* set motors to idle speed */
+			if (test_motor > 0 && test_motor < 5) {
+				int motors[4] = {0, 0, 0, 0};
+				motors[test_motor - 1] = 10;
+				ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
+			} else {
+				ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
+			}
+
+		} else {
+			/* MAIN OPERATION MODE */
+
+			/* get a local copy of the vehicle state */
+			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+			/* get a local copy of the actuator controls */
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
+			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+			
+			/* for now only spin if armed and immediately shut down
+			 * if in failsafe
+			 */
+			//XXX fix this
+			//if (armed.armed && !armed.lockdown) {
+			if (state.flag_fmu_armed) {
+				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
+
+			} else {
+				/* Silently lock down motor speeds to zero */
+				ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
+			}
+		}
+
+		if (counter % 24 == 0) {
+			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
+
+			if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
+
+			if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
+
+			if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
+
+			if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
+
+			if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
+
+			if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
+
+			if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
+
+			led_counter++;
+
+			if (led_counter == 12) led_counter = 0;
+		}
+
+		/* run at approximately 200 Hz */
+		usleep(4500);
+
+		counter++;
+	}
+
+	/* restore old UART config */
+	int termios_state;
+
+	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
+		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
+	}
+
+	printf("[ardrone_interface] Restored original UART config, exiting..\n");
+
+	/* close uarts */
+	close(ardrone_write);
+	ar_multiplexing_deinit(gpios);
+
+	fflush(stdout);
+
+	thread_running = false;
+
+	return OK;
+}
+
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
new file mode 100644
index 000000000..ecd31a073
--- /dev/null
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -0,0 +1,492 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ardrone_motor_control.c
+ * Implementation of AR.Drone 1.0 / 2.0 motor control interface
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <systemlib/err.h>
+
+#include "ardrone_motor_control.h"
+
+static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
+static unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 };
+
+typedef union {
+	uint16_t motor_value;
+	uint8_t bytes[2];
+} motor_union_t;
+
+#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */
+
+/**
+ * @brief Generate the 8-byte motor set packet
+ *
+ * @return the number of bytes (8)
+ */
+void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
+{
+	motor_buf[0] = 0x20;
+	motor_buf[1] = 0x00;
+	motor_buf[2] = 0x00;
+	motor_buf[3] = 0x00;
+	motor_buf[4] = 0x00;
+	/*
+	 * {0x20, 0x00, 0x00, 0x00, 0x00};
+	 * 0x20 is start sign / motor command
+	 */
+	motor_union_t curr_motor;
+	uint16_t nineBitMask = 0x1FF;
+
+	/* Set motor 1 */
+	curr_motor.motor_value = (motor1 & nineBitMask) << 4;
+	motor_buf[0] |= curr_motor.bytes[1];
+	motor_buf[1] |= curr_motor.bytes[0];
+
+	/* Set motor 2 */
+	curr_motor.motor_value = (motor2 & nineBitMask) << 3;
+	motor_buf[1] |= curr_motor.bytes[1];
+	motor_buf[2] |= curr_motor.bytes[0];
+
+	/* Set motor 3 */
+	curr_motor.motor_value = (motor3 & nineBitMask) << 2;
+	motor_buf[2] |= curr_motor.bytes[1];
+	motor_buf[3] |= curr_motor.bytes[0];
+
+	/* Set motor 4 */
+	curr_motor.motor_value = (motor4 & nineBitMask) << 1;
+	motor_buf[3] |= curr_motor.bytes[1];
+	motor_buf[4] |= curr_motor.bytes[0];
+}
+
+void ar_enable_broadcast(int fd)
+{
+	ar_select_motor(fd, 0);
+}
+
+int ar_multiplexing_init()
+{
+	int		fd;
+	
+	fd = open(GPIO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("GPIO: open fail");
+		return fd;
+	}
+
+	/* deactivate all outputs */
+	if (ioctl(fd, GPIO_SET, motor_gpios)) {
+		warn("GPIO: clearing pins fail");
+		close(fd);
+		return -1;
+	}
+
+	/* configure all motor select GPIOs as outputs */
+	if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
+		warn("GPIO: output set fail");
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+int ar_multiplexing_deinit(int fd)
+{
+	if (fd < 0) {
+		printf("GPIO: no valid descriptor\n");
+		return fd;
+	}
+
+	int ret = 0;
+
+	/* deselect motor 1-4 */
+	ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+	if (ret != 0) {
+		printf("GPIO: clear failed %d times\n", ret);
+	}
+
+	if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) {
+		printf("GPIO: input set fail\n");
+		return -1;
+	}
+
+	close(fd);
+
+	return ret;
+}
+
+int ar_select_motor(int fd, uint8_t motor)
+{
+	int ret = 0;
+	/*
+	 *  Four GPIOS:
+	 *		GPIO_EXT1
+	 *		GPIO_EXT2
+	 *		GPIO_UART2_CTS
+	 *		GPIO_UART2_RTS
+	 */
+
+	/* select motor 0 to enable broadcast */
+	if (motor == 0) {
+		/* select motor 1-4 */
+		ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
+
+	} else {
+		/* select reqested motor */	
+		ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
+	}
+
+	return ret;
+}
+
+int ar_deselect_motor(int fd, uint8_t motor)
+{
+	int ret = 0;
+	/*
+	 *  Four GPIOS:
+	 *		GPIO_EXT1
+	 *		GPIO_EXT2
+	 *		GPIO_UART2_CTS
+	 *		GPIO_UART2_RTS
+	 */
+
+	if (motor == 0) {
+		/* deselect motor 1-4 */
+		ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+	} else {
+		/* deselect reqested motor */	
+		ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]);
+	}
+
+	return ret;
+}
+
+int ar_init_motors(int ardrone_uart, int gpios)
+{
+	/* Write ARDrone commands on UART2 */
+	uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
+	uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
+
+	/* deselect all motors */
+	ar_deselect_motor(gpios, 0);
+
+	/* initialize all motors
+	 * - select one motor at a time
+	 * - configure motor
+	 */
+	int i;
+	int errcounter = 0;
+
+
+	/* initial setup run */
+	for (i = 1; i < 5; ++i) {
+		/* Initialize motors 1-4 */
+		errcounter += ar_select_motor(gpios, i);
+		usleep(200);
+
+		/*
+		 * write 0xE0 - request status
+		 * receive one status byte
+		 */
+		write(ardrone_uart, &(initbuf[0]), 1);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*1);
+
+		/*
+		 * write 0x91 - request checksum
+		 * receive 120 status bytes
+		 */
+		write(ardrone_uart, &(initbuf[1]), 1);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*120);
+
+		/*
+		 * write 0xA1 - set status OK
+		 * receive one status byte - should be A0
+		 * to confirm status is OK
+		 */
+		write(ardrone_uart, &(initbuf[2]), 1);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*1);
+
+		/*
+		 * set as motor i, where i = 1..4
+		 * receive nothing
+		 */
+		initbuf[3] = (uint8_t)i;
+		write(ardrone_uart, &(initbuf[3]), 1);
+		fsync(ardrone_uart);
+
+		/*
+		 * write 0x40 - check version
+		 * receive 11 bytes encoding the version
+		 */
+		write(ardrone_uart, &(initbuf[4]), 1);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*11);
+
+		ar_deselect_motor(gpios, i);
+		/* sleep 200 ms */
+		usleep(200000);
+	}
+
+	/* start the multicast part */
+	errcounter += ar_select_motor(gpios, 0);
+	usleep(200);
+
+	/*
+	 * first round
+	 * write six times A0 - enable broadcast
+	 * receive nothing
+	 */
+	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
+	fsync(ardrone_uart);
+	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
+
+	/*
+	 * second round
+	 * write six times A0 - enable broadcast
+	 * receive nothing
+	 */
+	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
+	fsync(ardrone_uart);
+	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
+
+	/* set motors to zero speed (fsync is part of the write command */
+	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
+
+	if (errcounter != 0) {
+		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
+		fflush(stdout);
+	}
+	return errcounter;
+}
+
+/**
+ * Sets the leds on the motor controllers, 1 turns led on, 0 off.
+ */
+void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
+{
+	/*
+	 * 2 bytes are sent. The first 3 bits describe the command: 011 means led control
+	 * the following 4 bits are the red leds for motor 4, 3, 2, 1
+	 * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1
+	 * the last bit is unknown.
+	 *
+	 * The packet is therefore:
+	 * 011 rrrr 0000 gggg 0
+	 */
+	uint8_t leds[2];
+	leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
+	leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
+	write(ardrone_uart, leds, 2);
+}
+
+int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
+	const unsigned int min_motor_interval = 4900;
+	static uint64_t last_motor_time = 0;
+
+	static struct actuator_outputs_s outputs;
+	outputs.timestamp = hrt_absolute_time();
+	outputs.output[0] = motor1;
+	outputs.output[1] = motor2;
+	outputs.output[2] = motor3;
+	outputs.output[3] = motor4;
+	static orb_advert_t pub = 0;
+	if (pub == 0) {
+		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+	}
+
+	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
+		uint8_t buf[5] = {0};
+		ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
+		int ret;
+		ret = write(ardrone_fd, buf, sizeof(buf));
+		fsync(ardrone_fd);
+
+		/* publish just written values */
+		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
+
+		if (ret == sizeof(buf)) {
+			return OK;
+		} else {
+			return ret;
+		}
+	} else {
+		return -ERROR;
+	}
+}
+
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
+
+	float roll_control = actuators->control[0];
+	float pitch_control = actuators->control[1];
+	float yaw_control = actuators->control[2];
+	float motor_thrust = actuators->control[3];
+
+	//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
+
+	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
+	const float max_thrust = 1.0f;			/**< 100% max thrust */
+	const float scaling = 500.0f;			/**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
+	const float min_gas = min_thrust * scaling;	/**< value range sent to motors, minimum */
+	const float max_gas = max_thrust * scaling;	/**< value range sent to motors, maximum */
+
+	/* initialize all fields to zero */
+	uint16_t motor_pwm[4] = {0};
+	float motor_calc[4] = {0};
+
+	float output_band = 0.0f;
+	float band_factor = 0.75f;
+	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
+	float yaw_factor = 1.0f;
+
+	static bool initialized = false;
+	/* publish effective outputs */
+	static struct actuator_controls_effective_s actuator_controls_effective;
+	static orb_advert_t actuator_controls_effective_pub;
+
+	if (motor_thrust <= min_thrust) {
+		motor_thrust = min_thrust;
+		output_band = 0.0f;
+	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
+		output_band = band_factor * (motor_thrust - min_thrust);
+	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
+		output_band = band_factor * startpoint_full_control;
+	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
+		output_band = band_factor * (max_thrust - motor_thrust);
+	}
+
+	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
+
+	// FRONT (MOTOR 1)
+	motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
+
+	// RIGHT (MOTOR 2)
+	motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
+
+	// BACK (MOTOR 3)
+	motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
+
+	// LEFT (MOTOR 4)
+	motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
+
+	// if we are not in the output band
+	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
+	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
+	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
+	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
+
+		yaw_factor = 0.5f;
+		yaw_control *= yaw_factor;
+		// FRONT (MOTOR 1)
+		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
+
+		// RIGHT (MOTOR 2)
+		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
+
+		// BACK (MOTOR 3)
+		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
+
+		// LEFT (MOTOR 4)
+		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
+	}
+
+	for (int i = 0; i < 4; i++) {
+		//check for limits
+		if (motor_calc[i] < motor_thrust - output_band) {
+			motor_calc[i] = motor_thrust - output_band;
+		}
+
+		if (motor_calc[i] > motor_thrust + output_band) {
+			motor_calc[i] = motor_thrust + output_band;
+		}
+	}
+
+	/* publish effective outputs */
+	actuator_controls_effective.control_effective[0] = roll_control;
+	actuator_controls_effective.control_effective[1] = pitch_control;
+	/* yaw output after limiting */
+	actuator_controls_effective.control_effective[2] = yaw_control;
+	/* possible motor thrust limiting */
+	actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
+
+	if (!initialized) {
+		/* advertise and publish */
+		actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
+		initialized = true;
+	} else {
+		/* already initialized, just publishing */
+		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
+	}
+
+	/* set the motor values */
+
+	/* scale up from 0..1 to 10..512) */
+	motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
+	motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
+	motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
+	motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
+
+	/* Keep motors spinning while armed and prevent overflows */
+
+	/* Failsafe logic - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
+	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
+	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
+	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
+
+	/* Failsafe logic - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
+	motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
+	motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
+	motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
+
+	/* send motors via UART */
+	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
+}
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h
new file mode 100644
index 000000000..78b603b63
--- /dev/null
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 ardrone_motor_control.h
+ * Definition of AR.Drone 1.0 / 2.0 motor control interface
+ */
+
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+
+/**
+ * Generate the 5-byte motor set packet.
+ *
+ * @return the number of bytes (5)
+ */
+void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
+
+/**
+ * Select a motor in the multiplexing.
+ *
+ * @param fd GPIO file descriptor
+ * @param motor Motor number, from 1 to 4, 0 selects all
+ */
+int ar_select_motor(int fd, uint8_t motor);
+
+/**
+ * Deselect a motor in the multiplexing.
+ *
+ * @param fd GPIO file descriptor
+ * @param motor Motor number, from 1 to 4, 0 deselects all
+ */
+int ar_deselect_motor(int fd, uint8_t motor);
+
+void ar_enable_broadcast(int fd);
+
+int ar_multiplexing_init(void);
+int ar_multiplexing_deinit(int fd);
+
+/**
+ * Write four motor commands to an already initialized port.
+ *
+ * Writing 0 stops a motor, values from 1-512 encode the full thrust range.
+ * on some motor controller firmware revisions a minimum value of 10 is
+ * required to spin the motors.
+ */
+int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
+
+/**
+ * Initialize the motors.
+ */
+int ar_init_motors(int ardrone_uart, int gpio);
+
+/**
+ * Set LED pattern.
+ */
+void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
+
+/**
+ * Mix motors and output actuators
+ */
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk
new file mode 100644
index 000000000..058bd1397
--- /dev/null
+++ b/src/drivers/ardrone_interface/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (C) 2012-2013 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.
+#
+############################################################################
+
+#
+# AR.Drone motor driver
+#
+
+MODULE_COMMAND	 = ardrone_interface
+SRCS		 = ardrone_interface.c \
+			   ardrone_motor_control.c
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
new file mode 100644
index 000000000..7a64b72a4
--- /dev/null
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -0,0 +1,920 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
+ * blinkm start
+ *
+ * To start the system monitor put in the next line after the blinkm start:
+ * blinkm systemmonitor
+ *
+ *
+ * Description:
+ * After startup, the Application checked how many lipo cells are connected to the System.
+ * The recognized number off cells, will be blinked 5 times in purple color.
+ * 2 Cells = 2 blinks
+ * ...
+ * 5 Cells = 5 blinks
+ * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
+ *
+ * System disarmed:
+ * The BlinkM should lit solid red.
+ *
+ * System armed:
+ * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
+ *
+ * X-X-X-X-_-_-_-_-_-_-
+ * -------------------------
+ * G G G M
+ * P P P O
+ * S S S D
+ *       E
+ *
+ * (X = on, _=off)
+ *
+ * The first 3 blinks indicates the status of the GPS-Signal (red):
+ * 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
+ *   5 satellites = X-X-_-X-_-_-_-_-_-_-
+ *   6 satellites = X-_-_-X-_-_-_-_-_-_-
+ * >=7 satellites = _-_-_-X-_-_-_-_-_-_-
+ * If no GPS is found the first 3 blinks are white
+ *
+ * The fourth Blink indicates the Flightmode:
+ * MANUAL     : amber
+ * STABILIZED : yellow
+ * HOLD		  : blue
+ * AUTO       : green
+ *
+ * Battery Warning (low Battery Level):
+ * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X
+ *
+ * Battery Alert (critical Battery Level)
+ * Continuously blinking in red X-X-X-X-X-X-X-X-X-X
+ *
+ * General Error (no uOrb Data)
+ * Continuously blinking in white X-X-X-X-X-X-X-X-X-X
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <arch/board/board.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>
+
+#include <systemlib/systemlib.h>
+#include <poll.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+static const float MAX_CELL_VOLTAGE	= 4.3f;
+static const int LED_ONTIME = 120;
+static const int LED_OFFTIME = 120;
+static const int LED_BLINK = 1;
+static const int LED_NOBLINK = 0;
+
+class BlinkM : public device::I2C
+{
+public:
+	BlinkM(int bus, int blinkm);
+	virtual ~BlinkM();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		setMode(int mode);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	static const char	*const 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
+	};
+
+	enum ledColors {
+		LED_OFF,
+		LED_RED,
+		LED_YELLOW,
+		LED_PURPLE,
+		LED_GREEN,
+		LED_BLUE,
+		LED_WHITE,
+		LED_AMBER
+	};
+
+	work_s			_work;
+
+	int led_color_1;
+	int led_color_2;
+	int led_color_3;
+	int led_color_4;
+	int led_color_5;
+	int led_color_6;
+	int led_color_7;
+	int led_color_8;
+	int led_blink;
+
+	bool systemstate_run;
+
+	void 			setLEDColor(int ledcolor);
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	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 *const 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, int blinkm) :
+	I2C("blinkm", BLINKM_DEVICE_PATH, bus, blinkm, 100000),
+	led_color_1(LED_OFF),
+	led_color_2(LED_OFF),
+	led_color_3(LED_OFF),
+	led_color_4(LED_OFF),
+	led_color_5(LED_OFF),
+	led_color_6(LED_OFF),
+	led_color_7(LED_OFF),
+	led_color_8(LED_OFF),
+	led_blink(LED_NOBLINK),
+	systemstate_run(false)
+{
+	memset(&_work, 0, sizeof(_work));
+}
+
+BlinkM::~BlinkM()
+{
+}
+
+int
+BlinkM::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	stop_script();
+	set_rgb(0,0,0);
+
+	return OK;
+}
+
+int
+BlinkM::setMode(int mode)
+{
+	if(mode == 1) {
+		if(systemstate_run == false) {
+			stop_script();
+			set_rgb(0,0,0);
+			systemstate_run = true;
+			work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
+		}
+	} else {
+		systemstate_run = false;
+	}
+
+	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::led_trampoline(void *arg)
+{
+	BlinkM *bm = (BlinkM *)arg;
+
+	bm->led();
+}
+
+
+
+void
+BlinkM::led()
+{
+
+	static int vehicle_status_sub_fd;
+	static int vehicle_gps_position_sub_fd;
+
+	static int num_of_cells = 0;
+	static int detected_cells_runcount = 0;
+
+	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
+	static int t_led_blink = 0;
+	static int led_thread_runcount=0;
+	static int led_interval = 1000;
+
+	static int no_data_vehicle_status = 0;
+	static int no_data_vehicle_gps_position = 0;
+
+	static bool topic_initialized = false;
+	static bool detected_cells_blinked = false;
+	static bool led_thread_ready = true;
+
+	int num_of_used_sats = 0;
+
+	if(!topic_initialized) {
+		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
+		orb_set_interval(vehicle_status_sub_fd, 1000);
+
+		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
+		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
+
+		topic_initialized = true;
+	}
+
+	if(led_thread_ready == true) {
+		if(!detected_cells_blinked) {
+			if(num_of_cells > 0) {
+				t_led_color[0] = LED_PURPLE;
+			}
+			if(num_of_cells > 1) {
+				t_led_color[1] = LED_PURPLE;
+			}
+			if(num_of_cells > 2) {
+				t_led_color[2] = LED_PURPLE;
+			}
+			if(num_of_cells > 3) {
+				t_led_color[3] = LED_PURPLE;
+			}
+			if(num_of_cells > 4) {
+				t_led_color[4] = LED_PURPLE;
+			}
+			t_led_color[5] = LED_OFF;
+			t_led_color[6] = LED_OFF;
+			t_led_color[7] = LED_OFF;
+			t_led_blink = LED_BLINK;
+		} else {
+			t_led_color[0] = led_color_1;
+			t_led_color[1] = led_color_2;
+			t_led_color[2] = led_color_3;
+			t_led_color[3] = led_color_4;
+			t_led_color[4] = led_color_5;
+			t_led_color[5] = led_color_6;
+			t_led_color[6] = led_color_7;
+			t_led_color[7] = led_color_8;
+			t_led_blink = led_blink;
+		}
+		led_thread_ready = false;
+	}
+
+	if (led_thread_runcount & 1) {
+		if (t_led_blink)
+			setLEDColor(LED_OFF);
+		led_interval = LED_OFFTIME;
+	} else {
+		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
+		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
+		led_interval = LED_ONTIME;
+	}
+
+	if (led_thread_runcount == 15) {
+		/* obtained data for the first file descriptor */
+		struct vehicle_status_s vehicle_status_raw;
+		struct vehicle_gps_position_s vehicle_gps_position_raw;
+
+		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
+		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
+
+		bool new_data_vehicle_status;
+		bool new_data_vehicle_gps_position;
+
+		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
+
+		if (new_data_vehicle_status) {
+			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
+			no_data_vehicle_status = 0;
+		} else {
+			no_data_vehicle_status++;
+			if(no_data_vehicle_status >= 3)
+				no_data_vehicle_status = 3;
+		}
+
+		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
+
+		if (new_data_vehicle_gps_position) {
+			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
+			no_data_vehicle_gps_position = 0;
+		} else {
+			no_data_vehicle_gps_position++;
+			if(no_data_vehicle_gps_position >= 3)
+				no_data_vehicle_gps_position = 3;
+		}
+
+
+
+		/* get number of used satellites in navigation */
+		num_of_used_sats = 0;
+		//for(int satloop=0; satloop<20; satloop++) {
+		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
+				num_of_used_sats++;
+			}
+		}
+
+		if(new_data_vehicle_status || no_data_vehicle_status < 3){
+			if(num_of_cells == 0) {
+				/* looking for lipo cells that are connected */
+				printf("<blinkm> checking cells\n");
+				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
+					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+				}
+				printf("<blinkm> cells found:%d\n", num_of_cells);
+
+			} else {
+				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+					/* LED Pattern for battery low warning */
+					led_color_1 = LED_YELLOW;
+					led_color_2 = LED_YELLOW;
+					led_color_3 = LED_YELLOW;
+					led_color_4 = LED_YELLOW;
+					led_color_5 = LED_YELLOW;
+					led_color_6 = LED_YELLOW;
+					led_color_7 = LED_YELLOW;
+					led_color_8 = LED_YELLOW;
+					led_blink = LED_BLINK;
+
+				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+					/* LED Pattern for battery critical alerting */
+					led_color_1 = LED_RED;
+					led_color_2 = LED_RED;
+					led_color_3 = LED_RED;
+					led_color_4 = LED_RED;
+					led_color_5 = LED_RED;
+					led_color_6 = LED_RED;
+					led_color_7 = LED_RED;
+					led_color_8 = LED_RED;
+					led_blink = LED_BLINK;
+
+				} else {
+					/* no battery warnings here */
+
+					if(vehicle_status_raw.flag_fmu_armed == false) {
+						/* system not armed */
+						led_color_1 = LED_RED;
+						led_color_2 = LED_RED;
+						led_color_3 = LED_RED;
+						led_color_4 = LED_RED;
+						led_color_5 = LED_RED;
+						led_color_6 = LED_RED;
+						led_color_7 = LED_RED;
+						led_color_8 = LED_RED;
+						led_blink = LED_NOBLINK;
+
+					} else {
+						/* armed system - initial led pattern */
+						led_color_1 = LED_RED;
+						led_color_2 = LED_RED;
+						led_color_3 = LED_RED;
+						led_color_4 = LED_OFF;
+						led_color_5 = LED_OFF;
+						led_color_6 = LED_OFF;
+						led_color_7 = LED_OFF;
+						led_color_8 = LED_OFF;
+						led_blink = LED_BLINK;
+
+						/* handle 4th led - flightmode indicator */
+#warning fix this
+//						switch((int)vehicle_status_raw.flight_mode) {
+//							case VEHICLE_FLIGHT_MODE_MANUAL:
+//								led_color_4 = LED_AMBER;
+//								break;
+//
+//							case VEHICLE_FLIGHT_MODE_STAB:
+//								led_color_4 = LED_YELLOW;
+//								break;
+//
+//							case VEHICLE_FLIGHT_MODE_HOLD:
+//								led_color_4 = LED_BLUE;
+//								break;
+//
+//							case VEHICLE_FLIGHT_MODE_AUTO:
+//								led_color_4 = LED_GREEN;
+//								break;
+//						}
+
+						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
+							/* handling used sat�s */
+							if(num_of_used_sats >= 7) {
+								led_color_1 = LED_OFF;
+								led_color_2 = LED_OFF;
+								led_color_3 = LED_OFF;
+							} else if(num_of_used_sats == 6) {
+								led_color_2 = LED_OFF;
+								led_color_3 = LED_OFF;
+							} else if(num_of_used_sats == 5) {
+								led_color_3 = LED_OFF;
+							}
+
+						} else {
+							/* no vehicle_gps_position data */
+							led_color_1 = LED_WHITE;
+							led_color_2 = LED_WHITE;
+							led_color_3 = LED_WHITE;
+
+						}
+
+					}
+				}
+			}
+		} else {
+			/* LED Pattern for general Error - no vehicle_status can retrieved */
+			led_color_1 = LED_WHITE;
+			led_color_2 = LED_WHITE;
+			led_color_3 = LED_WHITE;
+			led_color_4 = LED_WHITE;
+			led_color_5 = LED_WHITE;
+			led_color_6 = LED_WHITE;
+			led_color_7 = LED_WHITE;
+			led_color_8 = LED_WHITE;
+			led_blink = LED_BLINK;
+
+		}
+
+		/*
+		printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
+		vehicle_status_raw.voltage_battery,
+		vehicle_status_raw.flag_system_armed,
+		vehicle_status_raw.flight_mode,
+		num_of_cells,
+		no_data_vehicle_status,
+		no_data_vehicle_gps_position,
+		num_of_used_sats,
+		vehicle_gps_position_raw.fix_type,
+		vehicle_gps_position_raw.satellites_visible);
+		*/
+
+		led_thread_runcount=0;
+		led_thread_ready = true;
+		led_interval = LED_OFFTIME;
+
+		if(detected_cells_runcount < 4){
+			detected_cells_runcount++;
+		} else {
+			detected_cells_blinked = true;
+		}
+
+	} else {
+		led_thread_runcount++;
+	}
+
+	if(systemstate_run == true) {
+		/* re-queue ourselves to run again later */
+		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
+	} else {
+		stop_script();
+		set_rgb(0,0,0);
+	}
+}
+
+void BlinkM::setLEDColor(int ledcolor) {
+	switch (ledcolor) {
+		case LED_OFF:	// off
+			set_rgb(0,0,0);
+			break;
+		case LED_RED:	// red
+			set_rgb(255,0,0);
+			break;
+		case LED_YELLOW:	// yellow
+			set_rgb(255,70,0);
+			break;
+		case LED_PURPLE:	// purple
+			set_rgb(255,0,255);
+			break;
+		case LED_GREEN:	// green
+			set_rgb(0,255,0);
+			break;
+		case LED_BLUE:	// blue
+			set_rgb(0,0,255);
+			break;
+		case LED_WHITE:	// white
+			set_rgb(255,255,255);
+			break;
+		case LED_AMBER:	// amber
+			set_rgb(255,20,0);
+			break;
+	}
+}
+
+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, 2);
+}
+
+void blinkm_usage() {
+	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
+	fprintf(stderr, "options:\n");
+	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
+	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+}
+
+int
+blinkm_main(int argc, char *argv[])
+{
+
+	int i2cdevice = PX4_I2C_BUS_EXPANSION;
+	int blinkmadr = 9;
+
+	int x;
+
+	for (x = 1; x < argc; x++) {
+		if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
+			if (argc > x + 1) {
+				i2cdevice = atoi(argv[x + 1]);
+			}
+		}
+
+		if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) {
+			if (argc > x + 1) {
+				blinkmadr = atoi(argv[x + 1]);
+			}
+		}
+
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (g_blinkm != nullptr)
+			errx(1, "already started");
+
+		g_blinkm = new BlinkM(i2cdevice, blinkmadr);
+
+		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) {
+		fprintf(stderr, "not started\n");
+		blinkm_usage();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "systemstate")) {
+		g_blinkm->setMode(1);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "ledoff")) {
+		g_blinkm->setMode(0);
+		exit(0);
+	}
+
+
+	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");
+
+	g_blinkm->setMode(0);
+	if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
+		exit(0);
+
+	blinkm_usage();
+	exit(0);
+}
diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk
new file mode 100644
index 000000000..b48b90f3f
--- /dev/null
+++ b/src/drivers/blinkm/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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
+#
+
+MODULE_COMMAND	= blinkm
+
+SRCS		= blinkm.cpp
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
new file mode 100644
index 000000000..4409a8a9c
--- /dev/null
+++ b/src/drivers/bma180/bma180.cpp
@@ -0,0 +1,929 @@
+/****************************************************************************
+ *
+ *   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 bma180.cpp
+ * Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+
+#define ADDR_CHIP_ID			0x00
+#define CHIP_ID				0x03
+
+#define ADDR_ACC_X_LSB			0x02
+#define ADDR_ACC_Y_LSB			0x04
+#define ADDR_ACC_Z_LSB			0x06
+#define ADDR_TEMPERATURE		0x08
+
+#define ADDR_CTRL_REG0			0x0D
+#define REG0_WRITE_ENABLE		0x10
+
+#define ADDR_RESET			0x10
+#define SOFT_RESET				0xB6
+
+#define ADDR_BW_TCS			0x20
+#define BW_TCS_BW_MASK				(0xf<<4)
+#define BW_TCS_BW_10HZ				(0<<4)
+#define BW_TCS_BW_20HZ				(1<<4)
+#define BW_TCS_BW_40HZ				(2<<4)
+#define BW_TCS_BW_75HZ				(3<<4)
+#define BW_TCS_BW_150HZ				(4<<4)
+#define BW_TCS_BW_300HZ				(5<<4)
+#define BW_TCS_BW_600HZ				(6<<4)
+#define BW_TCS_BW_1200HZ			(7<<4)
+
+#define ADDR_HIGH_DUR			0x27
+#define HIGH_DUR_DIS_I2C			(1<<0)
+
+#define ADDR_TCO_Z			0x30
+#define TCO_Z_MODE_MASK				0x3
+
+#define ADDR_GAIN_Y			0x33
+#define GAIN_Y_SHADOW_DIS			(1<<0)
+
+#define ADDR_OFFSET_LSB1		0x35
+#define OFFSET_LSB1_RANGE_MASK			(7<<1)
+#define OFFSET_LSB1_RANGE_1G			(0<<1)
+#define OFFSET_LSB1_RANGE_2G			(2<<1)
+#define OFFSET_LSB1_RANGE_3G			(3<<1)
+#define OFFSET_LSB1_RANGE_4G			(4<<1)
+#define OFFSET_LSB1_RANGE_8G			(5<<1)
+#define OFFSET_LSB1_RANGE_16G			(6<<1)
+
+#define ADDR_OFFSET_T			0x37
+#define OFFSET_T_READOUT_12BIT			(1<<0)
+
+extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); }
+
+class BMA180 : public device::SPI
+{
+public:
+	BMA180(int bus, spi_dev_e device);
+	virtual ~BMA180();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+
+	struct hrt_call		_call;
+	unsigned		_call_interval;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	struct accel_report	*_reports;
+
+	struct accel_scale	_accel_scale;
+	float			_accel_range_scale;
+	float			_accel_range_m_s2;
+	orb_advert_t		_accel_topic;
+
+	unsigned		_current_lowpass;
+	unsigned		_current_range;
+
+	perf_counter_t		_sample_perf;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the BMA180
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the BMA180
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the BMA180
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the BMA180 measurement range.
+	 *
+	 * @param max_g		The maximum G value the range must support.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_range(unsigned max_g);
+
+	/**
+	 * Set the BMA180 internal lowpass filter frequency.
+	 *
+	 * @param frequency	The internal lowpass filter frequency is set to a value
+	 *			equal or greater to this.
+	 *			Zero selects the highest frequency supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			set_lowpass(unsigned frequency);
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+BMA180::BMA180(int bus, spi_dev_e device) :
+	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
+	_call_interval(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_accel_range_scale(0.0f),
+	_accel_range_m_s2(0.0f),
+	_accel_topic(-1),
+	_current_lowpass(0),
+	_current_range(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+}
+
+BMA180::~BMA180()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+
+	/* delete the perf counter */
+	perf_free(_sample_perf);
+}
+
+int
+BMA180::init()
+{
+	int ret = ERROR;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_oldest_report = _next_report = 0;
+	_reports = new struct accel_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	/* advertise sensor topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
+
+	/* perform soft reset (p48) */
+	write_reg(ADDR_RESET, SOFT_RESET);
+
+	/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
+	usleep(10000);
+
+	/* enable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
+
+	/* disable I2C interface */
+	modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
+
+	/* switch to low-noise mode */
+	modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
+
+	/* disable 12-bit mode */
+	modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
+
+	/* disable shadow-disable mode */
+	modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
+
+	/* disable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
+
+	if (set_range(4)) warnx("Failed setting range");
+
+	if (set_lowpass(75)) warnx("Failed setting lowpass");
+
+	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
+		ret = OK;
+
+	} else {
+		ret = ERROR;
+	}
+
+out:
+	return ret;
+}
+
+int
+BMA180::probe()
+{
+	/* dummy read to ensure SPI state machine is sane */
+	read_reg(ADDR_CHIP_ID);
+
+	if (read_reg(ADDR_CHIP_ID) == CHIP_ID)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+BMA180::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_report = _next_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _reports, sizeof(*_reports));
+	ret = sizeof(*_reports);
+
+	return ret;
+}
+
+int
+BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_interval = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* With internal low pass filters enabled, 250 Hz is sufficient */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_call.period = _call_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct accel_report *buf = new struct accel_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement */
+		return -EINVAL;
+
+	case ACCELIOCSSAMPLERATE:	/* sensor sample rate is not (really) adjustable */
+		return -EINVAL;
+
+	case ACCELIOCGSAMPLERATE:
+		return 1200;		/* always operating in low-noise mode */
+
+	case ACCELIOCSLOWPASS:
+		return set_lowpass(arg);
+
+	case ACCELIOCGLOWPASS:
+		return _current_lowpass;
+
+	case ACCELIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCSRANGE:
+		return set_range(arg);
+
+	case ACCELIOCGRANGE:
+		return _current_range;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+BMA180::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+BMA180::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+BMA180::set_range(unsigned max_g)
+{
+	uint8_t rangebits;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g > 16)
+		return -ERANGE;
+
+	if (max_g <= 2) {
+		_current_range = 2;
+		rangebits = OFFSET_LSB1_RANGE_2G;
+
+	} else if (max_g <= 3) {
+		_current_range = 3;
+		rangebits = OFFSET_LSB1_RANGE_3G;
+
+	} else if (max_g <= 4) {
+		_current_range = 4;
+		rangebits = OFFSET_LSB1_RANGE_4G;
+
+	} else if (max_g <= 8) {
+		_current_range = 8;
+		rangebits = OFFSET_LSB1_RANGE_8G;
+
+	} else if (max_g <= 16) {
+		_current_range = 16;
+		rangebits = OFFSET_LSB1_RANGE_16G;
+
+	} else {
+		return -EINVAL;
+	}
+
+	/* set new range scaling factor */
+	_accel_range_m_s2 = _current_range * 9.80665f;
+	_accel_range_scale = _accel_range_m_s2 / 8192.0f;
+
+	/* enable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
+
+	/* block writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
+
+	/* check if wanted value is now in register */
+	return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
+		 (OFFSET_LSB1_RANGE_MASK & rangebits));
+}
+
+int
+BMA180::set_lowpass(unsigned frequency)
+{
+	uint8_t	bwbits;
+
+	if (frequency > 1200) {
+		return -ERANGE;
+
+	} else if (frequency > 600) {
+		bwbits = BW_TCS_BW_1200HZ;
+
+	} else if (frequency > 300) {
+		bwbits = BW_TCS_BW_600HZ;
+
+	} else if (frequency > 150) {
+		bwbits = BW_TCS_BW_300HZ;
+
+	} else if (frequency > 75) {
+		bwbits = BW_TCS_BW_150HZ;
+
+	} else if (frequency > 40) {
+		bwbits = BW_TCS_BW_75HZ;
+
+	} else if (frequency > 20) {
+		bwbits = BW_TCS_BW_40HZ;
+
+	} else if (frequency > 10) {
+		bwbits = BW_TCS_BW_20HZ;
+
+	} else {
+		bwbits = BW_TCS_BW_10HZ;
+	}
+
+	/* enable writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
+
+	/* block writing to chip config */
+	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
+
+	/* check if wanted value is now in register */
+	return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
+		 (BW_TCS_BW_MASK & bwbits));
+}
+
+void
+BMA180::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_report = _next_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
+}
+
+void
+BMA180::stop()
+{
+	hrt_cancel(&_call);
+}
+
+void
+BMA180::measure_trampoline(void *arg)
+{
+	BMA180 *dev = (BMA180 *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+BMA180::measure()
+{
+	/* BMA180 measurement registers */
+// #pragma pack(push, 1)
+// 	struct {
+// 		uint8_t		cmd;
+// 		int16_t	x;
+// 		int16_t	y;
+// 		int16_t	z;
+// 	} raw_report;
+// #pragma pack(pop)
+
+	accel_report		*report = &_reports[_next_report];
+
+	/* start the performance counter */
+	perf_begin(_sample_perf);
+
+	/*
+	 * Fetch the full set of measurements from the BMA180 in one pass;
+	 * starting from the X LSB.
+	 */
+	//raw_report.cmd = ADDR_ACC_X_LSB;
+	// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
+
+	/*
+	 * Adjust and scale results to SI units.
+	 *
+	 * Note that we ignore the "new data" bits.  At any time we read, each
+	 * of the axis measurements are the "most recent", even if we've seen
+	 * them before.  There is no good way to synchronise with the internal
+	 * measurement flow without using the external interrupt.
+	 */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	/*
+	 * y of board is x of sensor and x of board is -y of sensor
+	 * perform only the axis assignment here.
+	 * Two non-value bits are discarded directly
+	 */
+	report->y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
+	report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
+	report->x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
+	report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
+	report->z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
+	report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
+
+	/* discard two non-value bits in the 16 bit measurement */
+	report->x_raw = (report->x_raw / 4);
+	report->y_raw = (report->y_raw / 4);
+	report->z_raw = (report->z_raw / 4);
+
+	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
+	report->y_raw = -report->y_raw;
+
+	report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	report->scaling = _accel_range_scale;
+	report->range_m_s2 = _accel_range_m_s2;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_report == _oldest_report)
+		INCREMENT(_oldest_report, _num_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
+
+	/* stop the perf counter */
+	perf_end(_sample_perf);
+}
+
+void
+BMA180::print_info()
+{
+	perf_print_counter(_sample_perf);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace bma180
+{
+
+BMA180	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd = -1;
+	struct accel_report a_report;
+	ssize_t sz;
+
+	/* get the driver */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
+		    ACCEL_DEVICE_PATH);
+
+	/* reset to manual polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
+		err(1, "reset to manual polling");
+
+	/* do a simple demand read */
+	sz = read(fd, &a_report, sizeof(a_report));
+
+	if (sz != sizeof(a_report))
+		err(1, "immediate acc read failed");
+
+	warnx("single read");
+	warnx("time:     %lld", a_report.timestamp);
+	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
+	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
+	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
+	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
+	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
+	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
+	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
+	      (double)(a_report.range_m_s2 / 9.81f));
+
+	/* XXX add poll-rate tests here too */
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "BMA180: driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+bma180_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		bma180::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		bma180::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		bma180::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		bma180::info();
+
+	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk
new file mode 100644
index 000000000..4c60ee082
--- /dev/null
+++ b/src/drivers/bma180/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Makefile to build the BMA180 driver.
+#
+
+MODULE_COMMAND	= bma180
+
+SRCS		= bma180.cpp
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk
new file mode 100644
index 000000000..66b776917
--- /dev/null
+++ b/src/drivers/boards/px4fmu/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMU
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c \
+		   px4fmu_led.c
diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c
new file mode 100644
index 000000000..187689ff9
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "px4fmu_internal.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c
new file mode 100644
index 000000000..69edc23ab
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_init.c
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs (empty call to NuttX' ledinit) */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct spi_dev_s *spi3;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+	int result;
+
+	/* configure always-on ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);
+	stm32_configgpio(GPIO_ADC1_IN11);
+	/* IN12 and IN13 further below */
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+	led_on(LED_BLUE);
+
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\r\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\r\n");
+
+	/*
+	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
+	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
+	 */
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	} else {
+		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
+		SPI_SETFREQUENCY(spi2, 10000000);
+		SPI_SETBITS(spi2, 8);
+		SPI_SETMODE(spi2, SPIDEV_MODE3);
+		SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
+		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
+
+		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
+	}
+
+	/* Get the SPI port for the microSD slot */
+
+	message("[boot] Initializing SPI port 3\n");
+	spi3 = up_spiinitialize(3);
+
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully initialized SPI port 3\n");
+
+	/* Now bind the SPI interface to the MMCSD driver */
+	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
+
+	if (result != OK) {
+		message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h
new file mode 100644
index 000000000..671a58f8f
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_internal.h
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_internal.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32_internal.h>
+
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+//#ifdef CONFIG_STM32_SPI2
+//#  error "SPI2 is not supported on this board"
+//#endif
+
+#if defined(CONFIG_STM32_CAN1)
+#  warning "CAN1 is not supported on this board"
+#endif
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+
+/* External interrupts */
+#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+// XXX MPU6000 DRDY?
+
+/* SPI chip selects */
+
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+
+/* User GPIOs
+ *
+ * GPIO0-1 are the buffered high-power GPIOs.
+ * GPIO2-5 are the USART2 pins.
+ * GPIO6-7 are the CAN2 pins.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* PWM
+ *
+ * The PX4FMU has five PWM outputs, of which only the output on
+ * pin PC8 is fixed assigned to this function. The other four possible
+ * pwm sources are the TX, RX, RTS and CTS pins of USART2
+ *
+ * Alternate function mapping:
+ * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+ */
+
+#ifdef CONFIG_PWM
+#  if defined(CONFIG_STM32_TIM3_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#	 define BUZZER_PWMTIMER 3
+#  elif defined(CONFIG_STM32_TIM8_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#    define BUZZER_PWMTIMER 8
+#  endif
+#endif
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c
new file mode 100644
index 000000000..34fd194c3
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_led.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1-2 GPIOs for output */
+
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 0)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED2, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 0)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED2, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..cb8918306
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c
new file mode 100644
index 000000000..b5d00eac0
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_spi.c
@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+	stm32_configgpio(GPIO_SPI_CS_SDCARD);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
+	return SPI_STATUS_PRESENT;
+}
+
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c
new file mode 100644
index 000000000..b0b669fbe
--- /dev/null
+++ b/src/drivers/boards/px4fmu/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include "up_arch.h"
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io/module.mk
new file mode 100644
index 000000000..2601a1c15
--- /dev/null
+++ b/src/drivers/boards/px4io/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IO
+#
+
+SRCS	= px4io_init.c \
+	  px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c
new file mode 100644
index 000000000..d36353c6f
--- /dev/null
+++ b/src/drivers/boards/px4io/px4io_init.c
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   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_init.c
+ *
+ * PX4IO-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+
+#include "stm32_internal.h"
+#include "px4io_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_pwm_output.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure GPIOs */
+	stm32_configgpio(GPIO_ACC1_PWR_EN);
+	stm32_configgpio(GPIO_ACC2_PWR_EN);
+	stm32_configgpio(GPIO_SERVO_PWR_EN);
+	stm32_configgpio(GPIO_RELAY1_EN);
+	stm32_configgpio(GPIO_RELAY2_EN);
+
+    	/* turn off - all leds are active low */
+	stm32_gpiowrite(GPIO_LED1, true);
+	stm32_gpiowrite(GPIO_LED2, true);
+	stm32_gpiowrite(GPIO_LED3, true);
+
+	/* LED config */
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+
+	stm32_configgpio(GPIO_ACC_OC_DETECT);
+	stm32_configgpio(GPIO_SERVO_OC_DETECT);
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	stm32_configgpio(GPIO_ADC_VBATT);
+	stm32_configgpio(GPIO_ADC_IN5);
+}
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h
new file mode 100644
index 000000000..eb2820bb7
--- /dev/null
+++ b/src/drivers/boards/px4io/px4io_internal.h
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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_internal.h
+ * 
+ * PX4IO hardware definitions.
+ */
+
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+#include <stm32_internal.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* PX4IO GPIOs **********************************************************************/
+/* LEDs */
+
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+
+/* Safety switch button *************************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ************************************************************/
+
+#define GPIO_ACC1_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_ACC2_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_ACC_OC_DETECT   (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
+
+#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
+
+/* Analog inputs ********************************************************************/
+
+#define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+#define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c
new file mode 100644
index 000000000..a2f73c429
--- /dev/null
+++ b/src/drivers/boards/px4io/px4io_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
new file mode 100644
index 000000000..422321850
--- /dev/null
+++ b/src/drivers/device/cdev.cpp
@@ -0,0 +1,398 @@
+/****************************************************************************
+ *
+ *   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 cdev.cpp
+ *
+ * Character device base class.
+ */
+
+#include "device.h"
+
+#include <sys/ioctl.h>
+#include <arch/irq.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef CONFIG_DISABLE_POLL
+# error This driver is not compatible with CONFIG_DISABLE_POLL
+#endif
+
+namespace device
+{
+
+/* how much to grow the poll waiter set each time it has to be increased */
+static const unsigned pollset_increment = 0;
+
+/*
+ * The standard NuttX operation dispatch table can't call C++ member functions
+ * directly, so we have to bounce them through this dispatch table.
+ */
+static int	cdev_open(struct file *filp);
+static int	cdev_close(struct file *filp);
+static ssize_t	cdev_read(struct file *filp, char *buffer, size_t buflen);
+static ssize_t	cdev_write(struct file *filp, const char *buffer, size_t buflen);
+static off_t	cdev_seek(struct file *filp, off_t offset, int whence);
+static int	cdev_ioctl(struct file *filp, int cmd, unsigned long arg);
+static int	cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
+
+/**
+ * Character device indirection table.
+ *
+ * Every cdev we register gets the same function table; we use the private data
+ * field in the inode to store the instance pointer.
+ *
+ * Note that we use the GNU extension syntax here because we don't get designated
+ * initialisers in gcc 4.6.
+ */
+const struct file_operations CDev::fops = {
+open	: cdev_open,
+close	: cdev_close,
+read	: cdev_read,
+write	: cdev_write,
+seek	: cdev_seek,
+ioctl	: cdev_ioctl,
+poll	: cdev_poll,
+};
+
+CDev::CDev(const char *name,
+	   const char *devname,
+	   int irq) :
+	// base class
+	Device(name, irq),
+	// public
+	// protected
+	// private
+	_devname(devname),
+	_registered(false),
+	_open_count(0)
+{
+	for (unsigned i = 0; i < _max_pollwaiters; i++)
+		_pollset[i] = nullptr;
+}
+
+CDev::~CDev()
+{
+	if (_registered)
+		unregister_driver(_devname);
+}
+
+int
+CDev::init()
+{
+	int ret = OK;
+
+	// base class init first
+	ret = Device::init();
+
+	if (ret != OK)
+		goto out;
+
+	// now register the driver
+	ret = register_driver(_devname, &fops, 0666, (void *)this);
+
+	if (ret != OK)
+		goto out;
+
+	_registered = true;
+
+out:
+	return ret;
+}
+
+/*
+ * Default implementations of the character device interface
+ */
+int
+CDev::open(struct file *filp)
+{
+	int ret = OK;
+
+	lock();
+	/* increment the open count */
+	_open_count++;
+
+	if (_open_count == 1) {
+
+		/* first-open callback may decline the open */
+		ret = open_first(filp);
+
+		if (ret != OK)
+			_open_count--;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::open_first(struct file *filp)
+{
+	return OK;
+}
+
+int
+CDev::close(struct file *filp)
+{
+	int ret = OK;
+
+	lock();
+
+	if (_open_count > 0) {
+		/* decrement the open count */
+		_open_count--;
+
+		/* callback cannot decline the close */
+		if (_open_count == 0)
+			ret = close_last(filp);
+
+	} else {
+		ret = -EBADF;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::close_last(struct file *filp)
+{
+	return OK;
+}
+
+ssize_t
+CDev::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return -ENOSYS;
+}
+
+ssize_t
+CDev::write(struct file *filp, const char *buffer, size_t buflen)
+{
+	return -ENOSYS;
+}
+
+off_t
+CDev::seek(struct file *filp, off_t offset, int whence)
+{
+	return -ENOSYS;
+}
+
+int
+CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+		/* fetch a pointer to the driver's private data */
+	case DIOC_GETPRIV:
+		*(void **)(uintptr_t)arg = (void *)this;
+		return OK;
+	}
+
+	return -ENOTTY;
+}
+
+int
+CDev::poll(struct file *filp, struct pollfd *fds, bool setup)
+{
+	int ret = OK;
+
+	/*
+	 * Lock against pollnotify() (and possibly other callers)
+	 */
+	lock();
+
+	if (setup) {
+		/*
+		 * Save the file pointer in the pollfd for the subclass'
+		 * benefit.
+		 */
+		fds->priv = (void *)filp;
+
+		/*
+		 * Handle setup requests.
+		 */
+		ret = store_poll_waiter(fds);
+
+		if (ret == OK) {
+
+			/*
+			 * Check to see whether we should send a poll notification
+			 * immediately.
+			 */
+			fds->revents |= fds->events & poll_state(filp);
+
+			/* yes? post the notification */
+			if (fds->revents != 0)
+				sem_post(fds->sem);
+		}
+
+	} else {
+		/*
+		 * Handle a teardown request.
+		 */
+		ret = remove_poll_waiter(fds);
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+CDev::poll_notify(pollevent_t events)
+{
+	/* lock against poll() as well as other wakeups */
+	irqstate_t state = irqsave();
+
+	for (unsigned i = 0; i < _max_pollwaiters; i++)
+		if (nullptr != _pollset[i])
+			poll_notify_one(_pollset[i], events);
+
+	irqrestore(state);
+}
+
+void
+CDev::poll_notify_one(struct pollfd *fds, pollevent_t events)
+{
+	/* update the reported event set */
+	fds->revents |= fds->events & events;
+
+	/* if the state is now interesting, wake the waiter if it's still asleep */
+	/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
+	if ((fds->revents != 0) && (fds->sem->semcount <= 0))
+		sem_post(fds->sem);
+}
+
+pollevent_t
+CDev::poll_state(struct file *filp)
+{
+	/* by default, no poll events to report */
+	return 0;
+}
+
+int
+CDev::store_poll_waiter(struct pollfd *fds)
+{
+	/*
+	 * Look for a free slot.
+	 */
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (nullptr == _pollset[i]) {
+
+			/* save the pollfd */
+			_pollset[i] = fds;
+
+			return OK;
+		}
+	}
+
+	return ENOMEM;
+}
+
+int
+CDev::remove_poll_waiter(struct pollfd *fds)
+{
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (fds == _pollset[i]) {
+
+			_pollset[i] = nullptr;
+			return OK;
+
+		}
+	}
+
+	puts("poll: bad fd state");
+	return -EINVAL;
+}
+
+static int
+cdev_open(struct file *filp)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->open(filp);
+}
+
+static int
+cdev_close(struct file *filp)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->close(filp);
+}
+
+static ssize_t
+cdev_read(struct file *filp, char *buffer, size_t buflen)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->read(filp, buffer, buflen);
+}
+
+static ssize_t
+cdev_write(struct file *filp, const char *buffer, size_t buflen)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->write(filp, buffer, buflen);
+}
+
+static off_t
+cdev_seek(struct file *filp, off_t offset, int whence)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->seek(filp, offset, whence);
+}
+
+static int
+cdev_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->ioctl(filp, cmd, arg);
+}
+
+static int
+cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->poll(filp, fds, setup);
+}
+
+} // namespace device
\ No newline at end of file
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
new file mode 100644
index 000000000..04a5222c3
--- /dev/null
+++ b/src/drivers/device/device.cpp
@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   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 device.cpp
+ *
+ * Fundamental driver base class for the device framework.
+ */
+
+#include "device.h"
+
+#include <nuttx/arch.h>
+#include <stdio.h>
+#include <unistd.h>
+
+namespace device
+{
+
+/**
+ * Interrupt dispatch table entry.
+ */
+struct irq_entry {
+	int	irq;
+	Device	*owner;
+};
+
+static const unsigned	irq_nentries = 8;		/**< size of the interrupt dispatch table */
+static irq_entry	irq_entries[irq_nentries];	/**< interrupt dispatch table (XXX should be a vector) */
+
+/**
+ * Register an interrupt to a specific device.
+ *
+ * @param irq		The interrupt number to register.
+ * @param owner		The device receiving the interrupt.
+ * @return		OK if the interrupt was registered.
+ */
+static int	register_interrupt(int irq, Device *owner);
+
+/**
+ * Unregister an interrupt.
+ *
+ * @param irq		The previously-registered interrupt to be de-registered.
+ */
+static void	unregister_interrupt(int irq);
+
+/**
+ * Handle an interrupt.
+ *
+ * @param irq		The interrupt being invoked.
+ * @param context	The interrupt register context.
+ * @return		Always returns OK.
+ */
+static int	interrupt(int irq, void *context);
+
+Device::Device(const char *name,
+	       int irq) :
+	// public
+	// protected
+	_name(name),
+	_debug_enabled(false),
+	// private
+	_irq(irq),
+	_irq_attached(false)
+{
+	sem_init(&_lock, 0, 1);
+}
+
+Device::~Device()
+{
+	sem_destroy(&_lock);
+
+	if (_irq_attached)
+		unregister_interrupt(_irq);
+}
+
+int
+Device::init()
+{
+	int ret = OK;
+
+	// If assigned an interrupt, connect it
+	if (_irq) {
+		/* ensure it's disabled */
+		up_disable_irq(_irq);
+
+		/* register */
+		ret = register_interrupt(_irq, this);
+
+		if (ret != OK)
+			goto out;
+
+		_irq_attached = true;
+	}
+
+out:
+	return ret;
+}
+
+void
+Device::interrupt_enable()
+{
+	if (_irq_attached)
+		up_enable_irq(_irq);
+}
+
+void
+Device::interrupt_disable()
+{
+	if (_irq_attached)
+		up_disable_irq(_irq);
+}
+
+void
+Device::interrupt(void *context)
+{
+	// default action is to disable the interrupt so we don't get called again
+	interrupt_disable();
+}
+
+void
+Device::log(const char *fmt, ...)
+{
+	va_list	ap;
+
+	printf("[%s] ", _name);
+	va_start(ap, fmt);
+	vprintf(fmt, ap);
+	va_end(ap);
+	printf("\n");
+	fflush(stdout);
+}
+
+void
+Device::debug(const char *fmt, ...)
+{
+	va_list	ap;
+
+	if (_debug_enabled) {
+		printf("<%s> ", _name);
+		va_start(ap, fmt);
+		vprintf(fmt, ap);
+		va_end(ap);
+		printf("\n");
+		fflush(stdout);
+	}
+}
+
+static int
+register_interrupt(int irq, Device *owner)
+{
+	int ret = -ENOMEM;
+
+	// look for a slot where we can register the interrupt
+	for (unsigned i = 0; i < irq_nentries; i++) {
+		if (irq_entries[i].irq == 0) {
+
+			// great, we could put it here; try attaching it
+			ret = irq_attach(irq, &interrupt);
+
+			if (ret == OK) {
+				irq_entries[i].irq = irq;
+				irq_entries[i].owner = owner;
+			}
+
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static void
+unregister_interrupt(int irq)
+{
+	for (unsigned i = 0; i < irq_nentries; i++) {
+		if (irq_entries[i].irq == irq) {
+			irq_entries[i].irq = 0;
+			irq_entries[i].owner = nullptr;
+		}
+	}
+}
+
+static int
+interrupt(int irq, void *context)
+{
+	for (unsigned i = 0; i < irq_nentries; i++) {
+		if (irq_entries[i].irq == irq) {
+			irq_entries[i].owner->interrupt(context);
+			break;
+		}
+	}
+
+	return OK;
+}
+
+
+} // namespace device
\ No newline at end of file
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
new file mode 100644
index 000000000..7d375aab9
--- /dev/null
+++ b/src/drivers/device/device.h
@@ -0,0 +1,447 @@
+/****************************************************************************
+ *
+ *   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 device.h
+ *
+ * Definitions for the generic base classes in the device framework.
+ */
+
+#ifndef _DEVICE_DEVICE_H
+#define _DEVICE_DEVICE_H
+
+/*
+ * Includes here should only cover the needs of the framework definitions.
+ */
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <poll.h>
+
+#include <nuttx/fs/fs.h>
+
+/**
+ * Namespace encapsulating all device framework classes, functions and data.
+ */
+namespace device __EXPORT
+{
+
+/**
+ * Fundamental base class for all device drivers.
+ *
+ * This class handles the basic "being a driver" things, including
+ * interrupt registration and dispatch.
+ */
+class __EXPORT Device
+{
+public:
+	/**
+	 * Interrupt handler.
+	 */
+	virtual void	interrupt(void *ctx);	/**< interrupt handler */
+
+protected:
+	const char	*_name;			/**< driver name */
+	bool		_debug_enabled;		/**< if true, debug messages are printed */
+
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param irq		Interrupt assigned to the device.
+	 */
+	Device(const char *name,
+	       int irq = 0);
+	~Device();
+
+	/**
+	 * Initialise the driver and make it ready for use.
+	 *
+	 * @return	OK if the driver initialised OK.
+	 */
+	virtual int	init();
+
+	/**
+	 * Enable the device interrupt
+	 */
+	void		interrupt_enable();
+
+	/**
+	 * Disable the device interrupt
+	 */
+	void		interrupt_disable();
+
+	/**
+	 * Take the driver lock.
+	 *
+	 * Each driver instance has its own lock/semaphore.
+	 *
+	 * Note that we must loop as the wait may be interrupted by a signal.
+	 */
+	void		lock() {
+		do {} while (sem_wait(&_lock) != 0);
+	}
+
+	/**
+	 * Release the driver lock.
+	 */
+	void		unlock() {
+		sem_post(&_lock);
+	}
+
+	/**
+	 * Log a message.
+	 *
+	 * The message is prefixed with the driver name, and followed
+	 * by a newline.
+	 */
+	void		log(const char *fmt, ...);
+
+	/**
+	 * Print a debug message.
+	 *
+	 * The message is prefixed with the driver name, and followed
+	 * by a newline.
+	 */
+	void		debug(const char *fmt, ...);
+
+private:
+	int		_irq;
+	bool		_irq_attached;
+	sem_t		_lock;
+
+	/** disable copy construction for this and all subclasses */
+	Device(const Device &);
+
+	/** disable assignment for this and all subclasses */
+	Device &operator = (const Device &);
+
+	/**
+	 * Register ourselves as a handler for an interrupt
+	 *
+	 * @param irq		The interrupt to claim
+	 * @return		OK if the interrupt was registered
+	 */
+	int		dev_register_interrupt(int irq);
+
+	/**
+	 * Unregister ourselves as a handler for any interrupt
+	 */
+	void		dev_unregister_interrupt();
+
+	/**
+	 * Interrupt dispatcher
+	 *
+	 * @param irq		The interrupt that has been triggered.
+	 * @param context	Pointer to the interrupted context.
+	 */
+	static void	dev_interrupt(int irq, void *context);
+};
+
+/**
+ * Abstract class for any character device
+ */
+class __EXPORT CDev : public Device
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param irq		Interrupt assigned to the device
+	 */
+	CDev(const char *name, const char *devname, int irq = 0);
+
+	/**
+	 * Destructor
+	 */
+	~CDev();
+
+	virtual int	init();
+
+	/**
+	 * Handle an open of the device.
+	 *
+	 * This function is called for every open of the device. The default
+	 * implementation maintains _open_count and always returns OK.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the open is allowed, -errno otherwise.
+	 */
+	virtual int	open(struct file *filp);
+
+	/**
+	 * Handle a close of the device.
+	 *
+	 * This function is called for every close of the device. The default
+	 * implementation maintains _open_count and returns OK as long as it is not zero.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the close was successful, -errno otherwise.
+	 */
+	virtual int	close(struct file *filp);
+
+	/**
+	 * Perform a read from the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param buffer	Pointer to the buffer into which data should be placed.
+	 * @param buflen	The number of bytes to be read.
+	 * @return		The number of bytes read or -errno otherwise.
+	 */
+	virtual ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+
+	/**
+	 * Perform a write to the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param buffer	Pointer to the buffer from which data should be read.
+	 * @param buflen	The number of bytes to be written.
+	 * @return		The number of bytes written or -errno otherwise.
+	 */
+	virtual ssize_t	write(struct file *filp, const char *buffer, size_t buflen);
+
+	/**
+	 * Perform a logical seek operation on the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param offset	The new file position relative to whence.
+	 * @param whence	SEEK_OFS, SEEK_CUR or SEEK_END.
+	 * @return		The previous offset, or -errno otherwise.
+	 */
+	virtual off_t	seek(struct file *filp, off_t offset, int whence);
+
+	/**
+	 * Perform an ioctl operation on the device.
+	 *
+	 * The default implementation handles DIOC_GETPRIV, and otherwise
+	 * returns -ENOTTY. Subclasses should call the default implementation
+	 * for any command they do not handle themselves.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param cmd		The ioctl command value.
+	 * @param arg		The ioctl argument value.
+	 * @return		OK on success, or -errno otherwise.
+	 */
+	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Perform a poll setup/teardown operation.
+	 *
+	 * This is handled internally and should not normally be overridden.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param fds		Poll descriptor being waited on.
+	 * @param arg		True if this is establishing a request, false if
+	 *			it is being torn down.
+	 * @return		OK on success, or -errno otherwise.
+	 */
+	virtual int	poll(struct file *filp, struct pollfd *fds, bool setup);
+
+	/**
+	 * Test whether the device is currently open.
+	 *
+	 * This can be used to avoid tearing down a device that is still active.
+	 *
+	 * @return              True if the device is currently open.
+	 */
+	bool            is_open() { return _open_count > 0; }
+
+protected:
+	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	static const struct file_operations	fops;
+
+	/**
+	 * Check the current state of the device for poll events from the
+	 * perspective of the file.
+	 *
+	 * This function is called by the default poll() implementation when
+	 * a poll is set up to determine whether the poll should return immediately.
+	 *
+	 * The default implementation returns no events.
+	 *
+	 * @param filp		The file that's interested.
+	 * @return		The current set of poll events.
+	 */
+	virtual pollevent_t poll_state(struct file *filp);
+
+	/**
+	 * Report new poll events.
+	 *
+	 * This function should be called anytime the state of the device changes
+	 * in a fashion that might be interesting to a poll waiter.
+	 *
+	 * @param events	The new event(s) being announced.
+	 */
+	virtual void	poll_notify(pollevent_t events);
+
+	/**
+	 * Internal implementation of poll_notify.
+	 *
+	 * @param fds		A poll waiter to notify.
+	 * @param events	The event(s) to send to the waiter.
+	 */
+	virtual void	poll_notify_one(struct pollfd *fds, pollevent_t events);
+
+	/**
+	 * Notification of the first open.
+	 *
+	 * This function is called when the device open count transitions from zero
+	 * to one.  The driver lock is held for the duration of the call.
+	 *
+	 * The default implementation returns OK.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the open should proceed, -errno otherwise.
+	 */
+	virtual int	open_first(struct file *filp);
+
+	/**
+	 * Notification of the last close.
+	 *
+	 * This function is called when the device open count transitions from
+	 * one to zero.  The driver lock is held for the duration of the call.
+	 *
+	 * The default implementation returns OK.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the open should return OK, -errno otherwise.
+	 */
+	virtual int	close_last(struct file *filp);
+
+private:
+	static const unsigned _max_pollwaiters = 8;
+
+	const char	*_devname;		/**< device node name */
+	bool		_registered;		/**< true if device name was registered */
+	unsigned	_open_count;		/**< number of successful opens */
+
+	struct pollfd	*_pollset[_max_pollwaiters];
+
+	/**
+	 * Store a pollwaiter in a slot where we can find it later.
+	 *
+	 * Expands the pollset as required.  Must be called with the driver locked.
+	 *
+	 * @return		OK, or -errno on error.
+	 */
+	int		store_poll_waiter(struct pollfd *fds);
+
+	/**
+	 * Remove a poll waiter.
+	 *
+	 * @return		OK, or -errno on error.
+	 */
+	int		remove_poll_waiter(struct pollfd *fds);
+};
+
+/**
+ * Abstract class for character device accessed via PIO
+ */
+class __EXPORT PIO : public CDev
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param base		Base address of the device PIO area
+	 * @param irq		Interrupt assigned to the device (or zero if none)
+	 */
+	PIO(const char *name,
+	    const char *devname,
+	    uint32_t base,
+	    int irq = 0);
+	~PIO();
+
+	int		init();
+
+protected:
+
+	/**
+	 * Read a register
+	 *
+	 * @param offset	Register offset in bytes from the base address.
+	 */
+	uint32_t reg(uint32_t offset) {
+		return *(volatile uint32_t *)(_base + offset);
+	}
+
+	/**
+	 * Write a register
+	 *
+	 * @param offset	Register offset in bytes from the base address.
+	 * @param value	Value to write.
+	 */
+	void		reg(uint32_t offset, uint32_t value) {
+		*(volatile uint32_t *)(_base + offset) = value;
+	}
+
+	/**
+	 * Modify a register
+	 *
+	 * Note that there is a risk of a race during the read/modify/write cycle
+	 * that must be taken care of by the caller.
+	 *
+	 * @param offset	Register offset in bytes from the base address.
+	 * @param clearbits	Bits to clear in the register
+	 * @param setbits	Bits to set in the register
+	 */
+	void		modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) {
+		uint32_t val = reg(offset);
+		val &= ~clearbits;
+		val |= setbits;
+		reg(offset, val);
+	}
+
+private:
+	uint32_t	_base;
+};
+
+} // namespace device
+
+#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file
diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp
new file mode 100644
index 000000000..a416801eb
--- /dev/null
+++ b/src/drivers/device/i2c.cpp
@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.cpp
+ *
+ * Base class for devices attached via the I2C bus.
+ *
+ * @todo Bus frequency changes; currently we do nothing with the value
+ *       that is supplied.  Should we just depend on the bus knowing?
+ */
+
+#include "i2c.h"
+
+namespace device
+{
+
+I2C::I2C(const char *name,
+	 const char *devname,
+	 int bus,
+	 uint16_t address,
+	 uint32_t frequency,
+	 int irq) :
+	// base class
+	CDev(name, devname, irq),
+	// public
+	// protected
+	_retries(0),
+	// private
+	_bus(bus),
+	_address(address),
+	_frequency(frequency),
+	_dev(nullptr)
+{
+}
+
+I2C::~I2C()
+{
+	if (_dev)
+		up_i2cuninitialize(_dev);
+}
+
+int
+I2C::init()
+{
+	int ret = OK;
+
+	/* attach to the i2c bus */
+	_dev = up_i2cinitialize(_bus);
+
+	if (_dev == nullptr) {
+		debug("failed to init I2C");
+		ret = -ENOENT;
+		goto out;
+	}
+
+	// call the probe function to check whether the device is present
+	ret = probe();
+
+	if (ret != OK) {
+		debug("probe failed");
+		goto out;
+	}
+
+	// do base class init, which will create device node, etc
+	ret = CDev::init();
+
+	if (ret != OK) {
+		debug("cdev init failed");
+		goto out;
+	}
+
+	// tell the world where we are
+	log("on I2C bus %d at 0x%02x", _bus, _address);
+
+out:
+	return ret;
+}
+
+int
+I2C::probe()
+{
+	// Assume the device is too stupid to be discoverable.
+	return OK;
+}
+
+int
+I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	struct i2c_msg_s msgv[2];
+	unsigned msgs;
+	int ret;
+	unsigned retry_count = 0;
+
+	do {
+		//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+
+		msgs = 0;
+
+		if (send_len > 0) {
+			msgv[msgs].addr = _address;
+			msgv[msgs].flags = 0;
+			msgv[msgs].buffer = const_cast<uint8_t *>(send);
+			msgv[msgs].length = send_len;
+			msgs++;
+		}
+
+		if (recv_len > 0) {
+			msgv[msgs].addr = _address;
+			msgv[msgs].flags = I2C_M_READ;
+			msgv[msgs].buffer = recv;
+			msgv[msgs].length = recv_len;
+			msgs++;
+		}
+
+		if (msgs == 0)
+			return -EINVAL;
+
+		/*
+		 * I2C architecture means there is an unavoidable race here
+		 * if there are any devices on the bus with a different frequency
+		 * preference.  Really, this is pointless.
+		 */
+		I2C_SETFREQUENCY(_dev, _frequency);
+		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
+
+		/* success */
+		if (ret == OK)
+			break;
+
+		/* if we have already retried once, or we are going to give up, then reset the bus */
+		if ((retry_count >= 1) || (retry_count >= _retries))
+			up_i2creset(_dev);
+
+	} while (retry_count++ < _retries);
+
+	return ret;
+
+}
+
+int
+I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
+{
+	int ret;
+	unsigned retry_count = 0;
+
+	/* force the device address into the message vector */
+	for (unsigned i = 0; i < msgs; i++)
+		msgv[i].addr = _address;
+
+
+	do {
+		/*
+		 * I2C architecture means there is an unavoidable race here
+		 * if there are any devices on the bus with a different frequency
+		 * preference.  Really, this is pointless.
+		 */
+		I2C_SETFREQUENCY(_dev, _frequency);
+		ret = I2C_TRANSFER(_dev, msgv, msgs);
+
+		/* success */
+		if (ret == OK)
+			break;
+
+		/* if we have already retried once, or we are going to give up, then reset the bus */
+		if ((retry_count >= 1) || (retry_count >= _retries))
+			up_i2creset(_dev);
+
+	} while (retry_count++ < _retries);
+
+	return ret;
+}
+
+} // namespace device
\ No newline at end of file
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
new file mode 100644
index 000000000..b4a9cdd53
--- /dev/null
+++ b/src/drivers/device/i2c.h
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   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.h
+ *
+ * Base class for devices connected via I2C.
+ */
+
+#ifndef _DEVICE_I2C_H
+#define _DEVICE_I2C_H
+
+#include "device.h"
+
+#include <nuttx/i2c.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on I2C
+ */
+class __EXPORT I2C : public CDev
+{
+
+public:
+
+    /**
+     * Get the address
+     */
+    uint16_t get_address() {
+      return _address;
+    }
+
+protected:
+	/**
+	 * The number of times a read or write operation will be retried on
+	 * error.
+	 */
+	unsigned		_retries;
+
+	/**
+	 * The I2C bus number the device is attached to.
+	 */
+	int			_bus;
+
+	/**
+	 * @ Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		I2C bus on which the device lives
+	 * @param address	I2C bus address, or zero if set_address will be used
+	 * @param frequency	I2C bus frequency for the device (currently not used)
+	 * @param irq		Interrupt assigned to the device (or zero if none)
+	 */
+	I2C(const char *name,
+	    const char *devname,
+	    int bus,
+	    uint16_t address,
+	    uint32_t frequency,
+	    int irq = 0);
+	~I2C();
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe();
+
+	/**
+	 * Perform an I2C transaction to the device.
+	 *
+	 * At least one of send_len and recv_len must be non-zero.
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(const uint8_t *send, unsigned send_len,
+				 uint8_t *recv, unsigned recv_len);
+
+	/**
+	 * Perform a multi-part I2C transaction to the device.
+	 *
+	 * @param msgv		An I2C message vector.
+	 * @param msgs		The number of entries in the message vector.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(i2c_msg_s *msgv, unsigned msgs);
+
+	/**
+	 * Change the bus address.
+	 *
+	 * Most often useful during probe() when the driver is testing
+	 * several possible bus addresses.
+	 *
+	 * @param address	The new bus address to set.
+	 */
+	void		set_address(uint16_t address) {
+		_address = address;
+	}
+
+private:
+	uint16_t		_address;
+	uint32_t		_frequency;
+	struct i2c_dev_s	*_dev;
+};
+
+} // namespace device
+
+#endif /* _DEVICE_I2C_H */
diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk
new file mode 100644
index 000000000..8c716d9cd
--- /dev/null
+++ b/src/drivers/device/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build the device driver framework.
+#
+
+SRCS		= cdev.cpp \
+		  device.cpp \
+		  i2c.cpp \
+		  pio.cpp \
+		  spi.cpp
diff --git a/src/drivers/device/pio.cpp b/src/drivers/device/pio.cpp
new file mode 100644
index 000000000..f3a805a5e
--- /dev/null
+++ b/src/drivers/device/pio.cpp
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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 pio.cpp
+ *
+ * Base class for devices accessed via PIO to registers.
+ */
+
+#include "device.h"
+
+namespace device
+{
+
+PIO::PIO(const char *name,
+	 const char *devname,
+	 uint32_t base,
+	 int irq) :
+	// base class
+	CDev(name, devname, irq),
+	// public
+	// protected
+	// private
+	_base(base)
+{
+}
+
+PIO::~PIO()
+{
+	// nothing to do here...
+}
+
+int
+PIO::init()
+{
+	int ret = OK;
+
+	// base class init first
+	ret = CDev::init();
+
+	return ret;
+}
+
+} // namespace device
\ No newline at end of file
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
new file mode 100644
index 000000000..8fffd60cb
--- /dev/null
+++ b/src/drivers/device/spi.cpp
@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   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 spi.cpp
+ *
+ * Base class for devices connected via SPI.
+ *
+ * @todo Work out if caching the mode/frequency would save any time.
+ *
+ * @todo A separate bus/device abstraction would allow for mixed interrupt-mode
+ * and non-interrupt-mode clients to arbitrate for the bus.  As things stand,
+ * a bus shared between clients of both kinds is vulnerable to races between
+ * the two, where an interrupt-mode client will ignore the lock held by the
+ * non-interrupt-mode client.
+ */
+
+#include <nuttx/arch.h>
+
+#include "spi.h"
+
+#ifndef CONFIG_SPI_EXCHANGE
+# error This driver requires CONFIG_SPI_EXCHANGE
+#endif
+
+namespace device
+{
+
+SPI::SPI(const char *name,
+	 const char *devname,
+	 int bus,
+	 enum spi_dev_e device,
+	 enum spi_mode_e mode,
+	 uint32_t frequency,
+	 int irq) :
+	// base class
+	CDev(name, devname, irq),
+	// public
+	// protected
+	// private
+	_bus(bus),
+	_device(device),
+	_mode(mode),
+	_frequency(frequency),
+	_dev(nullptr)
+{
+}
+
+SPI::~SPI()
+{
+	// XXX no way to let go of the bus...
+}
+
+int
+SPI::init()
+{
+	int ret = OK;
+
+	/* attach to the spi bus */
+	if (_dev == nullptr)
+		_dev = up_spiinitialize(_bus);
+
+	if (_dev == nullptr) {
+		debug("failed to init SPI");
+		ret = -ENOENT;
+		goto out;
+	}
+
+	/* deselect device to ensure high to low transition of pin select */
+	SPI_SELECT(_dev, _device, false);
+
+	/* call the probe function to check whether the device is present */
+	ret = probe();
+
+	if (ret != OK) {
+		debug("probe failed");
+		goto out;
+	}
+
+	/* do base class init, which will create the device node, etc. */
+	ret = CDev::init();
+
+	if (ret != OK) {
+		debug("cdev init failed");
+		goto out;
+	}
+
+	/* tell the workd where we are */
+	log("on SPI bus %d at %d", _bus, _device);
+
+out:
+	return ret;
+}
+
+int
+SPI::probe()
+{
+	// assume the device is too stupid to be discoverable
+	return OK;
+}
+
+int
+SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+
+	if ((send == nullptr) && (recv == nullptr))
+		return -EINVAL;
+
+	/* do common setup */
+	if (!up_interrupt_context())
+		SPI_LOCK(_dev, true);
+
+	SPI_SETFREQUENCY(_dev, _frequency);
+	SPI_SETMODE(_dev, _mode);
+	SPI_SETBITS(_dev, 8);
+	SPI_SELECT(_dev, _device, true);
+
+	/* do the transfer */
+	SPI_EXCHANGE(_dev, send, recv, len);
+
+	/* and clean up */
+	SPI_SELECT(_dev, _device, false);
+
+	if (!up_interrupt_context())
+		SPI_LOCK(_dev, false);
+
+	return OK;
+}
+
+} // namespace device
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
new file mode 100644
index 000000000..d2d01efb3
--- /dev/null
+++ b/src/drivers/device/spi.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   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 spi.h
+ *
+ * Base class for devices connected via SPI.
+ */
+
+#ifndef _DEVICE_SPI_H
+#define _DEVICE_SPI_H
+
+#include "device.h"
+
+#include <nuttx/spi.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on SPI
+ */
+class __EXPORT SPI : public CDev
+{
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		SPI bus on which the device lives
+	 * @param device	Device handle (used by SPI_SELECT)
+	 * @param mode		SPI clock/data mode
+	 * @param frequency	SPI clock frequency
+	 * @param irq		Interrupt assigned to the device (or zero if none)
+	 */
+	SPI(const char *name,
+	    const char *devname,
+	    int bus,
+	    enum spi_dev_e device,
+	    enum spi_mode_e mode,
+	    uint32_t frequency,
+	    int irq = 0);
+	~SPI();
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe();
+
+	/**
+	 * Perform a SPI transfer.
+	 *
+	 * If called from interrupt context, this interface does not lock
+	 * the bus and may interfere with non-interrupt-context callers.
+	 *
+	 * Clients in a mixed interrupt/non-interrupt configuration must
+	 * ensure appropriate interlocking.
+	 *
+	 * At least one of send or recv must be non-null.
+	 *
+	 * @param send		Bytes to send to the device, or nullptr if
+	 *			no data is to be sent.
+	 * @param recv		Buffer for receiving bytes from the device,
+	 *			or nullptr if no bytes are to be received.
+	 * @param len		Number of bytes to transfer.
+	 * @return		OK if the exchange was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
+
+private:
+	int			_bus;
+	enum spi_dev_e		_device;
+	enum spi_mode_e		_mode;
+	uint32_t		_frequency;
+	struct spi_dev_s	*_dev;
+};
+
+} // namespace device
+
+#endif /* _DEVICE_SPI_H */
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
new file mode 100644
index 000000000..794de584b
--- /dev/null
+++ b/src/drivers/drv_accel.h
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   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 Accelerometer driver interface.
+ */
+
+#ifndef _DRV_ACCEL_H
+#define _DRV_ACCEL_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define ACCEL_DEVICE_PATH	"/dev/accel"
+
+/**
+ * accel report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct accel_report {
+	uint64_t timestamp;
+	float x;		/**< acceleration in the NED X board axis in m/s^2 */
+	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
+	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
+	float temperature;	/**< temperature in degrees celsius */
+	float range_m_s2;	/**< range in m/s^2 (+- this value) */
+	float scaling;
+
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
+	int16_t temperature_raw;
+};
+
+/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct accel_scale {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
+
+/*
+ * ObjDev tag for raw accelerometer data.
+ */
+ORB_DECLARE(sensor_accel);
+
+/*
+ * ioctl() definitions
+ *
+ * Accelerometer drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _ACCELIOCBASE		(0x2100)
+#define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
+
+
+/** set the accel internal sample rate to at least (arg) Hz */
+#define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
+
+/** return the accel internal sample rate in Hz */
+#define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
+
+/** set the accel internal lowpass filter to no lower than (arg) Hz */
+#define ACCELIOCSLOWPASS	_ACCELIOC(2)
+
+/** return the accel internal lowpass filter in Hz */
+#define ACCELIOCGLOWPASS	_ACCELIOC(3)
+
+/** set the accel scaling constants to the structure pointed to by (arg) */
+#define ACCELIOCSSCALE		_ACCELIOC(5)
+
+/** get the accel scaling constants into the structure pointed to by (arg) */
+#define ACCELIOCGSCALE		_ACCELIOC(6)
+
+/** set the accel measurement range to handle at least (arg) g */
+#define ACCELIOCSRANGE		_ACCELIOC(7)
+
+/** get the current accel measurement range in g */
+#define ACCELIOCGRANGE		_ACCELIOC(8)
+
+/** get the result of a sensor self-test */
+#define ACCELIOCSELFTEST	_ACCELIOC(9)
+
+#endif /* _DRV_ACCEL_H */
diff --git a/src/drivers/drv_adc.h b/src/drivers/drv_adc.h
new file mode 100644
index 000000000..8ec6d1233
--- /dev/null
+++ b/src/drivers/drv_adc.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   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_adc.h
+ *
+ * ADC driver interface.
+ *
+ * This defines additional operations over and above the standard NuttX
+ * ADC API.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define ADC_DEVICE_PATH		"/dev/adc0"
+
+/*
+ * ioctl definitions
+ */
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
new file mode 100644
index 000000000..bffc35c62
--- /dev/null
+++ b/src/drivers/drv_airspeed.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 Airspeed driver interface.
+ * @author Simon Wilks
+ */
+
+#ifndef _DRV_AIRSPEED_H
+#define _DRV_AIRSPEED_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define AIRSPEED_DEVICE_PATH	"/dev/airspeed"
+
+/*
+ * ioctl() definitions
+ *
+ * Airspeed drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _AIRSPEEDIOCBASE		(0x7700)
+#define __AIRSPEEDIOC(_n)		(_IOC(_AIRSPEEDIOCBASE, _n))
+
+
+#endif /* _DRV_AIRSPEED_H */
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
new file mode 100644
index 000000000..176f798c0
--- /dev/null
+++ b/src/drivers/drv_baro.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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 Barometric pressure sensor driver interface.
+ */
+
+#ifndef _DRV_BARO_H
+#define _DRV_BARO_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define BARO_DEVICE_PATH	"/dev/baro"
+
+/**
+ * baro report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct baro_report {
+	float pressure;
+	float altitude;
+	float temperature;
+	uint64_t timestamp;
+};
+
+/*
+ * ObjDev tag for raw barometer data.
+ */
+ORB_DECLARE(sensor_baro);
+
+/*
+ * ioctl() definitions
+ */
+
+#define _BAROIOCBASE		(0x2200)
+#define _BAROIOC(_n)		(_IOC(_BAROIOCBASE, _n))
+
+/** set corrected MSL pressure in pascals */
+#define BAROIOCSMSLPRESSURE	_BAROIOC(0)
+
+/** get current MSL pressure in pascals */
+#define BAROIOCGMSLPRESSURE	_BAROIOC(1)
+
+#endif /* _DRV_BARO_H */
diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h
new file mode 100644
index 000000000..9c278f6c5
--- /dev/null
+++ b/src/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/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
new file mode 100644
index 000000000..92d184326
--- /dev/null
+++ b/src/drivers/drv_gpio.h
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   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_gpio.h
+ *
+ * Generic GPIO ioctl interface.
+ */
+
+#ifndef _DRV_GPIO_H
+#define _DRV_GPIO_H
+
+#include <sys/ioctl.h>
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU
+/*
+ * PX4FMU GPIO numbers.
+ *
+ * For shared pins, alternate function 1 selects the non-GPIO mode 
+ * (USART2, CAN2, etc.)
+ */
+# define GPIO_EXT_1		(1<<0)		/**< high-power GPIO 1 */
+# define GPIO_EXT_2		(1<<1)		/**< high-power GPIO 1 */
+# define GPIO_MULTI_1		(1<<2)		/**< USART2 CTS */
+# define GPIO_MULTI_2		(1<<3)		/**< USART2 RTS */
+# define GPIO_MULTI_3		(1<<4)		/**< USART2 TX */
+# define GPIO_MULTI_4		(1<<5)		/**< USART2 RX */
+# define GPIO_CAN_TX		(1<<6)		/**< CAN2 TX */
+# define GPIO_CAN_RX		(1<<7)		/**< CAN2 RX */
+
+/**
+ * Default GPIO device - other devices may also support this protocol if
+ * they also export GPIO-like things.  This is always the GPIOs on the
+ * main board.
+ */
+# define GPIO_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO
+/*
+ * PX4IO GPIO numbers.
+ *
+ * XXX note that these are here for reference/future use; currently
+ * there is no good way to wire these up without a common STM32 GPIO
+ * driver, which isn't implemented yet.
+ */
+/* outputs */
+# define GPIO_ACC1_POWER	(1<<0)		/**< accessory power 1 */
+# define GPIO_ACC2_POWER	(1<<1)		/**< accessory power 2 */
+# define GPIO_SERVO_POWER	(1<<2)		/**< servo power */
+# define GPIO_RELAY1		(1<<3)		/**< relay 1 */
+# define GPIO_RELAY2		(1<<4)		/**< relay 2 */
+# define GPIO_LED_BLUE		(1<<5)		/**< blue LED */
+# define GPIO_LED_AMBER		(1<<6)		/**< amber/red LED */
+# define GPIO_LED_SAFETY	(1<<7)		/**< safety LED */
+
+/* inputs */
+# define GPIO_ACC_OVERCURRENT	(1<<8)		/**< accessory 1/2 overcurrent detect */
+# define GPIO_SERVO_OVERCURRENT	(1<<9)		/**< servo overcurrent detect */
+# define GPIO_SAFETY_BUTTON	(1<<10)		/**< safety button pressed */
+
+/**
+ * Default GPIO device - other devices may also support this protocol if
+ * they also export GPIO-like things.  This is always the GPIOs on the
+ * main board.
+ */
+# define GPIO_DEVICE_PATH	"/dev/px4io"
+
+#endif
+
+#ifndef GPIO_DEVICE_PATH
+#  error No GPIO support for this board.
+#endif
+
+/*
+ * IOCTL definitions.
+ *
+ * For all ioctls, the (arg) argument is a bitmask of GPIOs to be affected
+ * by the operation, with the LSB being the lowest-numbered GPIO.
+ *
+ * Note that there may be board-specific relationships between GPIOs;
+ * applications using GPIOs should be aware of this.
+ */
+#define _GPIOCBASE	0x2700
+#define GPIOC(_x)	_IOC(_GPIOCBASE, _x)
+
+/** reset all board GPIOs to their default state */
+#define GPIO_RESET	GPIOC(0)
+
+/** configure the board GPIOs in (arg) as outputs */
+#define GPIO_SET_OUTPUT	GPIOC(1)
+
+/** configure the board GPIOs in (arg) as inputs */
+#define GPIO_SET_INPUT	GPIOC(2)
+
+/** configure the board GPIOs in (arg) for the first alternate function (if supported) */
+#define GPIO_SET_ALT_1	GPIOC(3)
+
+/** configure the board GPIO (arg) for the second alternate function (if supported) */
+#define GPIO_SET_ALT_2	GPIOC(4)
+
+/** configure the board GPIO (arg) for the third alternate function (if supported) */
+#define GPIO_SET_ALT_3	GPIOC(5)
+
+/** configure the board GPIO (arg) for the fourth alternate function (if supported) */
+#define GPIO_SET_ALT_4	GPIOC(6)
+
+/** set the GPIOs in (arg) */
+#define GPIO_SET	GPIOC(10)
+
+/** clear the GPIOs in (arg) */
+#define GPIO_CLEAR	GPIOC(11)
+
+/** read all the GPIOs and return their values in *(uint32_t *)arg */
+#define GPIO_GET	GPIOC(12)
+
+#endif /* _DRV_GPIO_H */
\ No newline at end of file
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
new file mode 100644
index 000000000..1dda8ef0b
--- /dev/null
+++ b/src/drivers/drv_gps.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 GPS driver interface.
+ */
+
+#ifndef _DRV_GPS_H
+#define _DRV_GPS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+
+#define GPS_DEVICE_PATH	"/dev/gps"
+
+typedef enum {
+	GPS_DRIVER_MODE_NONE = 0,
+	GPS_DRIVER_MODE_UBX,
+	GPS_DRIVER_MODE_MTK,
+	GPS_DRIVER_MODE_NMEA,
+} gps_driver_mode_t;
+
+
+/*
+ * ObjDev tag for GPS data.
+ */
+ORB_DECLARE(gps);
+
+/*
+ * ioctl() definitions
+ */
+#define _GPSIOCBASE			(0x2800)            //TODO: arbitrary choice...
+#define _GPSIOC(_n)		(_IOC(_GPSIOCBASE, _n))
+
+#endif /* _DRV_GPS_H */
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
new file mode 100644
index 000000000..1d0fef6fc
--- /dev/null
+++ b/src/drivers/drv_gyro.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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 Gyroscope driver interface.
+ */
+
+#ifndef _DRV_GYRO_H
+#define _DRV_GYRO_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define GYRO_DEVICE_PATH	"/dev/gyro"
+
+/**
+ * gyro report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct gyro_report {
+	uint64_t timestamp;
+	float x;		/**< angular velocity in the NED X board axis in rad/s */
+	float y;		/**< angular velocity in the NED Y board axis in rad/s */
+	float z;		/**< angular velocity in the NED Z board axis in rad/s */
+	float temperature;	/**< temperature in degrees celcius */
+	float range_rad_s;
+	float scaling;
+
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
+	int16_t temperature_raw;
+};
+
+/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct gyro_scale {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
+
+/*
+ * ObjDev tag for raw gyro data.
+ */
+ORB_DECLARE(sensor_gyro);
+
+/*
+ * ioctl() definitions
+ */
+
+#define _GYROIOCBASE		(0x2300)
+#define _GYROIOC(_n)		(_IOC(_GYROIOCBASE, _n))
+
+/** set the gyro internal sample rate to at least (arg) Hz */
+#define GYROIOCSSAMPLERATE	_GYROIOC(0)
+
+/** return the gyro internal sample rate in Hz */
+#define GYROIOCGSAMPLERATE	_GYROIOC(1)
+
+/** set the gyro internal lowpass filter to no lower than (arg) Hz */
+#define GYROIOCSLOWPASS		_GYROIOC(2)
+
+/** set the gyro internal lowpass filter to no lower than (arg) Hz */
+#define GYROIOCGLOWPASS		_GYROIOC(3)
+
+/** set the gyro scaling constants to (arg) */
+#define GYROIOCSSCALE		_GYROIOC(4)
+
+/** get the gyro scaling constants into (arg) */
+#define GYROIOCGSCALE		_GYROIOC(5)
+
+/** set the gyro measurement range to handle at least (arg) degrees per second */
+#define GYROIOCSRANGE		_GYROIOC(6)
+
+/** get the current gyro measurement range in degrees per second */
+#define GYROIOCGRANGE		_GYROIOC(7)
+
+/** check the status of the sensor */
+#define GYROIOCSELFTEST		_GYROIOC(8)
+
+#endif /* _DRV_GYRO_H */
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
new file mode 100644
index 000000000..8a99eeca7
--- /dev/null
+++ b/src/drivers/drv_hrt.h
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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_hrt.h
+ *
+ * High-resolution timer with callouts and timekeeping.
+ */
+
+#pragma once
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <time.h>
+#include <queue.h>
+
+__BEGIN_DECLS
+
+/*
+ * Absolute time, in microsecond units.
+ *
+ * Absolute time is measured from some arbitrary epoch shortly after
+ * system startup.  It should never wrap or go backwards.
+ */
+typedef uint64_t	hrt_abstime;
+
+/*
+ * Callout function type.
+ *
+ * Note that callouts run in the timer interrupt context, so
+ * they are serialised with respect to each other, and must not
+ * block.
+ */
+typedef void	(* hrt_callout)(void *arg);
+
+/*
+ * Callout record.
+ */
+typedef struct hrt_call {
+	struct sq_entry_s	link;
+
+	hrt_abstime		deadline;
+	hrt_abstime		period;
+	hrt_callout		callout;
+	void			*arg;
+} *hrt_call_t;
+
+/*
+ * Get absolute time.
+ */
+__EXPORT extern hrt_abstime hrt_absolute_time(void);
+
+/*
+ * Convert a timespec to absolute time.
+ */
+__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
+
+/*
+ * Convert absolute time to a timespec.
+ */
+__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+
+/*
+ * Compute the delta between a timestamp taken in the past
+ * and now.
+ *
+ * This function is safe to use even if the timestamp is updated
+ * by an interrupt during execution.
+ */
+__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+__EXPORT extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+__EXPORT extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+__EXPORT extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
+
+/*
+ * If this returns true, the entry has been invoked and removed from the callout list,
+ * or it has never been entered.
+ *
+ * Always returns false for repeating callouts.
+ */
+__EXPORT extern bool	hrt_called(struct hrt_call *entry);
+
+/*
+ * Remove the entry from the callout list.
+ */
+__EXPORT extern void	hrt_cancel(struct hrt_call *entry);
+
+/*
+ * Initialise the HRT.
+ */
+__EXPORT extern void	hrt_init(void);
+
+__END_DECLS
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
new file mode 100644
index 000000000..21044f620
--- /dev/null
+++ b/src/drivers/drv_led.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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_led.h
+ *
+ * LED driver API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define LED_DEVICE_PATH		"/dev/led"
+
+#define _LED_BASE		0x2800
+
+/* PX4 LED colour codes */
+#define LED_AMBER		1
+#define LED_RED			1	/* some boards have red rather than amber */
+#define LED_BLUE		0
+#define LED_SAFETY		2
+
+#define LED_ON			_IOC(_LED_BASE, 0)
+#define LED_OFF			_IOC(_LED_BASE, 1)
+
+__BEGIN_DECLS
+
+/*
+ * Initialise the LED driver.
+ */
+__EXPORT extern void drv_led_start(void);
+
+__END_DECLS
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
new file mode 100644
index 000000000..9aab995a1
--- /dev/null
+++ b/src/drivers/drv_mag.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   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 Magnetometer driver interface.
+ */
+
+#ifndef _DRV_MAG_H
+#define _DRV_MAG_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define MAG_DEVICE_PATH		"/dev/mag"
+
+/**
+ * mag report structure.  Reads from the device must be in multiples of this
+ * structure.
+ *
+ * Output values are in gauss.
+ */
+struct mag_report {
+	uint64_t timestamp;
+	float x;
+	float y;
+	float z;
+	float range_ga;
+	float scaling;
+
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
+};
+
+/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct mag_scale {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
+
+/*
+ * ObjDev tag for raw magnetometer data.
+ */
+ORB_DECLARE(sensor_mag);
+
+/*
+ * ioctl() definitions
+ */
+
+#define _MAGIOCBASE		(0x2400)
+#define _MAGIOC(_n)		(_IOC(_MAGIOCBASE, _n))
+
+/** set the mag internal sample rate to at least (arg) Hz */
+#define MAGIOCSSAMPLERATE	_MAGIOC(0)
+
+/** set the mag internal lowpass filter to no lower than (arg) Hz */
+#define MAGIOCSLOWPASS		_MAGIOC(1)
+
+/** set the mag scaling constants to the structure pointed to by (arg) */
+#define MAGIOCSSCALE		_MAGIOC(2)
+
+/** copy the mag scaling constants to the structure pointed to by (arg) */
+#define MAGIOCGSCALE		_MAGIOC(3)
+
+/** set the measurement range to handle (at least) arg Gauss */
+#define MAGIOCSRANGE		_MAGIOC(4)
+
+/** perform self-calibration, update scale factors to canonical units */
+#define MAGIOCCALIBRATE		_MAGIOC(5)
+
+/** excite strap */
+#define MAGIOCEXSTRAP		_MAGIOC(6)
+
+/** perform self test and report status */
+#define MAGIOCSELFTEST		_MAGIOC(7)
+
+#endif /* _DRV_MAG_H */
diff --git a/src/drivers/drv_mixer.h b/src/drivers/drv_mixer.h
new file mode 100644
index 000000000..9f43015d9
--- /dev/null
+++ b/src/drivers/drv_mixer.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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_mixer.h
+ *
+ * Mixer ioctl interfaces.
+ *
+ * Normal workflow is:
+ *
+ * - open mixer device
+ * - add mixer(s)
+ * loop:
+ *  - mix actuators to array
+ *
+ * Each client has its own configuration.
+ *
+ * When mixing, outputs are produced by mixers in the order they are
+ * added.  A simple mixer produces one output; a multotor mixer will
+ * produce several outputs, etc.
+ */
+
+#ifndef _DRV_MIXER_H
+#define _DRV_MIXER_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define MIXER_DEVICE_PATH		"/dev/mixer"
+
+/*
+ * ioctl() definitions
+ */
+#define _MIXERIOCBASE		(0x2500)
+#define _MIXERIOC(_n)		(_IOC(_MIXERIOCBASE, _n))
+
+/** get the number of mixable outputs */
+#define MIXERIOCGETOUTPUTCOUNT	_MIXERIOC(0)
+
+/** reset (clear) the mixer configuration */
+#define MIXERIOCRESET		_MIXERIOC(1)
+
+/** simple channel scaler */
+struct mixer_scaler_s {
+	float			negative_scale;
+	float			positive_scale;
+	float			offset;
+	float			min_output;
+	float			max_output;
+};
+
+/** mixer input */
+struct mixer_control_s {
+	uint8_t			control_group;	/**< group from which the input reads */
+	uint8_t			control_index;	/**< index within the control group */
+	struct mixer_scaler_s 	scaler;		/**< scaling applied to the input before use */
+};
+
+/** simple mixer */
+struct mixer_simple_s {
+	uint8_t			control_count;	/**< number of inputs */
+	struct mixer_scaler_s	output_scaler;	/**< scaling for the output */
+	struct mixer_control_s	controls[0];	/**< actual size of the array is set by control_count */
+};
+
+#define MIXER_SIMPLE_SIZE(_icount)	(sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s))
+
+/**
+ * add a simple mixer in (struct mixer_simple_s *)arg
+ */
+#define MIXERIOCADDSIMPLE	_MIXERIOC(2)
+
+/* _MIXERIOC(3) was deprecated */
+/* _MIXERIOC(4) was deprecated */
+
+/**
+ * Add mixer(s) from the buffer in (const char *)arg
+ */
+#define MIXERIOCLOADBUF		_MIXERIOC(5)
+
+/*
+ * XXX Thoughts for additional operations:
+ *
+ * - get/set output scale, for tuning center/limit values.
+ * - save/serialise for saving tuned mixers.
+ */
+
+#endif /* _DRV_ACCEL_H */
diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
new file mode 100644
index 000000000..713618545
--- /dev/null
+++ b/src/drivers/drv_orb_dev.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef _DRV_UORB_H
+#define _DRV_UORB_H
+
+/**
+ * @file uORB published object driver.
+ */
+
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <stdint.h>
+
+/* XXX for ORB_DECLARE used in many drivers */
+#include "../modules/uORB/uORB.h"
+
+/*
+ * ioctl() definitions
+ */
+
+/** path to the uORB control device for pub/sub topics */
+#define TOPIC_MASTER_DEVICE_PATH	"/obj/_obj_"
+
+/** path to the uORB control device for parameter topics */
+#define PARAM_MASTER_DEVICE_PATH	"/param/_param_"
+
+/** maximum ogbject name length */
+#define ORB_MAXNAME		32
+
+#define _ORBIOCBASE		(0x2600)
+#define _ORBIOC(_n)		(_IOC(_ORBIOCBASE, _n))
+
+/*
+ * IOCTLs for the uORB control device
+ */
+
+/** Advertise a new topic described by *(uorb_metadata *)arg */
+#define ORBIOCADVERTISE		_ORBIOC(0)
+
+/*
+ * IOCTLs for individual topics.
+ */
+
+/** Fetch the time at which the topic was last updated into *(uint64_t *)arg */
+#define ORBIOCLASTUPDATE	_ORBIOC(10)
+
+/** Check whether the topic has been updated since it was last read, sets *(bool *)arg */
+#define ORBIOCUPDATED		_ORBIOC(11)
+
+/** Set the minimum interval at which the topic can be seen to be updated for this subscription */
+#define ORBIOCSETINTERVAL	_ORBIOC(12)
+
+/** Get the global advertiser handle for the topic */
+#define ORBIOCGADVERTISER	_ORBIOC(13)
+
+#endif /* _DRV_UORB_H */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
new file mode 100644
index 000000000..56af71059
--- /dev/null
+++ b/src/drivers/drv_pwm_output.h
@@ -0,0 +1,207 @@
+/****************************************************************************
+ *
+ *   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 PWM servo output interface.
+ *
+ * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
+ * pwm_output_values structure to the device, or by publishing to the
+ * output_pwm ORB topic.
+ * Writing a value of 0 to a channel suppresses any output for that
+ * channel.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+__BEGIN_DECLS
+
+/**
+ * Path for the default PWM output device.
+ *
+ * Note that on systems with more than one PWM output path (e.g.
+ * PX4FMU with PX4IO connected) there may be other devices that
+ * respond to this protocol.
+ */
+#define PWM_OUTPUT_DEVICE_PATH	"/dev/pwm_output"
+
+/**
+ * Maximum number of PWM output channels supported by the device.
+ */
+#define PWM_OUTPUT_MAX_CHANNELS	16
+
+/**
+ * Servo output signal type, value is actual servo output pulse
+ * width in microseconds.
+ */
+typedef uint16_t	servo_position_t;
+
+/**
+ * Servo output status structure.
+ *
+ * May be published to output_pwm, or written to a PWM output
+ * device.
+ */
+struct pwm_output_values {
+	/** desired pulse widths for each of the supported channels */
+	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
+};
+
+/*
+ * ORB tag for PWM outputs.
+ */
+ORB_DECLARE(output_pwm);
+
+/*
+ * ioctl() definitions
+ *
+ * Note that ioctls and ORB updates should not be mixed, as the
+ * behaviour of the system in this case is not defined.
+ */
+#define _PWM_SERVO_BASE		0x2a00
+
+/** arm all servo outputs handle by this driver */
+#define PWM_SERVO_ARM		_IOC(_PWM_SERVO_BASE, 0)
+
+/** disarm all servo outputs (stop generating pulses) */
+#define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
+
+/** set alternate servo update rate */
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
+/** get the number of servos in *(unsigned *)arg */
+#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
+
+/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
+#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+
+/** set the 'ARM ok' bit, which activates the safety switch */
+#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 5)
+
+/** clear the 'ARM ok' bit, which deactivates the safety switch */
+#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
+
+/** set a single servo to a specific value */
+#define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
+
+/** get a single specific servo value */
+#define PWM_SERVO_GET(_servo)	_IOC(_PWM_SERVO_BASE, 0x40 + _servo)
+
+/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
+ *  whose update rates must be the same.
+ */
+#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n)
+
+/*
+ * Low-level PWM output interface.
+ *
+ * This is the low-level API to the platform-specific PWM driver.
+ */
+
+/**
+ * Intialise the PWM servo outputs using the specified configuration.
+ *
+ * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
+ *			This allows some of the channels to remain configured
+ *			as GPIOs or as another function.
+ * @return		OK on success.
+ */
+__EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);
+
+/**
+ * De-initialise the PWM servo outputs.
+ */
+__EXPORT extern void	up_pwm_servo_deinit(void);
+
+/**
+ * Arm or disarm servo outputs.
+ *
+ * When disarmed, servos output no pulse.
+ *
+ * @bug This function should, but does not, guarantee that any pulse
+ *      currently in progress is cleanly completed.
+ *
+ * @param armed		If true, outputs are armed; if false they
+ *			are disarmed.
+ */
+__EXPORT extern void	up_pwm_servo_arm(bool armed);
+
+/**
+ * Set the servo update rate for all rate groups.
+ *
+ * @param rate		The update rate in Hz to set.
+ * @return		OK on success, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate(unsigned rate);
+
+/**
+ * Get a bitmap of output channels assigned to a given rate group.
+ *
+ * @param group		The rate group to query. Rate groups are assigned contiguously
+ *			starting from zero.
+ * @return		A bitmap of channels assigned to the rate group, or zero if
+ *			the group number has no channels.
+ */
+__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
+
+/**
+ * Set the update rate for a given rate group.
+ *
+ * @param group		The rate group whose update rate will be changed.
+ * @param rate		The update rate in Hz.
+ * @return		OK if the group was adjusted, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
+
+/**
+ * Set the current output value for a channel.
+ *
+ * @param channel	The channel to set.
+ * @param value		The output pulse width in microseconds.
+ */
+__EXPORT extern int	up_pwm_servo_set(unsigned channel, servo_position_t value);
+
+/**
+ * Get the current output value for a channel.
+ *
+ * @param channel	The channel to read.
+ * @return		The output pulse width in microseconds, or zero if
+ *			outputs are not armed or not configured.
+ */
+__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
+
+__END_DECLS
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
new file mode 100644
index 000000000..03a82ec5d
--- /dev/null
+++ b/src/drivers/drv_range_finder.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 Rangefinder driver interface.
+ */
+
+#ifndef _DRV_RANGEFINDER_H
+#define _DRV_RANGEFINDER_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
+
+/**
+ * range finder report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct range_finder_report {
+	uint64_t timestamp;
+	float distance; 			/** in meters */
+	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
+};
+
+/*
+ * ObjDev tag for raw range finder data.
+ */
+ORB_DECLARE(sensor_range_finder);
+
+/*
+ * ioctl() definitions
+ *
+ * Rangefinder drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _RANGEFINDERIOCBASE			(0x7900)
+#define __RANGEFINDERIOC(_n)		(_IOC(_RANGEFINDERIOCBASE, _n))
+
+/** set the minimum effective distance of the device */
+#define RANGEFINDERIOCSETMINIUMDISTANCE	__RANGEFINDERIOC(1)
+
+/** set the maximum effective distance of the device */
+#define RANGEFINDERIOCSETMAXIUMDISTANCE	__RANGEFINDERIOC(2)
+
+
+#endif /* _DRV_RANGEFINDER_H */
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
new file mode 100644
index 000000000..4decc324e
--- /dev/null
+++ b/src/drivers/drv_rc_input.h
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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 R/C input interface.
+ *
+ */
+
+#ifndef _DRV_RC_INPUT_H
+#define _DRV_RC_INPUT_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+/**
+ * Path for the default R/C input device.
+ *
+ * Note that on systems with more than one R/C input path (e.g.
+ * PX4FMU with PX4IO connected) there may be other devices that
+ * respond to this protocol.
+ *
+ * Input data may be obtained by subscribing to the input_rc
+ * object, or by poll/reading from the device.
+ */
+#define RC_INPUT_DEVICE_PATH	"/dev/input_rc"
+
+/**
+ * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
+ */
+#define RC_INPUT_MAX_CHANNELS	18
+
+/**
+ * Input signal type, value is a control position from zero to 100
+ * percent.
+ */
+typedef uint16_t		rc_input_t;
+
+enum RC_INPUT_SOURCE {
+	RC_INPUT_SOURCE_UNKNOWN = 0,
+	RC_INPUT_SOURCE_PX4FMU_PPM,
+	RC_INPUT_SOURCE_PX4IO_PPM,
+	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
+	RC_INPUT_SOURCE_PX4IO_SBUS
+};
+
+/**
+ * R/C input status structure.
+ *
+ * Published to input_rc, may also be published to other names depending
+ * on the board involved.
+ */
+struct rc_input_values {
+	/** decoding time */
+	uint64_t		timestamp;
+
+	/** number of channels actually being seen */
+	uint32_t		channel_count;
+
+	/** Input source */
+	enum RC_INPUT_SOURCE 	input_source;
+
+	/** measured pulse widths for each of the supported channels */
+	rc_input_t		values[RC_INPUT_MAX_CHANNELS];
+};
+
+/*
+ * ObjDev tag for R/C inputs.
+ */
+ORB_DECLARE(input_rc);
+
+#define _RC_INPUT_BASE		0x2b00
+
+/** Fetch R/C input values into (rc_input_values *)arg */
+
+#define RC_INPUT_GET			_IOC(_RC_INPUT_BASE, 0)
+
+
+#endif /* _DRV_RC_INPUT_H */
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
new file mode 100644
index 000000000..3a89cab9d
--- /dev/null
+++ b/src/drivers/drv_sensor.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 Common sensor API and ioctl definitions.
+ */
+
+#ifndef _DRV_SENSOR_H
+#define _DRV_SENSOR_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/*
+ * ioctl() definitions
+ *
+ * Note that a driver may not implement all of these operations, but
+ * if the operation is implemented it should conform to this API.
+ */
+
+#define _SENSORIOCBASE		(0x2000)
+#define _SENSORIOC(_n)		(_IOC(_SENSORIOCBASE, _n))
+
+/**
+ * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
+ * constants
+ */
+#define SENSORIOCSPOLLRATE	_SENSORIOC(0)
+
+/**
+ * Return the driver's approximate polling rate in Hz, or one of the
+ * SENSOR_POLLRATE values.
+ */
+#define SENSORIOCGPOLLRATE	_SENSORIOC(1)
+
+#define SENSOR_POLLRATE_MANUAL		1000000	/**< poll when read */
+#define SENSOR_POLLRATE_EXTERNAL	1000001	/**< poll when device signals ready */
+#define SENSOR_POLLRATE_MAX		1000002	/**< poll at device maximum rate */
+#define SENSOR_POLLRATE_DEFAULT		1000003	/**< poll at driver normal rate */
+
+/**
+ * Set the internal queue depth to (arg) entries, must be at least 1
+ *
+ * This sets the upper bound on the number of readings that can be
+ * read from the driver.
+ */
+#define SENSORIOCSQUEUEDEPTH	_SENSORIOC(2)
+
+/** return the internal queue depth */
+#define SENSORIOCGQUEUEDEPTH	_SENSORIOC(3)
+
+/**
+ * Reset the sensor to its default configuration.
+ */
+#define SENSORIOCRESET		_SENSORIOC(4)
+
+#endif /* _DRV_SENSOR_H */
\ No newline at end of file
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
new file mode 100644
index 000000000..0c6afc64c
--- /dev/null
+++ b/src/drivers/drv_tone_alarm.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.
+ *
+ ****************************************************************************/
+
+/*
+ * Driver for the PX4 audio alarm port, /dev/tone_alarm.
+ *
+ * The tone_alarm driver supports a set of predefined "alarm"
+ * patterns and one user-supplied pattern.  Patterns are ordered by
+ * priority, with a higher-priority pattern interrupting any
+ * lower-priority pattern that might be playing.
+ *
+ * The TONE_SET_ALARM ioctl can be used to select a predefined
+ * alarm pattern, from 1 - <TBD>.  Selecting pattern zero silences
+ * the alarm.
+ *
+ * To supply a custom pattern, write an array of 1 - <TBD> tone_note
+ * structures to /dev/tone_alarm.  The custom pattern has a priority
+ * of zero.
+ *
+ * Patterns will normally play once and then silence (if a pattern
+ * was overridden it will not resume).  A pattern may be made to
+ * repeat by inserting a note with the duration set to
+ * DURATION_REPEAT.  This pattern will loop until either a
+ * higher-priority pattern is started or pattern zero is requested
+ * via the ioctl.
+ */
+
+#ifndef DRV_TONE_ALARM_H_
+#define DRV_TONE_ALARM_H_
+
+#include <sys/ioctl.h>
+
+#define _TONE_ALARM_BASE	0x7400
+#define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
+
+/* structure describing one note in a tone pattern */
+struct tone_note {
+	uint8_t		pitch;
+	uint8_t		duration;	/* duration in multiples of 10ms */
+#define DURATION_END		0	/* ends the pattern */
+#define DURATION_REPEAT		255	/* resets the note counter to zero */
+};
+
+enum tone_pitch {
+	TONE_NOTE_E4,   /* E4 */
+	TONE_NOTE_F4,   /* F4 */
+	TONE_NOTE_F4S,  /* F#4/Gb4 */
+	TONE_NOTE_G4,   /* G4 */
+	TONE_NOTE_G4S,  /* G#4/Ab4 */
+	TONE_NOTE_A4,   /* A4 */
+	TONE_NOTE_A4S,  /* A#4/Bb4 */
+	TONE_NOTE_B4,   /* B4 */
+	TONE_NOTE_C5,   /* C5 */
+	TONE_NOTE_C5S,  /* C#5/Db5 */
+	TONE_NOTE_D5,   /* D5 */
+	TONE_NOTE_D5S,  /* D#5/Eb5 */
+	TONE_NOTE_E5,   /* E5 */
+	TONE_NOTE_F5,   /* F5 */
+	TONE_NOTE_F5S,  /* F#5/Gb5 */
+	TONE_NOTE_G5,   /* G5 */
+	TONE_NOTE_G5S,  /* G#5/Ab5 */
+	TONE_NOTE_A5,   /* A5 */
+	TONE_NOTE_A5S,  /* A#5/Bb5 */
+	TONE_NOTE_B5,   /* B5 */
+	TONE_NOTE_C6,   /* C6 */
+	TONE_NOTE_C6S,  /* C#6/Db6 */
+	TONE_NOTE_D6,   /* D6 */
+	TONE_NOTE_D6S,  /* D#6/Eb6 */
+	TONE_NOTE_E6,   /* E6 */
+	TONE_NOTE_F6,   /* F6 */
+	TONE_NOTE_F6S,  /* F#6/Gb6 */
+	TONE_NOTE_G6,   /* G6 */
+	TONE_NOTE_G6S,  /* G#6/Ab6 */
+	TONE_NOTE_A6,   /* A6 */
+	TONE_NOTE_A6S,  /* A#6/Bb6 */
+	TONE_NOTE_B6,   /* B6 */
+	TONE_NOTE_C7,   /* C7 */
+	TONE_NOTE_C7S,  /* C#7/Db7 */
+	TONE_NOTE_D7,   /* D7 */
+	TONE_NOTE_D7S,  /* D#7/Eb7 */
+	TONE_NOTE_E7,   /* E7 */
+	TONE_NOTE_F7,   /* F7 */
+	TONE_NOTE_F7S,  /* F#7/Gb7 */
+	TONE_NOTE_G7,   /* G7 */
+	TONE_NOTE_G7S,  /* G#7/Ab7 */
+	TONE_NOTE_A7,   /* A7 */
+	TONE_NOTE_A7S,  /* A#7/Bb7 */
+	TONE_NOTE_B7,   /* B7 */
+	TONE_NOTE_C8,   /* C8 */
+	TONE_NOTE_C8S,  /* C#8/Db8 */
+	TONE_NOTE_D8,   /* D8 */
+	TONE_NOTE_D8S,  /* D#8/Eb8 */
+
+	TONE_NOTE_SILENCE,
+	TONE_NOTE_MAX
+};
+
+#endif /* DRV_TONE_ALARM_H_ */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
new file mode 100644
index 000000000..e50395e47
--- /dev/null
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -0,0 +1,832 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 ets_airspeed.cpp
+ * @author Simon Wilks
+ *
+ * Driver for the Eagle Tree Airspeed V3 connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Default I2C bus */
+#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
+
+/* I2C bus address */
+#define I2C_ADDRESS		0x75	/* 7-bit address. 8-bit address is 0xEA */
+
+/* Register address */
+#define READ_CMD		0x07		/* Read the data */
+	 
+/**
+ * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. 
+ */
+#define MIN_ACCURATE_DIFF_PRES_PA 12
+
+/* Measurement rate is 100Hz */
+#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class ETSAirspeed : public device::I2C
+{
+public:
+	ETSAirspeed(int bus, int address = I2C_ADDRESS);
+	virtual ~ETSAirspeed();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+private:
+	work_s						_work;
+	unsigned					_num_reports;
+	volatile unsigned			_next_report;
+	volatile unsigned			_oldest_report;
+	differential_pressure_s		*_reports;
+	bool						_sensor_ok;
+	int							_measure_ticks;
+	bool						_collect_phase;
+	int 						_diff_pres_offset;
+	
+	orb_advert_t				_airspeed_pub;
+
+	perf_counter_t				_sample_perf;
+	perf_counter_t				_comms_errors;
+	perf_counter_t				_buffer_overflows;
+
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
+
+ETSAirspeed::ETSAirspeed(int bus, int address) :
+	I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_diff_pres_offset(0),
+	_airspeed_pub(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+ETSAirspeed::~ETSAirspeed()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+ETSAirspeed::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct differential_pressure_s[_num_reports];
+	for (unsigned i = 0; i < _num_reports; i++)
+		_reports[i].max_differential_pressure_pa = 0;
+
+	if (_reports == nullptr)
+		goto out;
+
+	_oldest_report = _next_report = 0;
+
+	/* get a publish handle on the airspeed topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
+
+	if (_airspeed_pub < 0)
+		debug("failed to create airspeed sensor object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+ETSAirspeed::probe()
+{
+	return measure();
+}
+
+int
+ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* add one to account for the sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+		
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct differential_pressure_s);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_oldest_report = _next_report = 0;
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+ETSAirspeed::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = READ_CMD;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+ETSAirspeed::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 2);
+	
+	if (ret < 0) {
+		log("error reading from sensor: %d", ret);
+		return ret;
+	}
+	
+	uint16_t diff_pres_pa = val[1] << 8 | val[0];
+
+	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
+	
+	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { 
+		diff_pres_pa = 0;
+	} else {
+		diff_pres_pa -= _diff_pres_offset;	
+	}
+
+    // XXX we may want to smooth out the readings to remove noise.
+
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
+
+	// Track maximum differential pressure measured (so we can work out top speed).
+	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
+		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	}
+
+	/* announce the airspeed if needed, just publish else */
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, toss it */
+	if (_next_report == _oldest_report) {
+		perf_count(_buffer_overflows);
+		INCREMENT(_oldest_report, _num_reports);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	
+	return ret;
+}
+
+void
+ETSAirspeed::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_DIFFPRESSURE};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+ETSAirspeed::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+ETSAirspeed::cycle_trampoline(void *arg)
+{
+	ETSAirspeed *dev = (ETSAirspeed *)arg;
+
+	dev->cycle();
+}
+
+void
+ETSAirspeed::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&ETSAirspeed::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&ETSAirspeed::cycle_trampoline,
+		   this,
+		   USEC2TICK(CONVERSION_INTERVAL));
+}
+
+void
+ETSAirspeed::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ets_airspeed
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+ETSAirspeed	*g_dev;
+
+void	start(int i2c_bus);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(int i2c_bus)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new ETSAirspeed(i2c_bus);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void
+stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct differential_pressure_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("diff pressure: %d pa", report.differential_pressure_pa);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("diff pressure: %d pa", report.differential_pressure_pa);
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+
+static void 
+ets_airspeed_usage() 
+{
+	fprintf(stderr, "usage: ets_airspeed [options] command\n");
+	fprintf(stderr, "options:\n");
+	fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
+	fprintf(stderr, "command:\n");
+	fprintf(stderr, "\tstart|stop|reset|test|info\n");
+}
+
+int
+ets_airspeed_main(int argc, char *argv[])
+{
+	int i2c_bus = PX4_I2C_BUS_DEFAULT;
+
+	int i;
+	for (i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+			if (argc > i + 1) {
+				i2c_bus = atoi(argv[i + 1]);
+			}
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		ets_airspeed::start(i2c_bus);
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 ets_airspeed::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		ets_airspeed::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		ets_airspeed::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		ets_airspeed::info();
+
+	ets_airspeed_usage();
+	exit(0);
+}
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
new file mode 100644
index 000000000..cb5d3b1ed
--- /dev/null
+++ b/src/drivers/ets_airspeed/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 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.
+#
+############################################################################
+
+#
+# Makefile to build the Eagle Tree Airspeed V3 driver.
+#
+
+MODULE_COMMAND		= ets_airspeed
+MODULE_STACKSIZE	= 1024
+
+SRCS			= ets_airspeed.cpp
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
new file mode 100644
index 000000000..38835418b
--- /dev/null
+++ b/src/drivers/gps/gps.cpp
@@ -0,0 +1,545 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 gps.cpp
+ * Driver for the GPS on a serial port
+ */
+
+#include <nuttx/clock.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/i2c.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/scheduling_priorities.h>
+#include <systemlib/err.h>
+#include <drivers/drv_gps.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include "ubx.h"
+#include "mtk.h"
+
+
+#define TIMEOUT_5HZ 500
+#define RATE_MEASUREMENT_PERIOD 5000000
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+
+
+class GPS : public device::CDev
+{
+public:
+	GPS(const char* uart_path);
+	virtual ~GPS();
+
+	virtual int			init();
+
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void				print_info();
+
+private:
+
+	bool				_task_should_exit;				///< flag to make the main worker task exit
+	int				_serial_fd;					///< serial interface to GPS
+	unsigned			_baudrate;					///< current baudrate
+	char				_port[20];					///< device / serial port path
+	volatile int			_task;						//< worker task
+	bool				_healthy;					///< flag to signal if the GPS is ok
+	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
+	bool				_mode_changed;					///< flag that the GPS mode has changed
+	gps_driver_mode_t		_mode;						///< current mode
+	GPS_Helper			*_Helper;					///< instance of GPS parser
+	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
+	orb_advert_t			_report_pub;					///< uORB pub for gps position
+	float				_rate;						///< position update rate
+
+
+	/**
+	 * Try to configure the GPS, handle outgoing communication to the GPS
+	 */
+	void				config();
+
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void			task_main_trampoline(void *arg);
+
+
+	/**
+	 * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
+	 */
+	void				task_main(void);
+
+	/**
+	 * Set the baudrate of the UART to the GPS
+	 */
+	int				set_baudrate(unsigned baud);
+
+	/**
+	 * Send a reset command to the GPS
+	 */
+	void				cmd_reset();
+
+};
+
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int gps_main(int argc, char *argv[]);
+
+namespace
+{
+
+GPS	*g_dev;
+
+}
+
+
+GPS::GPS(const char* uart_path) :
+	CDev("gps", GPS_DEVICE_PATH),
+	_task_should_exit(false),
+	_healthy(false),
+	_mode_changed(false),
+	_mode(GPS_DRIVER_MODE_UBX),
+	_Helper(nullptr),
+	_report_pub(-1),
+	_rate(0.0f)
+{
+	/* store port name */
+	strncpy(_port, uart_path, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	/* we need this potentially before it could be set in task_main */
+	g_dev = this;
+	memset(&_report, 0, sizeof(_report));
+
+	_debug_enabled = true;
+}
+
+GPS::~GPS()
+{
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
+
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
+	}
+
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
+	g_dev = nullptr;
+
+}
+
+int
+GPS::init()
+{
+	int ret = ERROR;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK)
+		goto out;
+
+	/* start the GPS driver worker task */
+	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+
+	if (_task < 0) {
+		warnx("task start failed: %d", errno);
+		return -errno;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	lock();
+
+	int ret = OK;
+
+	switch (cmd) {
+	case SENSORIOCRESET:
+		cmd_reset();
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+GPS::task_main_trampoline(void *arg)
+{
+	g_dev->task_main();
+}
+
+void
+GPS::task_main()
+{
+	log("starting");
+
+	/* open the serial port */
+	_serial_fd = ::open(_port, O_RDWR);
+
+	if (_serial_fd < 0) {
+		log("failed to open serial port: %s err: %d", _port, errno);
+		/* tell the dtor that we are exiting, set error code */
+		_task = -1;
+		_exit(1);
+	}
+
+	uint64_t last_rate_measurement = hrt_absolute_time();
+	unsigned last_rate_count = 0;
+
+	/* loop handling received serial bytes and also configuring in between */
+	while (!_task_should_exit) {
+
+		if (_Helper != nullptr) {
+			delete(_Helper);
+			/* set to zero to ensure parser is not used while not instantiated */
+			_Helper = nullptr;
+		}
+
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_Helper = new UBX(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_Helper = new MTK(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_NMEA:
+				//_Helper = new NMEA(); //TODO: add NMEA
+				break;
+			default:
+				break;
+		}
+		unlock();
+		if (_Helper->configure(_baudrate) == 0) {
+			unlock();
+
+			// GPS is obviously detected successfully, reset statistics
+			_Helper->reset_update_rates();
+
+			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+//				lock();
+				/* opportunistic publishing - else invalid data would end up on the bus */
+				if (_report_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+				} else {
+					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+				}
+
+				last_rate_count++;
+
+				/* measure update rate every 5 seconds */
+				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
+					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
+					last_rate_measurement = hrt_absolute_time();
+					last_rate_count = 0;
+					_Helper->store_update_rates();
+					_Helper->reset_update_rates();
+				}
+
+				if (!_healthy) {
+					warnx("module found");
+					_healthy = true;
+				}
+			}
+			if (_healthy) {
+				warnx("module lost");
+				_healthy = false;
+				_rate = 0.0f;
+			}
+
+			lock();
+		}
+		lock();
+
+		/* select next mode */
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_mode = GPS_DRIVER_MODE_MTK;
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_mode = GPS_DRIVER_MODE_UBX;
+				break;
+		//				case GPS_DRIVER_MODE_NMEA:
+		//					_mode = GPS_DRIVER_MODE_UBX;
+		//					break;
+			default:
+				break;
+		}
+
+	}
+	debug("exiting");
+
+	::close(_serial_fd);
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+
+
+void
+GPS::cmd_reset()
+{
+	//XXX add reset?
+}
+
+void
+GPS::print_info()
+{
+	switch (_mode) {
+		case GPS_DRIVER_MODE_UBX:
+			warnx("protocol: UBX");
+			break;
+		case GPS_DRIVER_MODE_MTK:
+			warnx("protocol: MTK");
+			break;
+		case GPS_DRIVER_MODE_NMEA:
+			warnx("protocol: NMEA");
+			break;
+		default:
+			break;
+	}
+	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+	if (_report.timestamp_position != 0) {
+		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
+			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
+		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
+		warnx("rate publication:\t%6.2f Hz", (double)_rate);
+
+	}
+
+	usleep(100000);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace gps
+{
+
+GPS	*g_dev;
+
+void	start(const char *uart_path);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *uart_path)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new GPS(uart_path);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
+		goto fail;
+	}
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver.
+ */
+void
+stop()
+{
+	delete g_dev;
+	g_dev = nullptr;
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	exit(0);
+}
+
+/**
+ * Print the status of the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+
+int
+gps_main(int argc, char *argv[])
+{
+
+	/* set to default */
+	char* device_name = GPS_DEFAULT_UART_PORT;
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		/* work around getopt unreliability */
+		if (argc > 3) {
+			if (!strcmp(argv[2], "-d")) {
+				device_name = argv[3];
+			} else {
+				goto out;
+			}
+		}
+		gps::start(device_name);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+		gps::stop();
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		gps::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		gps::reset();
+
+	/*
+	 * Print driver status.
+	 */
+	if (!strcmp(argv[1], "status"))
+		gps::info();
+
+out:
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
+}
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
new file mode 100644
index 000000000..ba86d370a
--- /dev/null
+++ b/src/drivers/gps/gps_helper.cpp
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#include <termios.h>
+#include <errno.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include "gps_helper.h"
+
+/**
+ * @file gps_helper.cpp
+ */
+
+float
+GPS_Helper::get_position_update_rate()
+{
+	return _rate_lat_lon;
+}
+
+float
+GPS_Helper::get_velocity_update_rate()
+{
+	return _rate_vel;
+}
+
+float
+GPS_Helper::reset_update_rates()
+{
+	_rate_count_vel = 0;
+	_rate_count_lat_lon = 0;
+	_interval_rate_start = hrt_absolute_time();
+}
+
+float
+GPS_Helper::store_update_rates()
+{
+	_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+	_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+}
+
+int
+GPS_Helper::set_baudrate(const int &fd, unsigned baud)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	warnx("try baudrate: %d\n", speed);
+
+	default:
+		warnx("ERROR: Unsupported baudrate: %d\n", baud);
+		return -EINVAL;
+	}
+	struct termios uart_config;
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERROR setting baudrate (tcsetattr)\n");
+		return -1;
+	}
+	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
+	return 0;
+}
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
new file mode 100644
index 000000000..defc1a074
--- /dev/null
+++ b/src/drivers/gps/gps_helper.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 gps_helper.h
+ */
+
+#ifndef GPS_HELPER_H
+#define GPS_HELPER_H
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+class GPS_Helper
+{
+public:
+	virtual int			configure(unsigned &baud) = 0;
+	virtual int 			receive(unsigned timeout) = 0;
+	int 				set_baudrate(const int &fd, unsigned baud);
+	float				get_position_update_rate();
+	float				get_velocity_update_rate();
+	float				reset_update_rates();
+	float				store_update_rates();
+
+protected:
+	uint8_t _rate_count_lat_lon;
+	uint8_t _rate_count_vel;
+
+	float _rate_lat_lon;
+	float _rate_vel;
+
+	uint64_t _interval_rate_start;
+};
+
+#endif /* GPS_HELPER_H */
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
new file mode 100644
index 000000000..097db2abf
--- /dev/null
+++ b/src/drivers/gps/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# GPS driver
+#
+
+MODULE_COMMAND	= gps
+
+SRCS		= gps.cpp \
+		  gps_helper.cpp \
+		  mtk.cpp \
+		  ubx.cpp
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
new file mode 100644
index 000000000..62941d74b
--- /dev/null
+++ b/src/drivers/gps/mtk.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 mkt.cpp */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "mtk.h"
+
+
+MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_mtk_revision(0)
+{
+	decode_init();
+}
+
+MTK::~MTK()
+{
+}
+
+int
+MTK::configure(unsigned &baudrate)
+{
+	/* set baudrate first */
+	if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
+		return -1;
+
+	baudrate = MTK_BAUDRATE;
+
+	/* Write config messages, don't wait for an answer */
+	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int
+MTK::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+	gps_mtk_packet_t packet;
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* first read whatever is left */
+		if (j < count) {
+			/* pass received bytes to the packet decoder */
+			while (j < count) {
+				if (parse_char(buf[j], packet) > 0) {
+					handle_message(packet);
+					return 1;
+				}
+				/* in case we keep trying but only get crap from GPS */
+				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+					return -1;
+				}
+				j++;
+			}
+			/* everything is read */
+			j = count = 0;
+		}
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
+		}
+	}
+}
+
+void
+MTK::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = MTK_DECODE_UNINIT;
+}
+int
+MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
+{
+	int ret = 0;
+
+	if (_decode_state == MTK_DECODE_UNINIT) {
+
+		if (b == MTK_SYNC1_V16) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 16;
+		} else if (b == MTK_SYNC1_V19) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 19;
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
+		if (b == MTK_SYNC2) {
+			_decode_state = MTK_DECODE_GOT_CK_B;
+
+		} else {
+			// Second start symbol was wrong, reset state machine
+			decode_init();
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
+		// Add to checksum
+		if (_rx_count < 33)
+			add_byte_to_checksum(b);
+
+		// Fill packet buffer
+		((uint8_t*)(&packet))[_rx_count] = b;
+		_rx_count++;
+
+		/* Packet size minus checksum, XXX ? */
+		if (_rx_count >= sizeof(packet)) {
+			/* Compare checksum */
+			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
+				ret = 1;
+			} else {
+				warnx("MTK Checksum invalid");
+				ret = -1;
+			}
+			// Reset state machine to decode next packet
+			decode_init();
+		}
+	}
+	return ret;
+}
+
+void
+MTK::handle_message(gps_mtk_packet_t &packet)
+{
+	if (_mtk_revision == 16) {
+		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
+		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+	} else if (_mtk_revision == 19) {
+		_gps_position->lat = packet.latitude; // both degrees*1e7
+		_gps_position->lon = packet.longitude; // both degrees*1e7
+	} else {
+		warnx("mtk: unknown revision");
+		_gps_position->lat = 0;
+		_gps_position->lon = 0;
+	}
+	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
+	_gps_position->fix_type = packet.fix_type;
+	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
+	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
+	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
+	_gps_position->satellites_visible = packet.satellites;
+
+	/* convert time and date information to unix timestamp */
+	struct tm timeinfo; //TODO: test this conversion
+	uint32_t timeinfo_conversion_temp;
+
+	timeinfo.tm_mday = packet.date * 1e-4;
+	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
+	timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
+	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
+
+	timeinfo.tm_hour = packet.utc_time * 1e-7;
+	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
+	timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
+	timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
+	timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
+	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
+	time_t epoch = mktime(&timeinfo);
+
+	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
+	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
+
+	// Position and velocity update always at the same time
+	_rate_count_vel++;
+	_rate_count_lat_lon++;
+
+	return;
+}
+
+void
+MTK::add_byte_to_checksum(uint8_t b)
+{
+	_rx_ck_a = _rx_ck_a + b;
+	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+}
diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
new file mode 100644
index 000000000..b5cfbf0a6
--- /dev/null
+++ b/src/drivers/gps/mtk.h
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 mtk.h */
+
+#ifndef MTK_H_
+#define MTK_H_
+
+#include "gps_helper.h"
+
+#define MTK_SYNC1_V16 0xd0
+#define MTK_SYNC1_V19 0xd1
+#define MTK_SYNC2 0xdd
+
+#define MTK_OUTPUT_5HZ		"$PMTK220,200*2C\r\n"
+#define MTK_SET_BINARY		"$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define SBAS_ON	        	"$PMTK313,1*2E\r\n"
+#define WAAS_ON				"$PMTK301,2*2E\r\n"
+#define MTK_NAVTHRES_OFF 	"$PMTK397,0*23\r\n"
+
+#define MTK_TIMEOUT_5HZ 400
+#define MTK_BAUDRATE 38400
+
+typedef enum {
+	MTK_DECODE_UNINIT = 0,
+	MTK_DECODE_GOT_CK_A = 1,
+	MTK_DECODE_GOT_CK_B = 2
+} mtk_decode_state_t;
+
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+typedef struct {
+	uint8_t payload; ///< Number of payload bytes
+	int32_t latitude;  ///< Latitude in degrees * 10^7
+	int32_t longitude; ///< Longitude in degrees * 10^7
+	uint32_t msl_altitude;  ///< MSL altitude in meters * 10^2
+	uint32_t ground_speed; ///< velocity in m/s
+	int32_t heading; ///< heading in degrees * 10^2
+	uint8_t satellites; ///< number of sattelites used
+	uint8_t fix_type;  ///< fix type: XXX correct for that
+	uint32_t date;
+	uint32_t utc_time;
+	uint16_t hdop; ///< horizontal dilution of position (without unit)
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_mtk_packet_t;
+
+#pragma pack(pop)
+
+#define MTK_RECV_BUFFER_SIZE 40
+
+class MTK : public GPS_Helper
+{
+public:
+	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~MTK();
+	int				receive(unsigned timeout);
+	int				configure(unsigned &baudrate);
+
+private:
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int				parse_char(uint8_t b, gps_mtk_packet_t &packet);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	void				handle_message(gps_mtk_packet_t &packet);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void				decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void				add_byte_to_checksum(uint8_t);
+
+	int					_fd;
+	struct vehicle_gps_position_s *_gps_position;
+	mtk_decode_state_t	_decode_state;
+	uint8_t				_mtk_revision;
+	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];
+	unsigned			_rx_count;
+	uint8_t 			_rx_ck_a;
+	uint8_t				_rx_ck_b;
+};
+
+#endif /* MTK_H_ */
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
new file mode 100644
index 000000000..b3093b0f6
--- /dev/null
+++ b/src/drivers/gps/ubx.cpp
@@ -0,0 +1,785 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 ubx.cpp
+ *
+ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <drivers/drv_hrt.h>
+
+#include "ubx.h"
+
+#define UBX_CONFIG_TIMEOUT 100
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_waiting_for_ack(false),
+_disable_cmd_counter(0)
+{
+	decode_init();
+}
+
+UBX::~UBX()
+{
+}
+
+int
+UBX::configure(unsigned &baudrate)
+{
+	_waiting_for_ack = true;
+
+	/* try different baudrates */
+	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+	for (int baud_i = 0; baud_i < 5; baud_i++) {
+		baudrate = baudrates_to_try[baud_i];
+		set_baudrate(_fd, baudrate);
+
+		/* Send a CFG-PRT message to set the UBX protocol for in and out
+		 * and leave the baudrate as it is, we just want an ACK-ACK from this
+		 */
+		type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
+		/* Set everything else of the packet to 0, otherwise the module wont accept it */
+		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_PRT;
+
+		/* Define the package contents, don't change the baudrate */
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= baudrate;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* Send a CFG-PRT message again, this time change the baudrate */
+
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		
+		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+		receive(UBX_CONFIG_TIMEOUT);
+		
+		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
+			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		}
+
+		/* send a CFT-RATE message to define update rate */
+		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
+		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_RATE;
+
+		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
+		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
+		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
+		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
+		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
+		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* send a NAV5 message to set the options for the internal filter */
+		type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
+		memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
+
+		cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
+		cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
+		cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
+		cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
+		cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
+		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
+		/* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
+		// 	0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
+			0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+
+		_waiting_for_ack = false;
+		return 0;
+	}
+	return -1;
+}
+
+int
+UBX::wait_for_ack(unsigned timeout)
+{
+	_waiting_for_ack = true;
+	int ret = receive(timeout);
+	_waiting_for_ack = false;
+	return ret;
+}
+
+int
+UBX::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* pass received bytes to the packet decoder */
+		while (j < count) {
+			if (parse_char(buf[j]) > 0) {
+				/* return to configure during configuration or to the gps driver during normal work
+				 * if a packet has arrived */
+				 if (handle_message() > 0)
+					return 1;
+			}
+			/* in case we keep trying but only get crap from GPS */
+			if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				return -1;
+			}
+			j++;
+		}
+
+		/* everything is read */
+		j = count = 0;
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
+		}
+	}
+}
+
+int
+UBX::parse_char(uint8_t b)
+{
+	switch (_decode_state) {
+		/* First, look for sync1 */
+		case UBX_DECODE_UNINIT:
+			if (b == UBX_SYNC1) {
+				_decode_state = UBX_DECODE_GOT_SYNC1;
+			}
+			break;
+		/* Second, look for sync2 */
+		case UBX_DECODE_GOT_SYNC1:
+			if (b == UBX_SYNC2) {
+				_decode_state = UBX_DECODE_GOT_SYNC2;
+			} else {
+				/* Second start symbol was wrong, reset state machine */
+				decode_init();
+			}
+			break;
+		/* Now look for class */
+		case UBX_DECODE_GOT_SYNC2:
+			/* everything except sync1 and sync2 needs to be added to the checksum */
+			add_byte_to_checksum(b);
+			/* check for known class */
+			switch (b) {
+				case UBX_CLASS_ACK:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = ACK;
+					break;
+
+				case UBX_CLASS_NAV:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = NAV;
+					break;
+
+//				case UBX_CLASS_RXM:
+//					_decode_state = UBX_DECODE_GOT_CLASS;
+//					_message_class = RXM;
+//					break;
+
+				case UBX_CLASS_CFG:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = CFG;
+					break;
+				default: //unknown class: reset state machine
+					decode_init();
+					break;
+			}
+			break;
+		case UBX_DECODE_GOT_CLASS:
+			add_byte_to_checksum(b);
+			switch (_message_class) {
+				case NAV:
+					switch (b) {
+					case UBX_MESSAGE_NAV_POSLLH:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_POSLLH;
+						break;
+
+					case UBX_MESSAGE_NAV_SOL:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SOL;
+						break;
+
+					case UBX_MESSAGE_NAV_TIMEUTC:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_TIMEUTC;
+						break;
+
+//					case UBX_MESSAGE_NAV_DOP:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = NAV_DOP;
+//						break;
+
+					case UBX_MESSAGE_NAV_SVINFO:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SVINFO;
+						break;
+
+					case UBX_MESSAGE_NAV_VELNED:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_VELNED;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+//				case RXM:
+//					switch (b) {
+//					case UBX_MESSAGE_RXM_SVSI:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = RXM_SVSI;
+//						break;
+//
+//					default: //unknown class: reset state machine, should not happen
+//						decode_init();
+//						break;
+//					}
+//					break;
+
+				case CFG:
+					switch (b) {
+					case UBX_MESSAGE_CFG_NAV5:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = CFG_NAV5;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+
+				case ACK:
+					switch (b) {
+					case UBX_MESSAGE_ACK_ACK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_ACK;
+						break;
+					case UBX_MESSAGE_ACK_NAK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_NAK;
+						break;
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+				default: //should not happen because we set the class
+					warnx("UBX Error, we set a class that we don't know");
+					decode_init();
+//					config_needed = true;
+					break;
+				}
+				break;
+			case UBX_DECODE_GOT_MESSAGEID:
+				add_byte_to_checksum(b);
+				_payload_size = b; //this is the first length byte
+				_decode_state = UBX_DECODE_GOT_LENGTH1;
+				break;
+			case UBX_DECODE_GOT_LENGTH1:
+				add_byte_to_checksum(b);
+				_payload_size += b << 8; // here comes the second byte of length
+				_decode_state = UBX_DECODE_GOT_LENGTH2;
+				break;
+			case UBX_DECODE_GOT_LENGTH2:
+				/* Add to checksum if not yet at checksum byte */
+				if (_rx_count < _payload_size)
+					add_byte_to_checksum(b);
+					_rx_buffer[_rx_count] = b;
+					/* once the payload has arrived, we can process the information */
+					if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
+
+						/* compare checksum */
+						if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
+							return 1;
+						} else {
+							decode_init();
+							return -1;
+							warnx("ubx: Checksum wrong");
+						}
+
+						return 1;
+					} else if (_rx_count < RECV_BUFFER_SIZE) {
+						_rx_count++;
+					} else {
+						warnx("ubx: buffer full");
+						decode_init();
+						return -1;
+					}
+				break;
+		default:
+			break;
+	}
+	return 0; //XXX ?
+}
+
+
+int
+UBX::handle_message()
+{
+	int ret = 0;
+
+	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
+		case NAV_POSLLH: {
+//			printf("GOT NAV_POSLLH MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
+
+				_gps_position->lat = packet->lat;
+				_gps_position->lon = packet->lon;
+				_gps_position->alt = packet->height_msl;
+
+				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
+				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+
+				_rate_count_lat_lon++;
+
+				/* Add timestamp to finish the report */
+				_gps_position->timestamp_position = hrt_absolute_time();
+				/* only return 1 when new position is available */
+				ret = 1;
+			}
+			break;
+		}
+
+		case NAV_SOL: {
+//			printf("GOT NAV_SOL MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+
+				_gps_position->fix_type = packet->gpsFix;
+				_gps_position->s_variance_m_s = packet->sAcc;
+				_gps_position->p_variance_m = packet->pAcc;
+
+				_gps_position->timestamp_variance = hrt_absolute_time();
+			}
+			break;
+		}
+
+//		case NAV_DOP: {
+////		printf("GOT NAV_DOP MESSAGE\n");
+//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
+//
+//			_gps_position->eph_m =  packet->hDOP;
+//			_gps_position->epv =  packet->vDOP;
+//
+//			_gps_position->timestamp_posdilution = hrt_absolute_time();
+//
+//			_new_nav_dop = true;
+//
+//			break;
+//		}
+
+		case NAV_TIMEUTC: {
+//			printf("GOT NAV_TIMEUTC MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+
+				//convert to unix timestamp
+				struct tm timeinfo;
+				timeinfo.tm_year = packet->year - 1900;
+				timeinfo.tm_mon = packet->month - 1;
+				timeinfo.tm_mday = packet->day;
+				timeinfo.tm_hour = packet->hour;
+				timeinfo.tm_min = packet->min;
+				timeinfo.tm_sec = packet->sec;
+
+				time_t epoch = mktime(&timeinfo);
+
+				_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+
+				_gps_position->timestamp_time = hrt_absolute_time();
+			}
+			break;
+		}
+
+		case NAV_SVINFO: {
+//			printf("GOT NAV_SVINFO MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
+				const int length_part1 = 8;
+				char _rx_buffer_part1[length_part1];
+				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
+
+				//read checksum
+				const int length_part3 = 2;
+				char _rx_buffer_part3[length_part3];
+				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
+
+				//definitions needed to read numCh elements from the buffer:
+				const int length_part2 = 12;
+				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+				char _rx_buffer_part2[length_part2]; //for temporal storage
+
+				uint8_t satellites_used = 0;
+				int i;
+
+				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
+
+					/* Get satellite information from the buffer */
+					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
+					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
+
+
+					/* Write satellite information in the global storage */
+					_gps_position->satellite_prn[i] = packet_part2->svid;
+
+					//if satellite information is healthy store the data
+					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
+
+					if (!unhealthy) {
+						if ((packet_part2->flags) & 1) { //flags is a bitfield
+							_gps_position->satellite_used[i] = 1;
+							satellites_used++;
+
+						} else {
+							_gps_position->satellite_used[i] = 0;
+						}
+
+						_gps_position->satellite_snr[i] = packet_part2->cno;
+						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+
+					} else {
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
+					}
+
+				}
+
+				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
+					/* Unused channels have to be set to zero for e.g. MAVLink */
+					_gps_position->satellite_prn[i] = 0;
+					_gps_position->satellite_used[i] = 0;
+					_gps_position->satellite_snr[i] = 0;
+					_gps_position->satellite_elevation[i] = 0;
+					_gps_position->satellite_azimuth[i] = 0;
+				}
+				_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+
+				/* set timestamp if any sat info is available */
+				if (packet_part1->numCh > 0) {
+					_gps_position->satellite_info_available = true;
+				} else {
+					_gps_position->satellite_info_available = false;
+				}
+				_gps_position->timestamp_satellites = hrt_absolute_time();
+			}
+
+			break;
+		}
+
+		case NAV_VELNED: {
+
+			if (!_waiting_for_ack) {
+				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
+				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+
+				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
+				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
+				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->vel_ned_valid = true;
+				_gps_position->timestamp_velocity = hrt_absolute_time();
+
+				_rate_count_vel++;
+			}
+
+			break;
+		}
+
+//		case RXM_SVSI: {
+//				printf("GOT RXM_SVSI MESSAGE\n");
+//				const int length_part1 = 7;
+//				char _rx_buffer_part1[length_part1];
+//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
+//
+//				_gps_position->satellites_visible = packet->numVis;
+//				_gps_position->counter++;
+//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
+//
+//			break;
+//		}
+		case ACK_ACK: {
+//			printf("GOT ACK_ACK\n");
+			gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
+
+			if (_waiting_for_ack) {
+				if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
+					ret = 1;
+				}
+			}
+		}
+		break;
+
+		case ACK_NAK: {
+//			printf("GOT ACK_NAK\n");
+			warnx("UBX: Received: Not Acknowledged");
+			/* configuration obviously not successful */
+			ret = -1;
+			break;
+		}
+
+		default: //we don't know the message
+			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+			if (_disable_cmd_counter++ == 0) {
+				// Don't attempt for every message to disable, some might not be disabled */
+				warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+				configure_message_rate(_message_class, _message_id, 0);
+			}
+		return ret;
+			ret = -1;
+			break;
+		}
+	// end if _rx_count high enough
+	decode_init();
+	return ret; //XXX?
+}
+
+void
+UBX::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = UBX_DECODE_UNINIT;
+	_message_class = CLASS_UNKNOWN;
+	_message_id = ID_UNKNOWN;
+	_payload_size = 0;
+}
+
+void
+UBX::add_byte_to_checksum(uint8_t b)
+{
+	_rx_ck_a = _rx_ck_a + b;
+	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+}
+
+void
+UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
+{
+	uint8_t ck_a = 0;
+	uint8_t ck_b = 0;
+	unsigned i;
+
+	for (i = 0; i < length-2; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+	/* The checksum is written to the last to bytes of a message */
+	message[length-2] = ck_a;
+	message[length-1] = ck_b;
+}
+
+void
+UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+{
+	for (unsigned i = 0; i < length; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+}
+
+void
+UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+{
+    struct ubx_cfg_msg_rate msg;
+    msg.msg_class	= msg_class;
+    msg.msg_id		= msg_id;
+    msg.rate		= rate;
+    send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+}
+
+void
+UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+{
+	ssize_t ret = 0;
+
+	/* Calculate the checksum now */
+	add_checksum_to_message(packet, length);
+
+	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+
+	/* Start with the two sync bytes */
+	ret += write(fd, sync_bytes, sizeof(sync_bytes));
+	ret += write(fd, packet, length);
+
+	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
+		warnx("ubx: config write fail");
+}
+
+void
+UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
+{
+	struct ubx_header header;
+	uint8_t ck_a=0, ck_b=0;
+	header.sync1 = UBX_SYNC1;
+	header.sync2 = UBX_SYNC2;
+	header.msg_class = msg_class;
+	header.msg_id    = msg_id;
+	header.length    = size;
+
+	add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
+	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
+
+	// Configure receive check
+	_clsID_needed = msg_class;
+	_msgID_needed = msg_id;
+
+	write(_fd, (const char *)&header, sizeof(header));
+	write(_fd, (const char *)msg, size);
+	write(_fd, (const char *)&ck_a, 1);
+	write(_fd, (const char *)&ck_b, 1);
+}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
new file mode 100644
index 000000000..5a433642c
--- /dev/null
+++ b/src/drivers/gps/ubx.h
@@ -0,0 +1,419 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 U-Blox protocol definitions */
+
+#ifndef UBX_H_
+#define UBX_H_
+
+#include "gps_helper.h"
+
+#define UBX_SYNC1 0xB5
+#define UBX_SYNC2 0x62
+
+/* ClassIDs (the ones that are used) */
+#define UBX_CLASS_NAV 0x01
+//#define UBX_CLASS_RXM 0x02
+#define UBX_CLASS_ACK 0x05
+#define UBX_CLASS_CFG 0x06
+
+/* MessageIDs (the ones that are used) */
+#define UBX_MESSAGE_NAV_POSLLH 0x02
+#define UBX_MESSAGE_NAV_SOL 0x06
+#define UBX_MESSAGE_NAV_TIMEUTC 0x21
+//#define UBX_MESSAGE_NAV_DOP 0x04
+#define UBX_MESSAGE_NAV_SVINFO 0x30
+#define UBX_MESSAGE_NAV_VELNED 0x12
+//#define UBX_MESSAGE_RXM_SVSI 0x20
+#define UBX_MESSAGE_ACK_ACK 0x01
+#define UBX_MESSAGE_ACK_NAK 0x00
+#define UBX_MESSAGE_CFG_PRT 0x00
+#define UBX_MESSAGE_CFG_NAV5 0x24
+#define UBX_MESSAGE_CFG_MSG 0x01
+#define UBX_MESSAGE_CFG_RATE 0x08
+
+#define UBX_CFG_PRT_LENGTH 20
+#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
+#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400		/**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01		/**< UBX in */
+#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01		/**< UBX out */
+
+#define UBX_CFG_RATE_LENGTH 6
+#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200		/**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
+#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
+
+
+#define UBX_CFG_NAV5_LENGTH 36
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005		/**< XXX only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
+#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2			/**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
+
+#define UBX_CFG_MSG_LENGTH 8
+#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
+
+#define UBX_MAX_PAYLOAD_LENGTH 500
+
+// ************
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+struct ubx_header {
+	uint8_t sync1;
+	uint8_t sync2;
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint16_t length;
+};
+
+typedef struct {
+	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
+	int32_t lon;					/**<  Longitude * 1e-7, deg */
+	int32_t lat;					/**<  Latitude * 1e-7, deg */
+	int32_t height;					/**<  Height above Ellipsoid, mm */
+	int32_t height_msl;				/**<  Height above mean sea level, mm */
+	uint32_t hAcc;  				/**< Horizontal Accuracy Estimate, mm */
+	uint32_t vAcc;  				/**< Vertical Accuracy Estimate, mm */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_posllh_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**< GPS Millisecond Time of Week */
+	int32_t time_nanoseconds;		/**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
+	int16_t week;					/**< GPS week (GPS time) */
+	uint8_t gpsFix;					/**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
+	uint8_t flags;
+	int32_t ecefX;
+	int32_t ecefY;
+	int32_t ecefZ;
+	uint32_t pAcc;
+	int32_t ecefVX;
+	int32_t ecefVY;
+	int32_t ecefVZ;
+	uint32_t sAcc;
+	uint16_t pDOP;
+	uint8_t reserved1;
+	uint8_t numSV;
+	uint32_t reserved2;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_sol_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds;		/**< GPS Millisecond Time of Week */
+	uint32_t time_accuracy; 		/**< Time Accuracy Estimate, ns */
+	int32_t time_nanoseconds; 		/**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
+	uint16_t year; 					/**< Year, range 1999..2099 (UTC) */
+	uint8_t month; 					/**< Month, range 1..12 (UTC) */
+	uint8_t day; 					/**< Day of Month, range 1..31 (UTC) */
+	uint8_t hour; 					/**< Hour of Day, range 0..23 (UTC) */
+	uint8_t min; 					/**< Minute of Hour, range 0..59 (UTC) */
+	uint8_t sec; 					/**< Seconds of Minute, range 0..59 (UTC) */
+	uint8_t valid_flag; 			/**< Validity Flags (see ubx documentation) */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_timeutc_packet_t;
+
+//typedef struct {
+//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
+//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
+//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
+//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
+//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
+//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
+//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
+//	uint8_t ck_a;
+//	uint8_t ck_b;
+//} gps_bin_nav_dop_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+	uint8_t numCh; 					/**< Number of channels */
+	uint8_t globalFlags;
+	uint16_t reserved2;
+
+} gps_bin_nav_svinfo_part1_packet_t;
+
+typedef struct {
+	uint8_t chn; 					/**< Channel number, 255 for SVs not assigned to a channel */
+	uint8_t svid; 					/**< Satellite ID */
+	uint8_t flags;
+	uint8_t quality;
+	uint8_t cno; 					/**< Carrier to Noise Ratio (Signal Strength), dbHz */
+	int8_t elev; 					/**< Elevation in integer degrees */
+	int16_t azim; 					/**< Azimuth in integer degrees */
+	int32_t prRes; 					/**< Pseudo range residual in centimetres */
+
+} gps_bin_nav_svinfo_part2_packet_t;
+
+typedef struct {
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_svinfo_part3_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; // GPS Millisecond Time of Week
+	int32_t velN; //NED north velocity, cm/s
+	int32_t velE; //NED east velocity, cm/s
+	int32_t velD; //NED down velocity, cm/s
+	uint32_t speed; //Speed (3-D), cm/s
+	uint32_t gSpeed; //Ground Speed (2-D), cm/s
+	int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
+	uint32_t sAcc; //Speed Accuracy Estimate, cm/s
+	uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_velned_packet_t;
+
+//typedef struct {
+//	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
+//	int16_t week; 					/**< Measurement GPS week number */
+//	uint8_t numVis;					/**< Number of visible satellites */
+//
+//	//... rest of package is not used in this implementation
+//
+//} gps_bin_rxm_svsi_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_ack_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_nak_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t portID;
+	uint8_t res0;
+	uint16_t res1;
+	uint32_t mode;
+	uint32_t baudRate;
+	uint16_t inProtoMask;
+	uint16_t outProtoMask;
+	uint16_t flags;
+	uint16_t pad;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_prt_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t measRate;
+	uint16_t navRate;
+	uint16_t timeRef;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_rate_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t mask;
+	uint8_t dynModel;
+	uint8_t fixMode;
+	int32_t fixedAlt;
+	uint32_t fixedAltVar;
+	int8_t minElev;
+	uint8_t drLimit;
+	uint16_t pDop;
+	uint16_t tDop;
+	uint16_t pAcc;
+	uint16_t tAcc;
+	uint8_t staticHoldThresh;
+	uint8_t dgpsTimeOut;
+	uint32_t reserved2;
+	uint32_t reserved3;
+	uint32_t reserved4;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_nav5_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t msgClass_payload;
+	uint8_t msgID_payload;
+	uint8_t rate;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_msg_packet_t;
+
+struct ubx_cfg_msg_rate {
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint8_t rate;
+};
+
+
+// END the structures of the binary packets
+// ************
+
+typedef enum {
+	UBX_CONFIG_STATE_PRT = 0,
+	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
+	UBX_CONFIG_STATE_RATE,
+	UBX_CONFIG_STATE_NAV5,
+	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
+	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
+	UBX_CONFIG_STATE_MSG_NAV_DOP,
+	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
+	UBX_CONFIG_STATE_MSG_NAV_SOL,
+	UBX_CONFIG_STATE_MSG_NAV_VELNED,
+//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
+	UBX_CONFIG_STATE_CONFIGURED
+} ubx_config_state_t;
+
+typedef enum {
+	CLASS_UNKNOWN = 0,
+	NAV = 1,
+	RXM = 2,
+	ACK = 3,
+	CFG = 4
+} ubx_message_class_t;
+
+typedef enum {
+	//these numbers do NOT correspond to the message id numbers of the ubx protocol
+	ID_UNKNOWN = 0,
+	NAV_POSLLH,
+	NAV_SOL,
+	NAV_TIMEUTC,
+//	NAV_DOP,
+	NAV_SVINFO,
+	NAV_VELNED,
+//	RXM_SVSI,
+	CFG_NAV5,
+	ACK_ACK,
+	ACK_NAK,
+} ubx_message_id_t;
+
+typedef enum {
+	UBX_DECODE_UNINIT = 0,
+	UBX_DECODE_GOT_SYNC1,
+	UBX_DECODE_GOT_SYNC2,
+	UBX_DECODE_GOT_CLASS,
+	UBX_DECODE_GOT_MESSAGEID,
+	UBX_DECODE_GOT_LENGTH1,
+	UBX_DECODE_GOT_LENGTH2
+} ubx_decode_state_t;
+
+//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
+#pragma pack(pop)
+
+#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+
+class UBX : public GPS_Helper
+{
+public:
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~UBX();
+	int			receive(unsigned timeout);
+	int			configure(unsigned &baudrate);
+
+private:
+
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int			parse_char(uint8_t b);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	int			handle_message(void);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void			decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void			add_byte_to_checksum(uint8_t);
+
+	/**
+	 * Add the two checksum bytes to an outgoing message
+	 */
+	void			add_checksum_to_message(uint8_t* message, const unsigned length);
+
+	/**
+	 * Helper to send a config packet
+	 */
+	void			send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+
+	void			configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+
+	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+
+	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+
+	int			wait_for_ack(unsigned timeout);
+
+	int			_fd;
+	struct vehicle_gps_position_s *_gps_position;
+	ubx_config_state_t	_config_state;
+	bool			_waiting_for_ack;
+	uint8_t			_clsID_needed;
+	uint8_t			_msgID_needed;
+	ubx_decode_state_t	_decode_state;
+	uint8_t			_rx_buffer[RECV_BUFFER_SIZE];
+	unsigned		_rx_count;
+	uint8_t			_rx_ck_a;
+	uint8_t			_rx_ck_b;
+	ubx_message_class_t	_message_class;
+	ubx_message_id_t	_message_id;
+	unsigned		_payload_size;
+	uint8_t			_disable_cmd_counter;
+};
+
+#endif /* UBX_H_ */
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
new file mode 100644
index 000000000..d9aa772d4
--- /dev/null
+++ b/src/drivers/hil/hil.cpp
@@ -0,0 +1,851 @@
+/****************************************************************************
+ *
+ *   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 hil.cpp
+ *
+ * Driver/configurator for the virtual HIL port.
+ *
+ * This virtual driver emulates PWM / servo outputs for setups where
+ * the connected hardware does not provide enough or no PWM outputs.
+ *
+ * Its only function is to take actuator_control uORB messages,
+ * mix them with any loaded mixer and output the result to the
+ * actuator_output uORB topic. HIL can also be performed with normal
+ * PWM outputs, a special flag prevents the outputs to be operated
+ * during HIL mode. If HIL is not performed with a standalone FMU,
+ * but in a real system, it is NOT recommended to use this virtual
+ * driver. Use instead the normal FMU or IO driver.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/mixer/mixer.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <systemlib/err.h>
+
+class HIL : public device::CDev
+{
+public:
+	enum Mode {
+		MODE_2PWM,
+		MODE_4PWM,
+        MODE_8PWM,
+        MODE_12PWM,
+        MODE_16PWM,
+		MODE_NONE
+	};
+	HIL();
+	virtual ~HIL();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+
+	virtual int	init();
+
+	int		set_mode(Mode mode);
+	int		set_pwm_rate(unsigned rate);
+
+private:
+	static const unsigned _max_actuators = 4;
+
+	Mode		_mode;
+	int 		_update_rate;
+	int 		_current_update_rate;
+	int		_task;
+	int		_t_actuators;
+	int		_t_armed;
+	orb_advert_t	_t_outputs;
+	unsigned	_num_outputs;
+	bool		_primary_pwm_device;
+
+	volatile bool	_task_should_exit;
+	bool		_armed;
+
+	MixerGroup	*_mixers;
+
+	actuator_controls_s _controls;
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+	void		task_main() __attribute__((noreturn));
+
+	static int	control_callback(uintptr_t handle,
+			uint8_t control_group,
+			uint8_t control_index,
+			float &input);
+
+	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+	struct GPIOConfig {
+		uint32_t	input;
+		uint32_t	output;
+		uint32_t	alt;
+	};
+
+	static const GPIOConfig	_gpio_tab[];
+	static const unsigned	_ngpio;
+
+	void		gpio_reset(void);
+	void		gpio_set_function(uint32_t gpios, int function);
+	void		gpio_write(uint32_t gpios, int function);
+	uint32_t	gpio_read(void);
+	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
+
+};
+
+namespace
+{
+
+HIL	*g_hil;
+
+} // namespace
+
+HIL::HIL() :
+	CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
+	_mode(MODE_NONE),
+	_update_rate(50),
+	_current_update_rate(0),
+	_task(-1),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(0),
+	_num_outputs(0),
+	_primary_pwm_device(false),
+	_task_should_exit(false),
+	_armed(false),
+	_mixers(nullptr)
+{
+	_debug_enabled = true;
+}
+
+HIL::~HIL()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		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;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	g_hil = nullptr;
+}
+
+int
+HIL::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	// XXX already claimed with CDEV
+	///* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	//ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
+	/* reset GPIOs */
+	// gpio_reset();
+
+	/* start the HIL interface task */
+	_task = task_spawn("fmuhil",
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_DEFAULT,
+			   2048,
+			   (main_t)&HIL::task_main_trampoline,
+			   nullptr);
+
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+HIL::task_main_trampoline(int argc, char *argv[])
+{
+	g_hil->task_main();
+}
+
+int
+HIL::set_mode(Mode mode)
+{
+	/* 
+	 * Configure for PWM output.
+	 *
+	 * Note that regardless of the configured mode, the task is always
+	 * listening and mixing; the mode just selects which of the channels
+	 * are presented on the output pins.
+	 */
+	switch (mode) {
+	case MODE_2PWM:
+		debug("MODE_2PWM");
+		/* multi-port with flow control lines as PWM */
+		_update_rate = 50;	/* default output rate */
+		break;
+
+	case MODE_4PWM:
+		debug("MODE_4PWM");
+		/* multi-port as 4 PWM outs */
+		_update_rate = 50;	/* default output rate */
+		break;
+            
+    	case MODE_8PWM:
+            debug("MODE_8PWM");
+            /* multi-port as 8 PWM outs */
+            _update_rate = 50;	/* default output rate */
+            break;
+            
+        case MODE_12PWM:
+            debug("MODE_12PWM");
+            /* multi-port as 12 PWM outs */
+            _update_rate = 50;	/* default output rate */
+            break;
+            
+        case MODE_16PWM:
+            debug("MODE_16PWM");
+            /* multi-port as 16 PWM outs */
+            _update_rate = 50;	/* default output rate */
+            break;
+
+	case MODE_NONE:
+		debug("MODE_NONE");
+		/* disable servo outputs and set a very low update rate */
+		_update_rate = 10;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	_mode = mode;
+	return OK;
+}
+
+int
+HIL::set_pwm_rate(unsigned rate)
+{
+	if ((rate > 500) || (rate < 10))
+		return -EINVAL;
+
+	_update_rate = rate;
+	return OK;
+}
+
+void
+HIL::task_main()
+{
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	/* force a reset of the update rate */
+	_current_update_rate = 0;
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	/* advertise the mixed control outputs */
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
+
+	pollfd fds[2];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+
+	unsigned num_outputs;
+
+	/* select the number of virtual outputs */
+	switch (_mode) {
+	case MODE_2PWM:
+		num_outputs = 2;
+		break;
+
+	case MODE_4PWM:
+		num_outputs = 4;
+		break;
+
+	case MODE_8PWM:
+	case MODE_12PWM:
+	case MODE_16PWM:
+		// XXX only support the lower 8 - trivial to extend
+		num_outputs = 8;
+		break;
+
+	case MODE_NONE:
+	default:
+		num_outputs = 0;
+		break;
+	}
+
+	log("starting");
+
+	/* loop until killed */
+	while (!_task_should_exit) {
+
+		/* handle update rate changes */
+		if (_current_update_rate != _update_rate) {
+			int update_rate_in_ms = int(1000 / _update_rate);
+			if (update_rate_in_ms < 2)
+				update_rate_in_ms = 2;
+			orb_set_interval(_t_actuators, update_rate_in_ms);
+			// up_pwm_servo_set_rate(_update_rate);
+			_current_update_rate = _update_rate;
+		}
+
+		/* sleep waiting for data, but no more than a second */
+		int ret = ::poll(&fds[0], 2, 1000);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			continue;
+		}
+
+		/* do we have a control update? */
+		if (fds[0].revents & POLLIN) {
+
+			/* get controls - must always do this to avoid spinning */
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+			/* can we mix? */
+			if (_mixers != nullptr) {
+
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				/* iterate actuators */
+				for (unsigned i = 0; i < num_outputs; i++) {
+
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < (unsigned)outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					} else {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = 900;
+					}
+				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+			}
+		}
+
+		/* how about an arming update? */
+		if (fds[1].revents & POLLIN) {
+			actuator_armed_s aa;
+
+			/* get new value */
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+		}
+	}
+
+	::close(_t_actuators);
+	::close(_t_armed);
+
+	/* make sure servos are off */
+	// up_pwm_servo_deinit();
+
+	log("stopping");
+
+	/* note - someone else is responsible for restoring the GPIO config */
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+int
+HIL::control_callback(uintptr_t handle,
+				      uint8_t control_group,
+				      uint8_t control_index,
+				      float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls->control[control_index];
+	return 0;
+}
+
+int
+HIL::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret;
+
+	debug("ioctl 0x%04x 0x%08x", cmd, arg);
+
+	// /* try it as a GPIO ioctl first */
+	// ret = HIL::gpio_ioctl(filp, cmd, arg);
+	// if (ret != -ENOTTY)
+	// 	return ret;
+
+	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	switch(_mode) {
+	case MODE_2PWM:
+	case MODE_4PWM:
+        case MODE_8PWM:
+        case MODE_12PWM:
+        case MODE_16PWM:
+		ret = HIL::pwm_ioctl(filp, cmd, arg);
+		break;
+	default:
+		ret = -ENOTTY;
+		debug("not in a PWM mode");
+		break;
+	}
+
+	/* if nobody wants it, let CDev have it */
+	if (ret == -ENOTTY)
+		ret = CDev::ioctl(filp, cmd, arg);
+
+	return ret;
+}
+
+int
+HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+	// int channel;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		// up_pwm_servo_arm(true);
+		break;
+
+	case PWM_SERVO_DISARM:
+		// up_pwm_servo_arm(false);
+		break;
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		// HIL always outputs at the alternate (usually faster) rate 
+		g_hil->set_pwm_rate(arg);
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		// HIL always outputs at the alternate (usually faster) rate 
+		break;
+
+	case PWM_SERVO_SET(2):
+	case PWM_SERVO_SET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_SET(0):
+	case PWM_SERVO_SET(1):
+		if (arg < 2100) {
+			// channel = cmd - PWM_SERVO_SET(0);
+//			up_pwm_servo_set(channel, arg); XXX
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_GET(0):
+	case PWM_SERVO_GET(1): {
+			// channel = cmd - PWM_SERVO_SET(0);
+			// *(servo_position_t *)arg = up_pwm_servo_get(channel);
+			break;
+		}
+
+	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+		// no restrictions on output grouping
+		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+		*(uint32_t *)arg = (1 << channel);
+		break;
+	}
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		if (_mode == MODE_4PWM) {
+			*(unsigned *)arg = 4;
+
+		} else {
+			*(unsigned *)arg = 2;
+		}
+
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+		}
+
+		break;
+
+	case MIXERIOCADDSIMPLE: {
+			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
+
+			if (mixer->check()) {
+				delete mixer;
+				ret = -EINVAL;
+
+			} else {
+				if (_mixers == nullptr)
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
+
+				_mixers->add_mixer(mixer);
+			}
+
+			break;
+		}
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
+			if (_mixers == nullptr) {
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					debug("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					ret = -EINVAL;
+				}
+			}
+			break;
+		}
+
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+namespace
+{
+
+enum PortMode {
+	PORT_MODE_UNDEFINED = 0,
+	PORT1_MODE_UNSET,
+	PORT1_FULL_PWM,
+	PORT1_PWM_AND_SERIAL,
+	PORT1_PWM_AND_GPIO,
+	PORT2_MODE_UNSET,
+	PORT2_8PWM,
+	PORT2_12PWM,
+	PORT2_16PWM,
+};
+
+PortMode g_port_mode;
+
+int
+hil_new_mode(PortMode new_mode)
+{
+	// uint32_t gpio_bits;
+	
+
+//	/* reset to all-inputs */
+//	g_hil->ioctl(0, GPIO_RESET, 0);
+
+	// gpio_bits = 0;
+
+	HIL::Mode servo_mode = HIL::MODE_NONE;
+
+	switch (new_mode) {
+	case PORT_MODE_UNDEFINED:
+	case PORT1_MODE_UNSET:
+    	case PORT2_MODE_UNSET:
+            /* nothing more to do here */
+            break;
+
+	case PORT1_FULL_PWM:
+		/* select 4-pin PWM mode */
+		servo_mode = HIL::MODE_4PWM;
+		break;
+
+	case PORT1_PWM_AND_SERIAL:
+		/* select 2-pin PWM mode */
+		servo_mode = HIL::MODE_2PWM;
+//		/* set RX/TX multi-GPIOs to serial mode */
+//		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT1_PWM_AND_GPIO:
+		/* select 2-pin PWM mode */
+		servo_mode = HIL::MODE_2PWM;
+		break;
+            
+        case PORT2_8PWM:
+            /* select 8-pin PWM mode */
+            servo_mode = HIL::MODE_8PWM;
+            break;
+            
+        case PORT2_12PWM:
+            /* select 12-pin PWM mode */
+            servo_mode = HIL::MODE_12PWM;
+            break;
+            
+        case PORT2_16PWM:
+            /* select 16-pin PWM mode */
+            servo_mode = HIL::MODE_16PWM;
+            break;
+	}
+
+//	/* adjust GPIO config for serial mode(s) */
+//	if (gpio_bits != 0)
+//		g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+
+	/* (re)set the PWM output mode */
+	g_hil->set_mode(servo_mode);
+
+	return OK;
+}
+
+int
+hil_start(void)
+{
+	int ret = OK;
+
+	if (g_hil == nullptr) {
+
+		g_hil = new HIL;
+
+		if (g_hil == nullptr) {
+			ret = -ENOMEM;
+
+		} else {
+			ret = g_hil->init();
+
+			if (ret != OK) {
+				delete g_hil;
+				g_hil = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void
+test(void)
+{
+	int	fd;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		puts("open fail");
+		exit(1);
+	}
+
+	ioctl(fd, PWM_SERVO_ARM, 0);
+	ioctl(fd, PWM_SERVO_SET(0), 1000);
+
+	close(fd);
+
+	exit(0);
+}
+
+void
+fake(int argc, char *argv[])
+{
+	if (argc < 5) {
+		puts("hil fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
+		exit(1);
+	}
+
+	actuator_controls_s ac;
+
+	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
+
+	ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
+
+	ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
+
+	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
+
+	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+
+	if (handle < 0) {
+		puts("advertise failed");
+		exit(1);
+	}
+
+	exit(0);
+}
+
+} // namespace
+
+extern "C" __EXPORT int hil_main(int argc, char *argv[]);
+
+int
+hil_main(int argc, char *argv[])
+{
+	PortMode new_mode = PORT_MODE_UNDEFINED;
+	const char *verb = argv[1];
+
+	if (hil_start() != OK)
+		errx(1, "failed to start the HIL driver");
+
+	/*
+	 * Mode switches.
+	 */
+
+ 	// this was all cut-and-pasted from the FMU driver; it's junk
+	if (!strcmp(verb, "mode_pwm")) {
+		new_mode = PORT1_FULL_PWM;
+
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT1_PWM_AND_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT1_PWM_AND_GPIO;
+
+	} else if (!strcmp(verb, "mode_port2_pwm8")) {
+		new_mode = PORT2_8PWM;
+
+	} else if (!strcmp(verb, "mode_port2_pwm12")) {
+		new_mode = PORT2_8PWM;
+
+	} else if (!strcmp(verb, "mode_port2_pwm16")) {
+		new_mode = PORT2_8PWM;
+	}
+
+	/* was a new mode set? */
+	if (new_mode != PORT_MODE_UNDEFINED) {
+
+		/* yes but it's the same mode */
+		if (new_mode == g_port_mode)
+			return OK;
+
+		/* switch modes */
+		return hil_new_mode(new_mode);
+	}
+
+	if (!strcmp(verb, "test"))
+		test();
+
+	if (!strcmp(verb, "fake"))
+		fake(argc - 1, argv + 1);
+
+
+	fprintf(stderr, "HIL: unrecognized command, try:\n");
+	fprintf(stderr, "  mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
+	return -EINVAL;
+}
diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk
new file mode 100644
index 000000000..f8895f5d5
--- /dev/null
+++ b/src/drivers/hil/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Hardware in the Loop (HIL) simulation actuator output bank
+#
+
+MODULE_COMMAND	= hil
+
+SRCS		= hil.cpp
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
new file mode 100644
index 000000000..78eda327c
--- /dev/null
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -0,0 +1,1471 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 hmc5883.cpp
+ *
+ * Driver for the HMC5883 magnetometer connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <float.h>
+
+/*
+ * HMC5883 internal constants and data structures.
+ */
+
+#define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
+
+/* Max measurement rate is 160Hz */
+#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
+
+#define ADDR_CONF_A			0x00
+#define ADDR_CONF_B			0x01
+#define ADDR_MODE			0x02
+#define ADDR_DATA_OUT_X_MSB		0x03
+#define ADDR_DATA_OUT_X_LSB		0x04
+#define ADDR_DATA_OUT_Z_MSB		0x05
+#define ADDR_DATA_OUT_Z_LSB		0x06
+#define ADDR_DATA_OUT_Y_MSB		0x07
+#define ADDR_DATA_OUT_Y_LSB		0x08
+#define ADDR_STATUS			0x09
+#define ADDR_ID_A			0x0a
+#define ADDR_ID_B			0x0b
+#define ADDR_ID_C			0x0c
+
+/* modes not changeable outside of driver */
+#define HMC5883L_MODE_NORMAL		(0 << 0)  /* default */
+#define HMC5883L_MODE_POSITIVE_BIAS	(1 << 0)  /* positive bias */
+#define HMC5883L_MODE_NEGATIVE_BIAS	(1 << 1)  /* negative bias */
+
+#define HMC5883L_AVERAGING_1		(0 << 5) /* conf a register */
+#define HMC5883L_AVERAGING_2		(1 << 5)
+#define HMC5883L_AVERAGING_4		(2 << 5)
+#define HMC5883L_AVERAGING_8		(3 << 5)
+
+#define MODE_REG_CONTINOUS_MODE		(0 << 0)
+#define MODE_REG_SINGLE_MODE		(1 << 0) /* default */
+
+#define STATUS_REG_DATA_OUT_LOCK	(1 << 1) /* page 16: set if data is only partially read, read device to reset */
+#define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
+
+#define ID_A_WHO_AM_I			'H'
+#define ID_B_WHO_AM_I			'4'
+#define ID_C_WHO_AM_I			'3'
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class HMC5883 : public device::I2C
+{
+public:
+	HMC5883(int bus);
+	virtual ~HMC5883();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+	work_s			_work;
+	unsigned		_measure_ticks;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	mag_report		*_reports;
+	mag_scale		_scale;
+	float 			_range_scale;
+	float 			_range_ga;
+	bool			_collect_phase;
+
+	orb_advert_t		_mag_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	/* status reporting */
+	bool			_sensor_ok;		/**< sensor was found and reports ok */
+	bool			_calibrated;		/**< the calibration is valid */
+
+	/**
+	 * Test whether the device supported by the driver is present at a
+	 * specific address.
+	 *
+	 * @param address	The I2C bus address to probe.
+	 * @return		True if the device is present.
+	 */
+	int			probe_address(uint8_t address);
+
+	/**
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void			start();
+
+	/**
+	 * Stop the automatic measurement state machine.
+	 */
+	void			stop();
+
+	/**
+	 * Perform the on-sensor scale calibration routine.
+	 *
+	 * @note The sensor will continue to provide measurements, these
+	 *	 will however reflect the uncalibrated sensor state until
+	 *	 the calibration routine has been completed.
+	 *
+	 * @param enable set to 1 to enable self-test strap, 0 to disable
+	 */
+	int			calibrate(struct file *filp, unsigned enable);
+
+	/**
+	 * Perform the on-sensor scale calibration routine.
+	 *
+	 * @note The sensor will continue to provide measurements, these
+	 *	 will however reflect the uncalibrated sensor state until
+	 *	 the calibration routine has been completed.
+	 *
+	 * @param enable set to 1 to enable self-test positive strap, -1 to enable
+	 *        negative strap, 0 to set to normal mode
+	 */
+	int			set_excitement(unsigned enable);
+
+	/**
+	 * Set the sensor range.
+	 *
+	 * Sets the internal range to handle at least the argument in Gauss.
+	 */
+	int 			set_range(unsigned range);
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			cycle();
+
+	/**
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * Write a register.
+	 *
+	 * @param reg		The register to write.
+	 * @param val		The value to write.
+	 * @return		OK on write success.
+	 */
+	int			write_reg(uint8_t reg, uint8_t val);
+
+	/**
+	 * Read a register.
+	 *
+	 * @param reg		The register to read.
+	 * @param val		The value read.
+	 * @return		OK on read success.
+	 */
+	int			read_reg(uint8_t reg, uint8_t &val);
+
+	/**
+	 * Issue a measurement command.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int			measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			collect();
+
+	/**
+	 * Convert a big-endian signed 16-bit value to a float.
+	 *
+	 * @param in		A signed 16-bit big-endian value.
+	 * @return		The floating-point representation of the value.
+	 */
+	float			meas_to_float(uint8_t in[2]);
+
+	/**
+	 * Check the current calibration and update device status
+	 *
+	 * @return 0 if calibration is ok, 1 else
+	 */
+	 int 			check_calibration();
+
+	 /**
+	 * Check the current scale calibration
+	 *
+	 * @return 0 if scale calibration is ok, 1 else
+	 */
+	 int 			check_scale();
+
+	 /**
+	 * Check the current offset calibration
+	 *
+	 * @return 0 if offset calibration is ok, 1 else
+	 */
+	 int 			check_offset();
+
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
+
+
+HMC5883::HMC5883(int bus) :
+	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+	_measure_ticks(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_range_scale(0), /* default range scale from counts to gauss */
+	_range_ga(1.3f),
+	_mag_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
+	_sensor_ok(false),
+	_calibrated(false)
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scaling
+	_scale.x_offset = 0;
+	_scale.x_scale = 1.0f;
+	_scale.y_offset = 0;
+	_scale.y_scale = 1.0f;
+	_scale.z_offset = 0;
+	_scale.z_scale = 1.0f;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+HMC5883::~HMC5883()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+HMC5883::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct mag_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	_oldest_report = _next_report = 0;
+
+	/* get a publish handle on the mag topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
+
+	if (_mag_topic < 0)
+		debug("failed to create sensor_mag object");
+
+	/* set range */
+	set_range(_range_ga);
+
+	ret = OK;
+	/* sensor is ok, but not calibrated */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int HMC5883::set_range(unsigned range)
+{
+	uint8_t range_bits;
+
+	if (range < 1) {
+		range_bits = 0x00;
+		_range_scale = 1.0f / 1370.0f;
+		_range_ga = 0.88f;
+
+	} else if (range <= 1) {
+		range_bits = 0x01;
+		_range_scale = 1.0f / 1090.0f;
+		_range_ga = 1.3f;
+
+	} else if (range <= 2) {
+		range_bits = 0x02;
+		_range_scale = 1.0f / 820.0f;
+		_range_ga = 1.9f;
+
+	} else if (range <= 3) {
+		range_bits = 0x03;
+		_range_scale = 1.0f / 660.0f;
+		_range_ga = 2.5f;
+
+	} else if (range <= 4) {
+		range_bits = 0x04;
+		_range_scale = 1.0f / 440.0f;
+		_range_ga = 4.0f;
+
+	} else if (range <= 4.7f) {
+		range_bits = 0x05;
+		_range_scale = 1.0f / 390.0f;
+		_range_ga = 4.7f;
+
+	} else if (range <= 5.6f) {
+		range_bits = 0x06;
+		_range_scale = 1.0f / 330.0f;
+		_range_ga = 5.6f;
+
+	} else {
+		range_bits = 0x07;
+		_range_scale = 1.0f / 230.0f;
+		_range_ga = 8.1f;
+	}
+
+	int ret;
+
+	/*
+	 * Send the command to set the range
+	 */
+	ret = write_reg(ADDR_CONF_B, (range_bits << 5));
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	uint8_t range_bits_in;
+	ret = read_reg(ADDR_CONF_B, range_bits_in);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	return !(range_bits_in == (range_bits << 5));
+}
+
+int
+HMC5883::probe()
+{
+	uint8_t data[3] = {0, 0, 0};
+
+	_retries = 10;
+
+	if (read_reg(ADDR_ID_A, data[0]) ||
+	    read_reg(ADDR_ID_B, data[1]) ||
+	    read_reg(ADDR_ID_C, data[2]))
+		debug("read_reg fail");
+
+	_retries = 2;
+
+	if ((data[0] != ID_A_WHO_AM_I) ||
+	    (data[1] != ID_B_WHO_AM_I) ||
+	    (data[2] != ID_C_WHO_AM_I)) {
+		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+ssize_t
+HMC5883::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct mag_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_oldest_report = _next_report = 0;
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(HMC5883_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* add one to account for the sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct mag_report *buf = new struct mag_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case MAGIOCSSAMPLERATE:
+		/* not supported, always 1 sample per poll */
+		return -EINVAL;
+
+	case MAGIOCSRANGE:
+		return set_range(arg);
+
+	case MAGIOCSLOWPASS:
+		/* not supported, no internal filtering */
+		return -EINVAL;
+
+	case MAGIOCSSCALE:
+		/* set new scale factors */
+		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
+		/* check calibration, but not actually return an error */
+		(void)check_calibration();
+		return 0;
+
+	case MAGIOCGSCALE:
+		/* copy out scale factors */
+		memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
+		return 0;
+
+	case MAGIOCCALIBRATE:
+		return calibrate(filp, arg);
+
+	case MAGIOCEXSTRAP:
+		return set_excitement(arg);
+
+	case MAGIOCSELFTEST:
+		return check_calibration();
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+void
+HMC5883::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
+}
+
+void
+HMC5883::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+HMC5883::cycle_trampoline(void *arg)
+{
+	HMC5883 *dev = (HMC5883 *)arg;
+
+	dev->cycle();
+}
+
+void
+HMC5883::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&HMC5883::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&HMC5883::cycle_trampoline,
+		   this,
+		   USEC2TICK(HMC5883_CONVERSION_INTERVAL));
+}
+
+int
+HMC5883::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	return ret;
+}
+
+int
+HMC5883::collect()
+{
+#pragma pack(push, 1)
+	struct { /* status register and data as read back from the device */
+		uint8_t		x[2];
+		uint8_t		z[2];
+		uint8_t		y[2];
+	}	hmc_report;
+#pragma pack(pop)
+	struct {
+		int16_t		x, y, z;
+	} report;
+	int	ret = -EIO;
+	uint8_t	cmd;
+
+
+	perf_begin(_sample_perf);
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+
+	/*
+	 * @note  We could read the status register here, which could tell us that
+	 *        we were too early and that the output registers are still being
+	 *        written.  In the common case that would just slow us down, and
+	 *        we're better off just never being early.
+	 */
+
+	/* get measurements from the device */
+	cmd = ADDR_DATA_OUT_X_MSB;
+	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
+
+	if (ret != OK) {
+		perf_count(_comms_errors);
+		debug("data/status read error");
+		goto out;
+	}
+
+	/* swap the data we just received */
+	report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
+	report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
+	report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
+
+	/*
+	 * If any of the values are -4096, there was an internal math error in the sensor.
+	 * Generalise this to a simple range check that will also catch some bit errors.
+	 */
+	if ((abs(report.x) > 2048) ||
+	    (abs(report.y) > 2048) ||
+	    (abs(report.z) > 2048))
+		goto out;
+
+	/*
+	 * RAW outputs
+	 *
+	 * to align the sensor axes with the board, x and y need to be flipped
+	 * and y needs to be negated
+	 */
+	_reports[_next_report].x_raw = report.y;
+	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
+	/* z remains z */
+	_reports[_next_report].z_raw = report.z;
+
+	/* scale values for output */
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+#ifdef PX4_I2C_BUS_ONBOARD
+	if (_bus == PX4_I2C_BUS_ONBOARD) {
+		/* to align the sensor axes with the board, x and y need to be flipped */
+		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		/* flip axes and negate value for y */
+		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		/* z remains z */
+		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+	} else {
+#endif
+		/* XXX axis assignment of external sensor is yet unknown */
+		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		/* flip axes and negate value for y */
+		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		/* z remains z */
+		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+#ifdef PX4_I2C_BUS_ONBOARD
+	}
+#endif
+
+	/* publish it */
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, toss it */
+	if (_next_report == _oldest_report) {
+		perf_count(_buffer_overflows);
+		INCREMENT(_oldest_report, _num_reports);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+out:
+	perf_end(_sample_perf);
+	return ret;
+}
+
+int HMC5883::calibrate(struct file *filp, unsigned enable)
+{
+	struct mag_report report;
+	ssize_t sz;
+	int ret = 1;
+
+	// XXX do something smarter here
+	int fd = (int)enable;
+
+	struct mag_scale mscale_previous = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
+	unsigned i;
+
+	warnx("starting mag scale calibration");
+
+	/* do a simple demand read */
+	sz = read(filp, (char *)&report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		warn("immediate read failed");
+		ret = 1;
+		goto out;
+	}
+
+	warnx("current measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+	warnx("time:        %lld", report.timestamp);
+	warnx("sampling 500 samples for scaling offset");
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
+		warn("failed to set queue depth");
+		ret = 1;
+		goto out;
+	}
+
+	/* start the sensor polling at 50 Hz */
+	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
+		warn("failed to set 2Hz poll rate");
+		ret = 1;
+		goto out;
+	}
+
+	/* Set to 2.5 Gauss */
+	if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
+		warnx("failed to set 2.5 Ga range");
+		ret = 1;
+		goto out;
+	}
+
+	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
+		warnx("failed to enable sensor calibration mode");
+		ret = 1;
+		goto out;
+	}
+
+	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
+		warn("WARNING: failed to get scale / offsets for mag");
+		ret = 1;
+		goto out;
+	}
+
+	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
+		warn("WARNING: failed to set null scale / offsets for mag");
+		ret = 1;
+		goto out;
+	}
+
+	/* read the sensor 10x and report each value */
+	for (i = 0; i < 500; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = ::poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warn("timed out waiting for sensor data");
+			goto out;
+		}
+
+		/* now go get it */
+		sz = ::read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warn("periodic read failed");
+			goto out;
+
+		} else {
+			avg_excited[0] += report.x;
+			avg_excited[1] += report.y;
+			avg_excited[2] += report.z;
+		}
+
+		//warnx("periodic read %u", i);
+		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+	}
+
+	avg_excited[0] /= i;
+	avg_excited[1] /= i;
+	avg_excited[2] /= i;
+
+	warnx("done. Performed %u reads", i);
+	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
+
+	float scaling[3];
+
+	/* calculate axis scaling */
+	scaling[0] = fabsf(1.16f / avg_excited[0]);
+	/* second axis inverted */
+	scaling[1] = fabsf(1.16f / -avg_excited[1]);
+	scaling[2] = fabsf(1.08f / avg_excited[2]);
+
+	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
+
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+		goto out;
+	}
+
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+		warnx("failed to disable sensor calibration mode");
+		goto out;
+	}
+
+	/* set scaling in device */
+	mscale_previous.x_scale = scaling[0];
+	mscale_previous.y_scale = scaling[1];
+	mscale_previous.z_scale = scaling[2];
+
+	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
+		warn("WARNING: failed to set new scale / offsets for mag");
+		goto out;
+	}
+
+	ret = OK;
+
+out:
+
+	if (ret == OK) {
+		if (!check_scale()) {
+			warnx("mag scale calibration successfully finished.");
+		} else {
+			warnx("mag scale calibration finished with invalid results.");
+			ret = ERROR;
+		}
+
+	} else {
+		warnx("mag scale calibration failed.");
+	}
+
+	return ret;
+}
+
+int HMC5883::check_scale()
+{
+	bool scale_valid;
+
+	if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
+		(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
+		(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
+		/* scale is one */
+		scale_valid = false;
+	} else {
+		scale_valid = true;
+	}
+
+	/* return 0 if calibrated, 1 else */
+	return !scale_valid;
+}
+
+int HMC5883::check_offset()
+{
+	bool offset_valid;
+
+	if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
+		(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
+		(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
+		/* offset is zero */
+		offset_valid = false;
+	} else {
+		offset_valid = true;
+	}
+
+	/* return 0 if calibrated, 1 else */
+	return !offset_valid;
+}
+
+int HMC5883::check_calibration()
+{
+	bool offset_valid = (check_offset() == OK);
+	bool scale_valid  = (check_scale() == OK);
+
+	if (_calibrated != (offset_valid && scale_valid)) {
+		warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
+					  (offset_valid) ? "" : "offset invalid");
+		_calibrated = (offset_valid && scale_valid);
+
+
+		// XXX Change advertisement
+
+		/* notify about state change */
+		struct subsystem_info_s info = {
+			true,
+			true,
+			_calibrated,
+			SUBSYSTEM_TYPE_MAG};
+		static orb_advert_t pub = -1;
+
+		if (pub > 0) {
+			orb_publish(ORB_ID(subsystem_info), pub, &info);
+		} else {
+			pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		}
+	}
+
+	/* return 0 if calibrated, 1 else */
+	return (!_calibrated);
+}
+
+int HMC5883::set_excitement(unsigned enable)
+{
+	int ret;
+	/* arm the excitement strap */
+	uint8_t conf_reg;
+	ret = read_reg(ADDR_CONF_A, conf_reg);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	if (((int)enable) < 0) {
+		conf_reg |= 0x01;
+
+	} else if (enable > 0) {
+		conf_reg |= 0x02;
+
+	} else {
+		conf_reg &= ~0x03;
+	}
+
+	ret = write_reg(ADDR_CONF_A, conf_reg);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	uint8_t conf_reg_ret;
+	read_reg(ADDR_CONF_A, conf_reg_ret);
+
+	return !(conf_reg == conf_reg_ret);
+}
+
+int
+HMC5883::write_reg(uint8_t reg, uint8_t val)
+{
+	uint8_t cmd[] = { reg, val };
+
+	return transfer(&cmd[0], 2, nullptr, 0);
+}
+
+int
+HMC5883::read_reg(uint8_t reg, uint8_t &val)
+{
+	return transfer(&reg, 1, &val, 1);
+}
+
+float
+HMC5883::meas_to_float(uint8_t in[2])
+{
+	union {
+		uint8_t	b[2];
+		int16_t	w;
+	} u;
+
+	u.b[0] = in[1];
+	u.b[1] = in[0];
+
+	return (float) u.w;
+}
+
+void
+HMC5883::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace hmc5883
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+HMC5883	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+int	calibrate();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver, attempt expansion bus first */
+	g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
+	if (g_dev != nullptr && OK != g_dev->init()) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+			
+
+#ifdef PX4_I2C_BUS_ONBOARD
+	/* if this failed, attempt onboard sensor */
+	if (g_dev == nullptr) {
+		g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
+		if (g_dev != nullptr && OK != g_dev->init()) {
+			goto fail;
+		}
+	}
+#endif
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct mag_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+	warnx("time:        %lld", report.timestamp);
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+		errx(1, "failed to set queue depth");
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	errx(0, "PASS");
+}
+
+
+/**
+ * Automatic scale calibration.
+ *
+ * Basic idea:
+ *
+ *   output = (ext field +- 1.1 Ga self-test) * scale factor
+ *
+ * and consequently:
+ *
+ *   1.1 Ga = (excited - normal) * scale factor
+ *   scale factor = (excited - normal) / 1.1 Ga
+ *
+ *   sxy = (excited - normal) / 766	| for conf reg. B set to 0x60 / Gain = 3
+ *   sz  = (excited - normal) / 713	| for conf reg. B set to 0x60 / Gain = 3
+ *
+ * By subtracting the non-excited measurement the pure 1.1 Ga reading
+ * can be extracted and the sensitivity of all axes can be matched.
+ *
+ * SELF TEST OPERATION
+ * To check the HMC5883L for proper operation, a self test feature in incorporated
+ * in which the sensor offset straps are excited to create a nominal field strength
+ * (bias field) to be measured. To implement self test, the least significant bits
+ * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
+ * or 10 (negetive bias), e.g. 0x11 or 0x12.
+ * Then, by placing the mode register into single-measurement mode (0x01),
+ * two data acquisition cycles will be made on each magnetic vector.
+ * The first acquisition will be a set pulse followed shortly by measurement
+ * data of the external field. The second acquisition will have the offset strap
+ * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
+ * about a ±1.1 gauss self test field plus the external field. The first acquisition
+ * values will be subtracted from the second acquisition, and the net measurement
+ * will be placed into the data output registers.
+ * Since self test adds ~1.1 Gauss additional field to the existing field strength,
+ * using a reduced gain setting prevents sensor from being saturated and data registers
+ * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
+ * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
+ * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
+ * output register. To leave the self test mode, change MS1 and MS0 bit of the
+ * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
+ * Using the self test method described above, the user can scale sensor
+ */
+int calibrate()
+{
+	int ret;
+
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+
+	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
+		warnx("failed to enable sensor calibration mode");
+	}
+
+	close(fd);
+
+	if (ret == OK) {
+		errx(0, "PASS");
+
+	} else {
+		errx(1, "FAIL");
+	}
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+hmc5883_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		hmc5883::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		hmc5883::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		hmc5883::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		hmc5883::info();
+
+	/*
+	 * Autocalibrate the scaling
+	 */
+	if (!strcmp(argv[1], "calibrate")) {
+		if (hmc5883::calibrate() == 0) {
+			errx(0, "calibration successful");
+
+		} else {
+			errx(1, "calibration failed");
+		}
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'");
+}
diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk
new file mode 100644
index 000000000..07377556d
--- /dev/null
+++ b/src/drivers/hmc5883/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# HMC5883 driver
+#
+
+MODULE_COMMAND	= hmc5883
+
+# XXX seems excessive, check if 2048 is sufficient
+MODULE_STACKSIZE	= 4096
+
+SRCS		= hmc5883.cpp
diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c
new file mode 100644
index 000000000..a13a6ef58
--- /dev/null
+++ b/src/drivers/hott_telemetry/hott_telemetry_main.c
@@ -0,0 +1,306 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 hott_telemetry_main.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry implementation.
+ *
+ * The HoTT receiver polls each device at a regular interval at which point
+ * a data packet can be returned if necessary.
+ *
+ * TODO: Add support for at least the vario and GPS sensor data.
+ */
+
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <systemlib/systemlib.h>
+
+#include "messages.h"
+
+static int thread_should_exit = false;		/**< Deamon exit flag */
+static int thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static char *daemon_name = "hott_telemetry";
+static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
+
+
+/* A little console messaging experiment - console helper macro */
+#define FATAL_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
+#define ERROR_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
+#define INFO_MSG(_msg)		printf("[%s] %s\n", daemon_name, _msg);
+/**
+ * Deamon management function.
+ */
+__EXPORT int hott_telemetry_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int hott_telemetry_thread_main(int argc, char *argv[]);
+
+static int read_data(int uart, int *id);
+static int send_data(int uart, uint8_t *buffer, int size);
+
+static int open_uart(const char *device, struct termios *uart_config_original)
+{
+	/* baud rate */
+	int speed = B19200;
+	int uart;
+
+	/* open uart */
+	uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		char msg[80];
+		sprintf(msg, "Error opening port: %s\n", device);
+		FATAL_MSG(msg);
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+
+	/* Back up the original uart configuration to restore it after exit */
+	char msg[80];
+
+	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+		sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
+		close(uart);
+		FATAL_MSG(msg);
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
+			device, termios_state);
+		close(uart);
+		FATAL_MSG(msg);
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
+		close(uart);
+		FATAL_MSG(msg);
+	}
+
+	/* Activate single wire mode */
+	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	return uart;
+}
+
+int read_data(int uart, int *id)
+{
+	const int timeout = 1000;
+	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
+
+	char mode;
+
+	if (poll(fds, 1, timeout) > 0) {
+		/* Get the mode: binary or text  */
+		read(uart, &mode, 1);
+		/* Read the device ID being polled */
+		read(uart, id, 1);
+
+		/* if we have a binary mode request */
+		if (mode != BINARY_MODE_REQUEST_ID) {
+			return ERROR;
+		}
+
+	} else {
+		ERROR_MSG("UART timeout on TX/RX port");
+		return ERROR;
+	}
+
+	return OK;
+}
+
+int send_data(int uart, uint8_t *buffer, int size)
+{
+	usleep(POST_READ_DELAY_IN_USECS);
+
+	uint16_t checksum = 0;
+
+	for (int i = 0; i < size; i++) {
+		if (i == size - 1) {
+			/* Set the checksum: the first uint8_t is taken as the checksum. */
+			buffer[i] = checksum & 0xff;
+
+		} else {
+			checksum += buffer[i];
+		}
+
+		write(uart, &buffer[i], 1);
+
+		/* Sleep before sending the next byte. */
+		usleep(POST_WRITE_DELAY_IN_USECS);
+	}
+
+	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
+	/* TODO: Fix this!! */
+	uint8_t dummy[size];
+	read(uart, &dummy, size);
+
+	return OK;
+}
+
+int hott_telemetry_thread_main(int argc, char *argv[])
+{
+	INFO_MSG("starting");
+
+	thread_running = true;
+
+	char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				ERROR_MSG("missing parameter to -d");
+				ERROR_MSG(commandline_usage);
+				exit(1);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	struct termios uart_config_original;
+	int uart = open_uart(device, &uart_config_original);
+
+	if (uart < 0) {
+		ERROR_MSG("Failed opening HoTT UART, exiting.");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	messages_init();
+
+	uint8_t buffer[MESSAGE_BUFFER_SIZE];
+	int size = 0;
+	int id = 0;
+
+	while (!thread_should_exit) {
+		if (read_data(uart, &id) == OK) {
+			switch (id) {
+			case ELECTRIC_AIR_MODULE:
+				build_eam_response(buffer, &size);
+				break;
+
+			default:
+				continue;	// Not a module we support.
+			}
+
+			send_data(uart, buffer, size);
+		}
+	}
+
+	INFO_MSG("exiting");
+
+	close(uart);
+
+	thread_running = false;
+
+	return 0;
+}
+
+/**
+ * Process command line arguments and tart the daemon.
+ */
+int hott_telemetry_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		ERROR_MSG("missing command");
+		ERROR_MSG(commandline_usage);
+		exit(1);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			INFO_MSG("deamon already running");
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("hott_telemetry",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 40,
+					 2048,
+					 hott_telemetry_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			INFO_MSG("daemon is running");
+
+		} else {
+			INFO_MSG("daemon not started");
+		}
+
+		exit(0);
+	}
+
+	ERROR_MSG("unrecognized command");
+	ERROR_MSG(commandline_usage);
+	exit(1);
+}
+
+
+
diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
new file mode 100644
index 000000000..f0f32d244
--- /dev/null
+++ b/src/drivers/hott_telemetry/messages.c
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 messages.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ */
+
+#include "messages.h"
+
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <unistd.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_combined.h>
+
+static int airspeed_sub = -1;
+static int battery_sub = -1;
+static int sensor_sub = -1;
+
+void messages_init(void)
+{
+	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+}
+
+void build_eam_response(uint8_t *buffer, int *size)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+	/* get a local copy of the battery data */
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+	struct eam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
+	msg.sensor_id = EAM_SENSOR_ID;
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg.temperature2 = TEMP_ZERO_CELSIUS;
+	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
+
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
+	msg.altitude_L = (uint8_t)alt & 0xff;
+	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+	/* get a local copy of the current sensor values */
+	struct airspeed_s airspeed;
+	memset(&airspeed, 0, sizeof(airspeed));
+	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
+	msg.speed_L = (uint8_t)speed & 0xff;
+	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+
+	memcpy(buffer, &msg, *size);
+}
\ No newline at end of file
diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h
new file mode 100644
index 000000000..dd38075fa
--- /dev/null
+++ b/src/drivers/hott_telemetry/messages.h
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 messages.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry message generation.
+ *
+ */
+#ifndef MESSAGES_H_
+#define MESSAGES_H
+
+#include <stdlib.h>
+
+/* The buffer size used to store messages. This must be at least as big as the number of
+ * fields in the largest message struct.
+ */
+#define MESSAGE_BUFFER_SIZE 50
+
+/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
+ * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
+ * the message after the read which takes some milliseconds.
+ */
+#define POST_READ_DELAY_IN_USECS	4000
+/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
+ * values can be used in practise though.
+ */
+#define POST_WRITE_DELAY_IN_USECS	2000
+
+// Protocol constants.
+#define BINARY_MODE_REQUEST_ID	0x80	// Binary mode request.
+#define START_BYTE		0x7c
+#define STOP_BYTE		0x7d
+#define TEMP_ZERO_CELSIUS	0x14
+
+/* Electric Air Module (EAM) constants. */
+#define ELECTRIC_AIR_MODULE	0x8e
+#define EAM_SENSOR_ID		0xe0
+
+/* The Electric Air Module message. */
+struct eam_module_msg {
+	uint8_t start;			/**< Start byte   				*/
+	uint8_t eam_sensor_id;		/**< EAM sensor ID 				*/
+	uint8_t warning;
+	uint8_t sensor_id;		/**< Sensor ID, why different? 			*/
+	uint8_t alarm_inverse1;
+	uint8_t alarm_inverse2;
+	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported. 	*/
+	uint8_t cell2_L;
+	uint8_t cell3_L;
+	uint8_t cell4_L;
+	uint8_t cell5_L;
+	uint8_t cell6_L;
+	uint8_t cell7_L;
+	uint8_t cell1_H;
+	uint8_t cell2_H;
+	uint8_t cell3_H;
+	uint8_t cell4_H;
+	uint8_t cell5_H;
+	uint8_t cell6_H;
+	uint8_t cell7_H;
+	uint8_t batt1_voltage_L;	/**< Battery 1 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt1_voltage_H;
+	uint8_t batt2_voltage_L;	/**< Battery 2 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt2_voltage_H;
+	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees 		*/
+	uint8_t temperature2;
+	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters 	*/
+	uint8_t altitude_H;
+	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V 	*/
+	uint8_t current_H;
+	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V 	*/
+	uint8_t main_voltage_H;
+	uint8_t battery_capacity_L;	/**< Used battery capacity in steps of 10mAh 		*/
+	uint8_t battery_capacity_H;
+	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 		*/
+	uint8_t climbrate_H;
+	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 		*/
+	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min 		*/
+	uint8_t rpm_H;
+	uint8_t electric_min;		/**< Flight time in minutes. 				*/
+	uint8_t electric_sec;		/**< Flight time in seconds. 				*/
+	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h 		*/
+	uint8_t speed_H;
+	uint8_t stop;			/**< Stop byte 						*/
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. 			*/
+};
+
+void messages_init(void);
+void build_eam_response(uint8_t *buffer, int *size);
+
+#endif /* MESSAGES_H_ */
diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott_telemetry/module.mk
new file mode 100644
index 000000000..def1d59e9
--- /dev/null
+++ b/src/drivers/hott_telemetry/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Graupner HoTT Telemetry application.
+#
+
+MODULE_COMMAND	= hott_telemetry
+
+SRCS		= hott_telemetry_main.c \
+		  messages.c
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
new file mode 100644
index 000000000..98098c83b
--- /dev/null
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -0,0 +1,888 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 l3gd20.cpp
+ * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_gyro.h>
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* SPI protocol address bits */
+#define DIR_READ				(1<<7)
+#define DIR_WRITE				(0<<7)
+#define ADDR_INCREMENT				(1<<6)
+
+/* register addresses */
+#define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM				0xD4
+
+#define ADDR_CTRL_REG1			0x20
+#define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
+/* keep lowpass low to avoid noise issues */
+#define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_380HZ_LP_30HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
+#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+
+#define ADDR_CTRL_REG2			0x21
+#define ADDR_CTRL_REG3			0x22
+#define ADDR_CTRL_REG4			0x23
+#define REG4_RANGE_MASK				0x30 /* Mask to guard partial register update */
+#define RANGE_250DPS				(0<<4)
+#define RANGE_500DPS				(1<<4)
+#define RANGE_2000DPS				(3<<4)
+
+#define ADDR_CTRL_REG5			0x24
+#define ADDR_REFERENCE			0x25
+#define ADDR_OUT_TEMP			0x26
+#define ADDR_STATUS_REG			0x27
+#define ADDR_OUT_X_L			0x28
+#define ADDR_OUT_X_H			0x29
+#define ADDR_OUT_Y_L			0x2A
+#define ADDR_OUT_Y_H			0x2B
+#define ADDR_OUT_Z_L			0x2C
+#define ADDR_OUT_Z_H			0x2D
+#define ADDR_FIFO_CTRL_REG		0x2E
+#define ADDR_FIFO_SRC_REG		0x2F
+#define ADDR_INT1_CFG			0x30
+#define ADDR_INT1_SRC			0x31
+#define ADDR_INT1_TSH_XH		0x32
+#define ADDR_INT1_TSH_XL		0x33
+#define ADDR_INT1_TSH_YH		0x34
+#define ADDR_INT1_TSH_YL		0x35
+#define ADDR_INT1_TSH_ZH		0x36
+#define ADDR_INT1_TSH_ZL		0x37
+#define ADDR_INT1_DURATION		0x38
+
+
+/* Internal configuration values */
+#define REG1_POWER_NORMAL			(1<<3)
+#define REG1_Z_ENABLE				(1<<2)
+#define REG1_Y_ENABLE				(1<<1)
+#define REG1_X_ENABLE				(1<<0)
+
+#define REG4_BDU				(1<<7)
+#define REG4_BLE				(1<<6)
+//#define REG4_SPI_3WIRE			(1<<0)
+
+#define REG5_FIFO_ENABLE			(1<<6)
+#define REG5_REBOOT_MEMORY			(1<<7)
+
+#define STATUS_ZYXOR				(1<<7)
+#define STATUS_ZOR				(1<<6)
+#define STATUS_YOR				(1<<5)
+#define STATUS_XOR				(1<<4)
+#define STATUS_ZYXDA				(1<<3)
+#define STATUS_ZDA				(1<<2)
+#define STATUS_YDA				(1<<1)
+#define STATUS_XDA				(1<<0)
+
+#define FIFO_CTRL_BYPASS_MODE			(0<<5)
+#define FIFO_CTRL_FIFO_MODE			(1<<5)
+#define FIFO_CTRL_STREAM_MODE			(1<<6)
+#define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
+#define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
+
+extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
+
+class L3GD20 : public device::SPI
+{
+public:
+	L3GD20(int bus, const char* path, spi_dev_e device);
+	virtual ~L3GD20();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+
+	struct hrt_call		_call;
+	unsigned		_call_interval;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	struct gyro_report	*_reports;
+
+	struct gyro_scale	_gyro_scale;
+	float			_gyro_range_scale;
+	float			_gyro_range_rad_s;
+	orb_advert_t		_gyro_topic;
+
+	unsigned		_current_rate;
+	unsigned		_current_range;
+
+	perf_counter_t		_sample_perf;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the L3GD20
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the L3GD20
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the L3GD20
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the L3GD20 measurement range.
+	 *
+	 * @param max_dps	The measurement range is set to permit reading at least
+	 *			this rate in degrees per second.
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_range(unsigned max_dps);
+
+	/**
+	 * Set the L3GD20 internal sampling frequency.
+	 *
+	 * @param frequency	The internal sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			set_samplerate(unsigned frequency);
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
+	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
+	_call_interval(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_gyro_range_scale(0.0f),
+	_gyro_range_rad_s(0.0f),
+	_gyro_topic(-1),
+	_current_rate(0),
+	_current_range(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_gyro_scale.x_offset = 0;
+	_gyro_scale.x_scale  = 1.0f;
+	_gyro_scale.y_offset = 0;
+	_gyro_scale.y_scale  = 1.0f;
+	_gyro_scale.z_offset = 0;
+	_gyro_scale.z_scale  = 1.0f;
+}
+
+L3GD20::~L3GD20()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+
+	/* delete the perf counter */
+	perf_free(_sample_perf);
+}
+
+int
+L3GD20::init()
+{
+	int ret = ERROR;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_oldest_report = _next_report = 0;
+	_reports = new struct gyro_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	/* advertise sensor topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
+
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_reg(ADDR_CTRL_REG5, 0);
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);	  /* disable wake-on-interrupt */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
+
+	set_range(500);				/* default to 500dps */
+	set_samplerate(0);			/* max sample rate */
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+L3GD20::probe()
+{
+	/* read dummy value to void to clear SPI statemachine on sensor */
+	(void)read_reg(ADDR_WHO_AM_I);
+
+	/* verify that the device is attached and functioning */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+L3GD20::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct gyro_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_report = _next_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _reports, sizeof(*_reports));
+	ret = sizeof(*_reports);
+
+	return ret;
+}
+
+int
+L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_interval = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* With internal low pass filters enabled, 250 Hz is sufficient */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_call.period = _call_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct gyro_report *buf = new struct gyro_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement */
+		return -EINVAL;
+
+	case GYROIOCSSAMPLERATE:
+		return set_samplerate(arg);
+
+	case GYROIOCGSAMPLERATE:
+		return _current_rate;
+
+	case GYROIOCSLOWPASS:
+	case GYROIOCGLOWPASS:
+		/* XXX not implemented due to wacky interaction with samplerate */
+		return -EINVAL;
+
+	case GYROIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
+		return OK;
+
+	case GYROIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
+		return OK;
+
+	case GYROIOCSRANGE:
+		return set_range(arg);
+
+	case GYROIOCGRANGE:
+		return _current_range;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+L3GD20::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+L3GD20::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+L3GD20::set_range(unsigned max_dps)
+{
+	uint8_t bits = REG4_BDU;
+
+	if (max_dps == 0)
+		max_dps = 2000;
+
+	if (max_dps <= 250) {
+		_current_range = 250;
+		bits |= RANGE_250DPS;
+
+	} else if (max_dps <= 500) {
+		_current_range = 500;
+		bits |= RANGE_500DPS;
+
+	} else if (max_dps <= 2000) {
+		_current_range = 2000;
+		bits |= RANGE_2000DPS;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
+	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
+	write_reg(ADDR_CTRL_REG4, bits);
+
+	return OK;
+}
+
+int
+L3GD20::set_samplerate(unsigned frequency)
+{
+	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
+
+	if (frequency == 0)
+		frequency = 760;
+
+	if (frequency <= 95) {
+		_current_rate = 95;
+		bits |= RATE_95HZ_LP_25HZ;
+
+	} else if (frequency <= 190) {
+		_current_rate = 190;
+		bits |= RATE_190HZ_LP_25HZ;
+
+	} else if (frequency <= 380) {
+		_current_rate = 380;
+		bits |= RATE_380HZ_LP_30HZ;
+
+	} else if (frequency <= 760) {
+		_current_rate = 760;
+		bits |= RATE_760HZ_LP_30HZ;
+
+	} else {
+		return -EINVAL;
+	}
+
+	write_reg(ADDR_CTRL_REG1, bits);
+
+	return OK;
+}
+
+void
+L3GD20::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_report = _next_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
+}
+
+void
+L3GD20::stop()
+{
+	hrt_cancel(&_call);
+}
+
+void
+L3GD20::measure_trampoline(void *arg)
+{
+	L3GD20 *dev = (L3GD20 *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+L3GD20::measure()
+{
+	/* status register and data as read back from the device */
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		temp;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_report;
+#pragma pack(pop)
+
+	gyro_report		*report = &_reports[_next_report];
+
+	/* start the performance counter */
+	perf_begin(_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+	report->timestamp = hrt_absolute_time();
+	
+	/* swap x and y and negate y */
+	report->x_raw = raw_report.y;
+	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+	report->z_raw = raw_report.z;
+
+	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	report->scaling = _gyro_range_scale;
+	report->range_rad_s = _gyro_range_rad_s;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_report == _oldest_report)
+		INCREMENT(_oldest_report, _num_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+
+	/* stop the perf counter */
+	perf_end(_sample_perf);
+}
+
+void
+L3GD20::print_info()
+{
+	perf_print_counter(_sample_perf);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace l3gd20
+{
+
+L3GD20	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd_gyro = -1;
+	struct gyro_report g_report;
+	ssize_t sz;
+
+	/* get the driver */
+	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd_gyro < 0)
+		err(1, "%s open failed", GYRO_DEVICE_PATH);
+
+	/* reset to manual polling */
+	if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
+		err(1, "reset to manual polling");
+
+	/* do a simple demand read */
+	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
+	if (sz != sizeof(g_report))
+		err(1, "immediate gyro read failed");
+
+	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
+	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
+	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
+	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
+	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
+	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
+	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
+
+	/* XXX add poll-rate tests here too */
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+l3gd20_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		l3gd20::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		l3gd20::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		l3gd20::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		l3gd20::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
new file mode 100644
index 000000000..23e77e871
--- /dev/null
+++ b/src/drivers/l3gd20/module.mk
@@ -0,0 +1,6 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND	 = l3gd20
+SRCS		 = l3gd20.cpp
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
new file mode 100644
index 000000000..04b565358
--- /dev/null
+++ b/src/drivers/led/led.cpp
@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ *   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 led.cpp
+ *
+ * LED driver.
+ */
+
+#include <nuttx/config.h>
+#include <drivers/device/device.h>
+#include <drivers/drv_led.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+class LED : device::CDev
+{
+public:
+	LED();
+	virtual ~LED();
+
+	virtual int		init();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+};
+
+LED::LED() :
+	CDev("led", LED_DEVICE_PATH)
+{
+	// force immediate init/device registration
+	init();
+}
+
+LED::~LED()
+{
+}
+
+int
+LED::init()
+{
+	CDev::init();
+	led_init();
+
+	return 0;
+}
+
+int
+LED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int result = OK;
+
+	switch (cmd) {
+	case LED_ON:
+		led_on(arg);
+		break;
+
+	case LED_OFF:
+		led_off(arg);
+		break;
+
+	default:
+		result = CDev::ioctl(filp, cmd, arg);
+	}
+	return result;
+}
+
+namespace
+{
+LED	*gLED;
+}
+
+void
+drv_led_start()
+{
+	if (gLED == nullptr) {
+		gLED = new LED;
+		if (gLED != nullptr)
+			gLED->init();
+	}
+}
\ No newline at end of file
diff --git a/src/drivers/led/module.mk b/src/drivers/led/module.mk
new file mode 100644
index 000000000..777f3e442
--- /dev/null
+++ b/src/drivers/led/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Build the LED driver.
+#
+
+SRCS		= led.cpp
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
new file mode 100644
index 000000000..397686e8b
--- /dev/null
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -0,0 +1,840 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 mb12xx.cpp
+ * @author Greg Hulands
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Configuration Constants */
+#define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
+#define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+
+/* MB12xx Registers addresses */
+
+#define MB12XX_TAKE_RANGE_REG	0x51		/* Measure range Register */
+#define MB12XX_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
+#define MB12XX_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+
+/* Device limits */
+#define MB12XX_MIN_DISTANCE (0.20f)
+#define MB12XX_MAX_DISTANCE (7.65f)
+	 
+#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class MB12XX : public device::I2C
+{
+public:
+	MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
+	virtual ~MB12XX();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	unsigned			_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	range_finder_report	*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	
+	orb_advert_t		_range_finder_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
+	* and MB12XX_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
+
+MB12XX::MB12XX(int bus, int address) :
+	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
+	_min_distance(MB12XX_MIN_DISTANCE),
+	_max_distance(MB12XX_MAX_DISTANCE),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+MB12XX::~MB12XX()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+MB12XX::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct range_finder_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	_oldest_report = _next_report = 0;
+
+	/* get a publish handle on the range finder topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
+
+	if (_range_finder_topic < 0)
+		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+MB12XX::probe()
+{
+	return measure();
+}
+
+void
+MB12XX::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}	
+
+void
+MB12XX::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}	
+
+float
+MB12XX::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+MB12XX::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* add one to account for the sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct range_finder_report *buf = new struct range_finder_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+		
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	case RANGEFINDERIOCSETMINIUMDISTANCE:
+	{
+		set_minimum_distance(*(float *)arg);
+		return 0;
+	}
+	break;
+	case RANGEFINDERIOCSETMAXIUMDISTANCE:
+	{
+		set_maximum_distance(*(float *)arg);
+		return 0;
+	}
+	break;
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+MB12XX::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_oldest_report = _next_report = 0;
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(MB12XX_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+MB12XX::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+MB12XX::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 2);
+	
+	if (ret < 0)
+	{
+		log("error reading from sensor: %d", ret);
+		return ret;
+	}
+	
+	uint16_t distance = val[0] << 8 | val[1];
+	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	_reports[_next_report].distance = si_units;
+	_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	
+	/* publish it */
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, toss it */
+	if (_next_report == _oldest_report) {
+		perf_count(_buffer_overflows);
+		INCREMENT(_oldest_report, _num_reports);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+out:
+	perf_end(_sample_perf);
+	return ret;
+	
+	return ret;
+}
+
+void
+MB12XX::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_RANGEFINDER};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+MB12XX::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+MB12XX::cycle_trampoline(void *arg)
+{
+	MB12XX *dev = (MB12XX *)arg;
+
+	dev->cycle();
+}
+
+void
+MB12XX::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MB12XX::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MB12XX::cycle_trampoline,
+		   this,
+		   USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+}
+
+void
+MB12XX::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mb12xx
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+MB12XX	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new MB12XX(MB12XX_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+mb12xx_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		mb12xx::start();
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 mb12xx::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		mb12xx::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		mb12xx::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		mb12xx::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/mb12xx/module.mk b/src/drivers/mb12xx/module.mk
new file mode 100644
index 000000000..4e00ada02
--- /dev/null
+++ b/src/drivers/mb12xx/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2013 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.
+#
+############################################################################
+
+#
+# Makefile to build the Maxbotix Sonar driver.
+#
+
+MODULE_COMMAND	= mb12xx
+
+SRCS		= mb12xx.cpp
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
new file mode 100644
index 000000000..71932ad65
--- /dev/null
+++ b/src/drivers/md25/md25.cpp
@@ -0,0 +1,553 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#include "md25.hpp"
+#include <poll.h>
+#include <stdio.h>
+
+#include <systemlib/err.h>
+#include <arch/board/board.h>
+
+// registers
+enum {
+	// RW: read/write
+	// R: read
+	REG_SPEED1_RW = 0,
+	REG_SPEED2_RW,
+	REG_ENC1A_R,
+	REG_ENC1B_R,
+	REG_ENC1C_R,
+	REG_ENC1D_R,
+	REG_ENC2A_R,
+	REG_ENC2B_R,
+	REG_ENC2C_R,
+	REG_ENC2D_R,
+	REG_BATTERY_VOLTS_R,
+	REG_MOTOR1_CURRENT_R,
+	REG_MOTOR2_CURRENT_R,
+	REG_SW_VERSION_R,
+	REG_ACCEL_RATE_RW,
+	REG_MODE_RW,
+	REG_COMMAND_RW,
+};
+
+MD25::MD25(const char *deviceName, int bus,
+	   uint16_t address, uint32_t speed) :
+	I2C("MD25", deviceName, bus, address, speed),
+	_controlPoll(),
+	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_version(0),
+	_motor1Speed(0),
+	_motor2Speed(0),
+	_revolutions1(0),
+	_revolutions2(0),
+	_batteryVoltage(0),
+	_motor1Current(0),
+	_motor2Current(0),
+	_motorAccel(0),
+	_mode(MODE_UNSIGNED_SPEED),
+	_command(CMD_RESET_ENCODERS)
+{
+	// setup control polling
+	_controlPoll.fd = _actuators.getHandle();
+	_controlPoll.events = POLLIN;
+
+	// if initialization fails raise an error, unless
+	// probing
+	int ret = I2C::init();
+
+	if (ret != OK) {
+		warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address);
+	}
+
+	// setup default settings, reset encoders
+	setMotor1Speed(0);
+	setMotor2Speed(0);
+	resetEncoders();
+	_setMode(MD25::MODE_UNSIGNED_SPEED);
+	setSpeedRegulation(true);
+	setTimeout(true);
+}
+
+MD25::~MD25()
+{
+}
+
+int MD25::readData()
+{
+	uint8_t sendBuf[1];
+	sendBuf[0] = REG_SPEED1_RW;
+	uint8_t recvBuf[17];
+	int ret = transfer(sendBuf, sizeof(sendBuf),
+			   recvBuf, sizeof(recvBuf));
+
+	if (ret == OK) {
+		_version = recvBuf[REG_SW_VERSION_R];
+		_motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]);
+		_motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]);
+		_revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) +
+					 (recvBuf[REG_ENC1B_R] << 16) +
+					 (recvBuf[REG_ENC1C_R] << 8)  +
+					 recvBuf[REG_ENC1D_R]) / 360.0;
+		_revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) +
+					 (recvBuf[REG_ENC2B_R] << 16) +
+					 (recvBuf[REG_ENC2C_R] << 8)  +
+					 recvBuf[REG_ENC2D_R]) / 360.0;
+		_batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0;
+		_motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0;
+		_motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0;
+		_motorAccel = recvBuf[REG_ACCEL_RATE_RW];
+		_mode = e_mode(recvBuf[REG_MODE_RW]);
+		_command = e_cmd(recvBuf[REG_COMMAND_RW]);
+	}
+
+	return ret;
+}
+
+void MD25::status(char *string, size_t n)
+{
+	snprintf(string, n,
+		 "version:\t%10d\n" \
+		 "motor 1 speed:\t%10.2f\n" \
+		 "motor 2 speed:\t%10.2f\n" \
+		 "revolutions 1:\t%10.2f\n" \
+		 "revolutions 2:\t%10.2f\n" \
+		 "battery volts :\t%10.2f\n" \
+		 "motor 1 current :\t%10.2f\n" \
+		 "motor 2 current :\t%10.2f\n" \
+		 "motor accel :\t%10d\n" \
+		 "mode :\t%10d\n" \
+		 "command :\t%10d\n",
+		 getVersion(),
+		 double(getMotor1Speed()),
+		 double(getMotor2Speed()),
+		 double(getRevolutions1()),
+		 double(getRevolutions2()),
+		 double(getBatteryVolts()),
+		 double(getMotor1Current()),
+		 double(getMotor2Current()),
+		 getMotorAccel(),
+		 getMode(),
+		 getCommand());
+}
+
+uint8_t MD25::getVersion()
+{
+	return _version;
+}
+
+float MD25::getMotor1Speed()
+{
+	return _motor1Speed;
+}
+
+float MD25::getMotor2Speed()
+{
+	return _motor2Speed;
+}
+
+float MD25::getRevolutions1()
+{
+	return _revolutions1;
+}
+
+float MD25::getRevolutions2()
+{
+	return _revolutions2;
+}
+
+float MD25::getBatteryVolts()
+{
+	return _batteryVoltage;
+}
+
+float MD25::getMotor1Current()
+{
+	return _motor1Current;
+}
+float MD25::getMotor2Current()
+{
+	return _motor2Current;
+}
+
+uint8_t MD25::getMotorAccel()
+{
+	return _motorAccel;
+}
+
+MD25::e_mode MD25::getMode()
+{
+	return _mode;
+}
+
+MD25::e_cmd MD25::getCommand()
+{
+	return _command;
+}
+
+int MD25::resetEncoders()
+{
+	return _writeUint8(REG_COMMAND_RW,
+			   CMD_RESET_ENCODERS);
+}
+
+int MD25::_setMode(e_mode mode)
+{
+	return _writeUint8(REG_MODE_RW,
+			   mode);
+}
+
+int MD25::setSpeedRegulation(bool enable)
+{
+	if (enable) {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_ENABLE_SPEED_REGULATION);
+
+	} else {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_DISABLE_SPEED_REGULATION);
+	}
+}
+
+int MD25::setTimeout(bool enable)
+{
+	if (enable) {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_ENABLE_TIMEOUT);
+
+	} else {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_DISABLE_TIMEOUT);
+	}
+}
+
+int MD25::setDeviceAddress(uint8_t address)
+{
+	uint8_t sendBuf[1];
+	sendBuf[0] = CMD_CHANGE_I2C_SEQ_0;
+	int ret = OK;
+	ret = transfer(sendBuf, sizeof(sendBuf),
+		       nullptr, 0);
+
+	if (ret != OK) {
+		warnc(ret, "MD25::setDeviceAddress");
+		return ret;
+	}
+
+	usleep(5000);
+	sendBuf[0] = CMD_CHANGE_I2C_SEQ_1;
+	ret = transfer(sendBuf, sizeof(sendBuf),
+		       nullptr, 0);
+
+	if (ret != OK) {
+		warnc(ret, "MD25::setDeviceAddress");
+		return ret;
+	}
+
+	usleep(5000);
+	sendBuf[0] = CMD_CHANGE_I2C_SEQ_2;
+	ret = transfer(sendBuf, sizeof(sendBuf),
+		       nullptr, 0);
+
+	if (ret != OK) {
+		warnc(ret, "MD25::setDeviceAddress");
+		return ret;
+	}
+
+	return OK;
+}
+
+int MD25::setMotor1Speed(float value)
+{
+	return _writeUint8(REG_SPEED1_RW,
+			   _normToUint8(value));
+}
+
+int MD25::setMotor2Speed(float value)
+{
+	return _writeUint8(REG_SPEED2_RW,
+			   _normToUint8(value));
+}
+
+void MD25::update()
+{
+	// wait for an actuator publication,
+	// check for exit condition every second
+	// note "::poll" is required to distinguish global
+	// poll from member function for driver
+	if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+
+	// if new data, send to motors
+	if (_actuators.updated()) {
+		_actuators.update();
+		setMotor1Speed(_actuators.control[CH_SPEED_LEFT]);
+		setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]);
+	}
+}
+
+int MD25::probe()
+{
+	uint8_t goodAddress = 0;
+	bool found = false;
+	int ret = OK;
+
+	// try initial address first, if good, then done
+	if (readData() == OK) return ret;
+
+	// try all other addresses
+	uint8_t testAddress = 0;
+
+	//printf("searching for MD25 address\n");
+	while (true) {
+		set_address(testAddress);
+		ret = readData();
+
+		if (ret == OK && !found) {
+			//printf("device found at address: 0x%X\n", testAddress);
+			if (!found) {
+				found = true;
+				goodAddress = testAddress;
+			}
+		}
+
+		if (testAddress > 254) {
+			break;
+		}
+
+		testAddress++;
+	}
+
+	if (found) {
+		set_address(goodAddress);
+		return OK;
+
+	} else {
+		set_address(0);
+		return ret;
+	}
+}
+
+int MD25::search()
+{
+	uint8_t goodAddress = 0;
+	bool found = false;
+	int ret = OK;
+	// try all other addresses
+	uint8_t testAddress = 0;
+
+	//printf("searching for MD25 address\n");
+	while (true) {
+		set_address(testAddress);
+		ret = readData();
+
+		if (ret == OK && !found) {
+			printf("device found at address: 0x%X\n", testAddress);
+
+			if (!found) {
+				found = true;
+				goodAddress = testAddress;
+			}
+		}
+
+		if (testAddress > 254) {
+			break;
+		}
+
+		testAddress++;
+	}
+
+	if (found) {
+		set_address(goodAddress);
+		return OK;
+
+	} else {
+		set_address(0);
+		return ret;
+	}
+}
+
+int MD25::_writeUint8(uint8_t reg, uint8_t value)
+{
+	uint8_t sendBuf[2];
+	sendBuf[0] = reg;
+	sendBuf[1] = value;
+	return transfer(sendBuf, sizeof(sendBuf),
+			nullptr, 0);
+}
+
+int MD25::_writeInt8(uint8_t reg, int8_t value)
+{
+	uint8_t sendBuf[2];
+	sendBuf[0] = reg;
+	sendBuf[1] = value;
+	return transfer(sendBuf, sizeof(sendBuf),
+			nullptr, 0);
+}
+
+float MD25::_uint8ToNorm(uint8_t value)
+{
+	// TODO, should go from 0 to 255
+	// possibly should handle this differently
+	return (value - 128) / 127.0;
+}
+
+uint8_t MD25::_normToUint8(float value)
+{
+	if (value > 1) value = 1;
+
+	if (value < -1) value = -1;
+
+	// TODO, should go from 0 to 255
+	// possibly should handle this differently
+	return 127 * value + 128;
+}
+
+int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
+{
+	printf("md25 test: starting\n");
+
+	// setup
+	MD25 md25("/dev/md25", bus, address);
+
+	// print status
+	char buf[200];
+	md25.status(buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	// setup for test
+	md25.setSpeedRegulation(true);
+	md25.setTimeout(true);
+	float dt = 0.1;
+	float speed = 0.2;
+	float t = 0;
+
+	// motor 1 test
+	printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n");
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor1Speed(speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions1() > 1) {
+			printf("finished 1 revolution fwd\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor1Speed(0);
+	printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
+	md25.resetEncoders();
+
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor1Speed(-speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions1() < -1) {
+			printf("finished 1 revolution rev\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor1Speed(0);
+	printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
+	md25.resetEncoders();
+
+	// motor 2 test
+	printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n");
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor2Speed(speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions2() > 1) {
+			printf("finished 1 revolution fwd\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor2Speed(0);
+	printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
+	md25.resetEncoders();
+
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor2Speed(-speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions2() < -1) {
+			printf("finished 1 revolution rev\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor2Speed(0);
+	printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
+	md25.resetEncoders();
+
+	printf("Test complete\n");
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
new file mode 100644
index 000000000..e77511b16
--- /dev/null
+++ b/src/drivers/md25/md25.hpp
@@ -0,0 +1,293 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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 md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <stdio.h>
+#include <controllib/block/UOrbSubscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <drivers/device/i2c.h>
+
+/**
+ * This is a driver for the MD25 motor controller utilizing the I2C interface.
+ */
+class MD25 : public device::I2C
+{
+public:
+
+	/**
+	* modes
+	*
+	* NOTE: this driver assumes we are always
+	* in mode 0!
+	*
+	* seprate speed mode:
+	*  motor speed1 = speed1
+	*  motor speed2 = speed2
+	* turn speed mode:
+	*	motor speed1 = speed1 + speed2
+	*  motor speed2 = speed2 - speed2
+	* unsigned:
+	*  full rev (0), stop(128), full fwd (255)
+	* signed:
+	*  full rev (-127), stop(0), full fwd (128)
+	*
+	* modes numbers:
+	* 0 : unsigned separate (default)
+	* 1 : signed separate
+	* 2 : unsigned turn
+	* 3 : signed turn
+	*/
+	enum e_mode {
+		MODE_UNSIGNED_SPEED = 0,
+		MODE_SIGNED_SPEED,
+		MODE_UNSIGNED_SPEED_TURN,
+		MODE_SIGNED_SPEED_TURN,
+	};
+
+	/** commands */
+	enum e_cmd {
+		CMD_RESET_ENCODERS = 32,
+		CMD_DISABLE_SPEED_REGULATION = 48,
+		CMD_ENABLE_SPEED_REGULATION = 49,
+		CMD_DISABLE_TIMEOUT = 50,
+		CMD_ENABLE_TIMEOUT = 51,
+		CMD_CHANGE_I2C_SEQ_0 = 160,
+		CMD_CHANGE_I2C_SEQ_1 = 170,
+		CMD_CHANGE_I2C_SEQ_2 = 165,
+	};
+
+	/** control channels */
+	enum e_channels {
+		CH_SPEED_LEFT = 0,
+		CH_SPEED_RIGHT
+	};
+
+	/**
+	 * constructor
+	 * @param deviceName the name of the device e.g. "/dev/md25"
+	 * @param bus the I2C bus
+	 * @param address the adddress on the I2C bus
+	 * @param speed the speed of the I2C communication
+	 */
+	MD25(const char *deviceName,
+	     int bus,
+	     uint16_t address,
+	     uint32_t speed = 100000);
+
+	/**
+	 * deconstructor
+	 */
+	virtual ~MD25();
+
+	/**
+	 * @return software version
+	 */
+	uint8_t getVersion();
+
+	/**
+	 * @return speed of motor, normalized (-1, 1)
+	 */
+	float getMotor1Speed();
+
+	/**
+	 * @return speed of motor 2, normalized (-1, 1)
+	 */
+	float getMotor2Speed();
+
+	/**
+	 * @return number of rotations since reset
+	 */
+	float getRevolutions1();
+
+	/**
+	 * @return number of rotations since reset
+	 */
+	float getRevolutions2();
+
+	/**
+	 * @return battery voltage, volts
+	 */
+	float getBatteryVolts();
+
+	/**
+	 * @return motor 1 current, amps
+	 */
+	float getMotor1Current();
+
+	/**
+	 * @return motor 2 current, amps
+	 */
+	float getMotor2Current();
+
+	/**
+	 * @return the motor acceleration
+	 * controls motor speed change (1-10)
+	 * accel rate  | time for full fwd. to full rev.
+	 *  1          | 6.375 s
+	 *  2          | 1.6 s
+	 *  3          | 0.675 s
+	 *  5(default) | 1.275 s
+	 * 10          | 0.65 s
+	 */
+	uint8_t getMotorAccel();
+
+	/**
+	 * @return motor output mode
+	 * */
+	e_mode getMode();
+
+	/**
+	 * @return current command register value
+	 */
+	e_cmd getCommand();
+
+	/**
+	 * resets the encoders
+	 * @return non-zero -> error
+	 * */
+	int resetEncoders();
+
+	/**
+	 * enable/disable speed regulation
+	 * @return non-zero -> error
+	 */
+	int setSpeedRegulation(bool enable);
+
+	/**
+	 * set the timeout for the motors
+	 * enable/disable timeout (motor stop)
+	 * after 2 sec of no i2c messages
+	 * @return non-zero -> error
+	 */
+	int setTimeout(bool enable);
+
+	/**
+	 * sets the device address
+	 * can only be done with one MD25
+	 * on the bus
+	 * @return non-zero -> error
+	 */
+	int setDeviceAddress(uint8_t address);
+
+	/**
+	 * set motor 1 speed
+	 * @param normSpeed normalize speed between -1 and 1
+	 * @return non-zero -> error
+	*/
+	int setMotor1Speed(float normSpeed);
+
+	/**
+	 * set motor 2 speed
+	 * @param normSpeed normalize speed between -1 and 1
+	 * @return non-zero -> error
+	 */
+	int setMotor2Speed(float normSpeed);
+
+	/**
+	 * main update loop that updates MD25 motor
+	 * speeds based on actuator publication
+	 */
+	void update();
+
+	/**
+	 * probe for device
+	 */
+	virtual int probe();
+
+	/**
+	 * search for device
+	 */
+	int search();
+
+	/**
+	 * read data from i2c
+	 */
+	int readData();
+
+	/**
+	 * print status
+	 */
+	void status(char *string, size_t n);
+
+private:
+	/** poll structure for control packets */
+	struct pollfd _controlPoll;
+
+	/** actuator controls subscription */
+	control::UOrbSubscription<actuator_controls_s> _actuators;
+
+	// local copy of data from i2c device
+	uint8_t _version;
+	float _motor1Speed;
+	float _motor2Speed;
+	float _revolutions1;
+	float _revolutions2;
+	float _batteryVoltage;
+	float _motor1Current;
+	float _motor2Current;
+	uint8_t _motorAccel;
+	e_mode _mode;
+	e_cmd _command;
+
+	// private methods
+	int _writeUint8(uint8_t reg, uint8_t value);
+	int _writeInt8(uint8_t reg, int8_t value);
+	float _uint8ToNorm(uint8_t value);
+	uint8_t _normToUint8(float value);
+
+	/**
+	 * set motor control mode,
+	 * this driver assumed we are always in mode 0
+	 * so we don't let the user change the mode
+	 * @return non-zero -> error
+	 */
+	int _setMode(e_mode);
+};
+
+// unit testing
+int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
new file mode 100644
index 000000000..80850e708
--- /dev/null
+++ b/src/drivers/md25/md25_main.cpp
@@ -0,0 +1,264 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+
+#include <arch/board/board.h>
+#include "md25.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int md25_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int md25_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int md25_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("md25 already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("md25",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 2048,
+					 md25_thread_main,
+					 (const char **)argv);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "test")) {
+
+		if (argc < 4) {
+			printf("usage: md25 test bus address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		md25Test(deviceName, bus, address);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "probe")) {
+		if (argc < 4) {
+			printf("usage: md25 probe bus address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		MD25 md25(deviceName, bus, address);
+
+		int ret = md25.probe();
+
+		if (ret == OK) {
+			printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address());
+
+		} else {
+			printf("MD25 not found on bus %d\n", bus);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "search")) {
+		if (argc < 3) {
+			printf("usage: md25 search bus\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		MD25 md25(deviceName, bus, address);
+
+		md25.search();
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "change_address")) {
+		if (argc < 5) {
+			printf("usage: md25 change_address bus old_address new_address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t old_address = strtoul(argv[3], nullptr, 0);
+
+		uint8_t new_address = strtoul(argv[4], nullptr, 0);
+
+		MD25 md25(deviceName, bus, old_address);
+
+		int ret = md25.setDeviceAddress(new_address);
+
+		if (ret == OK)  {
+			printf("MD25 new address set to 0x%X\n", new_address);
+
+		} else {
+			printf("MD25 failed to set address to 0x%X\n", new_address);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tmd25 app is running\n");
+
+		} else {
+			printf("\tmd25 app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int md25_thread_main(int argc, char *argv[])
+{
+	printf("[MD25] starting\n");
+
+	if (argc < 5) {
+		// extra md25 in arg list since this is a thread
+		printf("usage: md25 start bus address\n");
+		exit(0);
+	}
+
+	const char *deviceName = "/dev/md25";
+
+	uint8_t bus = strtoul(argv[3], nullptr, 0);
+
+	uint8_t address = strtoul(argv[4], nullptr, 0);
+
+	// start
+	MD25 md25("/dev/md25", bus, address);
+
+	thread_running = true;
+
+	// loop
+	while (!thread_should_exit) {
+		md25.update();
+	}
+
+	// exit
+	printf("[MD25] exiting.\n");
+	thread_running = false;
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk
new file mode 100644
index 000000000..13821a6b5
--- /dev/null
+++ b/src/drivers/md25/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2013 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.
+#
+############################################################################
+
+#
+# MD25 I2C Based Motor Controller
+# http://www.robot-electronics.co.uk/htm/md25tech.htm
+#
+
+MODULE_COMMAND		= md25
+
+SRCS			= md25.cpp \
+			  md25_main.cpp
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
new file mode 100644
index 000000000..3a735e26f
--- /dev/null
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -0,0 +1,1442 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012,2013 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 mkblctrl.cpp
+ *
+ * Driver/configurator for the Mikrokopter BL-Ctrl.
+ * Marco Bauer
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/i2c.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_mixer.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <systemlib/err.h>
+#include <systemlib/ppm_decode.h>
+
+#define I2C_BUS_SPEED					400000
+#define UPDATE_RATE						400
+#define MAX_MOTORS 						8
+#define BLCTRL_BASE_ADDR 				0x29
+#define BLCTRL_OLD 						0
+#define BLCTRL_NEW 						1
+#define BLCTRL_MIN_VALUE				-0.920F
+#define MOTOR_STATE_PRESENT_MASK		0x80
+#define MOTOR_STATE_ERROR_MASK		0x7F
+#define MOTOR_SPINUP_COUNTER			2500
+
+
+class MK : public device::I2C
+{
+public:
+	enum Mode {
+		MODE_2PWM,
+		MODE_4PWM,
+		MODE_NONE
+	};
+
+	enum MappingMode {
+		MAPPING_MK = 0,
+		MAPPING_PX4,
+	};
+
+	enum FrameType {
+		FRAME_PLUS = 0,
+		FRAME_X,
+	};
+
+	MK(int bus);
+	~MK();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+	virtual int	init(unsigned motors);
+
+	int		set_mode(Mode mode);
+	int		set_pwm_rate(unsigned rate);
+	int		set_motor_count(unsigned count);
+	int		set_motor_test(bool motortest);
+	int		set_px4mode(int px4mode);
+	int		set_frametype(int frametype);
+	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
+
+private:
+	static const unsigned _max_actuators = MAX_MOTORS;
+	static const bool	showDebug = false;
+
+	Mode		_mode;
+	int 		_update_rate;
+	int 		_current_update_rate;
+	int		_task;
+	int		_t_actuators;
+	int		_t_armed;
+	unsigned int		_motor;
+	int    _px4mode;
+	int    _frametype;
+	orb_advert_t	_t_outputs;
+	orb_advert_t	_t_actuators_effective;
+	unsigned int	_num_outputs;
+	bool		_primary_pwm_device;
+	bool     _motortest;
+
+	volatile bool	_task_should_exit;
+	bool		_armed;
+
+	unsigned long	debugCounter;
+
+	MixerGroup	*_mixers;
+
+	actuator_controls_s _controls;
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+	void		task_main() __attribute__((noreturn));
+
+	static int	control_callback(uintptr_t handle,
+					 uint8_t control_group,
+					 uint8_t control_index,
+					 float &input);
+
+	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+	struct GPIOConfig {
+		uint32_t	input;
+		uint32_t	output;
+		uint32_t	alt;
+	};
+
+	static const GPIOConfig	_gpio_tab[];
+	static const unsigned	_ngpio;
+
+	void		gpio_reset(void);
+	void		gpio_set_function(uint32_t gpios, int function);
+	void		gpio_write(uint32_t gpios, int function);
+	uint32_t	gpio_read(void);
+	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
+	int			mk_servo_arm(bool status);
+
+	int 		mk_servo_set(unsigned int chan, float val);
+	int 		mk_servo_set_test(unsigned int chan, float val);
+	int 		mk_servo_test(unsigned int chan);
+
+
+};
+
+const MK::GPIOConfig MK::_gpio_tab[] = {
+	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
+	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
+	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
+	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
+	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
+	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+};
+
+const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
+const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad + configuration
+const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 };	// Addresstranslator for Hexa + configuration
+const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 };	// Addresstranslator for Octo + configuration
+
+const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad X configuration
+const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 };	// Addresstranslator for Hexa X configuration
+const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 };	// Addresstranslator for Octo X configuration
+
+const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
+
+int addrTranslator[] = {0,0,0,0,0,0,0,0};
+
+struct MotorData_t
+{
+	unsigned int Version;                        // the version of the BL (0 = old)
+  unsigned int SetPoint;                       // written by attitude controller
+  unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+  unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+  unsigned int ReadMode;                       // select data to read
+  // the following bytes must be exactly in that order!
+  unsigned int Current;                        // in 0.1 A steps, read back from BL
+  unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+  unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+  unsigned int RoundCount;
+};
+
+MotorData_t Motor[MAX_MOTORS];
+
+
+namespace
+{
+
+MK	*g_mk;
+
+} // namespace
+
+MK::MK(int bus) :
+	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
+	_mode(MODE_NONE),
+	_update_rate(50),
+	_task(-1),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(0),
+	_t_actuators_effective(0),
+	_num_outputs(0),
+	_motortest(false),
+	_motor(-1),
+	_px4mode(MAPPING_MK),
+	_frametype(FRAME_PLUS),
+	_primary_pwm_device(false),
+	_task_should_exit(false),
+	_armed(false),
+	_mixers(nullptr)
+{
+	_debug_enabled = true;
+}
+
+MK::~MK()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		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;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	g_mk = nullptr;
+}
+
+int
+MK::init(unsigned motors)
+{
+	_num_outputs = motors;
+	debugCounter = 0;
+	int ret;
+	ASSERT(_task == -1);
+
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	usleep(500000);
+
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
+	/* reset GPIOs */
+	gpio_reset();
+
+	/* start the IO interface task */
+	_task = task_spawn("mkblctrl",
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_MAX -20,
+			   2048,
+			   (main_t)&MK::task_main_trampoline,
+			   nullptr);
+
+
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+MK::task_main_trampoline(int argc, char *argv[])
+{
+	g_mk->task_main();
+}
+
+int
+MK::set_mode(Mode mode)
+{
+	/*
+	 * Configure for PWM output.
+	 *
+	 * Note that regardless of the configured mode, the task is always
+	 * listening and mixing; the mode just selects which of the channels
+	 * are presented on the output pins.
+	 */
+	switch (mode) {
+	case MODE_2PWM:
+		if(_num_outputs == 4) {
+			//debug("MODE_QUAD");
+		} else if(_num_outputs == 6) {
+			//debug("MODE_HEXA");
+		} else if(_num_outputs == 8) {
+			//debug("MODE_OCTO");
+		}
+		//up_pwm_servo_init(0x3);
+		up_pwm_servo_deinit();
+		_update_rate = UPDATE_RATE;	/* default output rate */
+		break;
+
+	case MODE_4PWM:
+		if(_num_outputs == 4) {
+			//debug("MODE_QUADRO");
+		} else if(_num_outputs == 6) {
+			//debug("MODE_HEXA");
+		} else if(_num_outputs == 8) {
+			//debug("MODE_OCTO");
+		}
+		//up_pwm_servo_init(0xf);
+		up_pwm_servo_deinit();
+		_update_rate = UPDATE_RATE;	/* default output rate */
+		break;
+
+	case MODE_NONE:
+		debug("MODE_NONE");
+		/* disable servo outputs and set a very low update rate */
+		up_pwm_servo_deinit();
+		_update_rate = UPDATE_RATE;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	_mode = mode;
+	return OK;
+}
+
+int
+MK::set_pwm_rate(unsigned rate)
+{
+	if ((rate > 500) || (rate < 10))
+		return -EINVAL;
+
+	_update_rate = rate;
+	return OK;
+}
+
+int
+MK::set_px4mode(int px4mode)
+{
+	_px4mode = px4mode;
+}
+
+int
+MK::set_frametype(int frametype)
+{
+	_frametype = frametype;
+}
+
+
+int
+MK::set_motor_count(unsigned count)
+{
+	if(count > 0) {
+
+		_num_outputs = count;
+
+		if(_px4mode == MAPPING_MK) {
+			if(_frametype == FRAME_PLUS) {
+				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
+			} else if(_frametype == FRAME_X) {
+				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
+			}
+			if(_num_outputs == 4) {
+				if(_frametype == FRAME_PLUS) {
+					memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
+				} else if(_frametype == FRAME_X) {
+					memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
+				}
+			} else if(_num_outputs == 6) {
+				if(_frametype == FRAME_PLUS) {
+					memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
+				} else if(_frametype == FRAME_X) {
+					memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
+				}
+			} else if(_num_outputs == 8) {
+				if(_frametype == FRAME_PLUS) {
+					memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
+				} else if(_frametype == FRAME_X) {
+					memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
+				}
+			}
+		} else {
+			fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
+			memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
+		}
+
+		if(_num_outputs == 4) {
+			fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
+		} else 	if(_num_outputs == 6) {
+			fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
+		} else 	if(_num_outputs == 8) {
+			fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
+		}
+
+		return OK;
+
+	} else {
+		return -1;
+	}
+
+}
+
+int
+MK::set_motor_test(bool motortest)
+{
+	_motortest = motortest;
+	return OK;
+}
+
+
+void
+MK::task_main()
+{
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	/* force a reset of the update rate */
+	_current_update_rate = 0;
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	/* advertise the mixed control outputs */
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
+
+	/* advertise the effective control inputs */
+	actuator_controls_effective_s controls_effective;
+	memset(&controls_effective, 0, sizeof(controls_effective));
+	/* advertise the effective control inputs */
+	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+				   &controls_effective);
+
+	pollfd fds[2];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+
+	// rc input, published to ORB
+	struct rc_input_values rc_in;
+	orb_advert_t to_input_rc = 0;
+
+	memset(&rc_in, 0, sizeof(rc_in));
+	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+	
+	log("starting");
+	long update_rate_in_us = 0;
+
+	/* loop until killed */
+	while (!_task_should_exit) {
+
+		/* handle update rate changes */
+		if (_current_update_rate != _update_rate) {
+			int update_rate_in_ms = int(1000 / _update_rate);
+			update_rate_in_us = long(1000000 / _update_rate);
+
+			/* reject faster than 500 Hz updates */
+			if (update_rate_in_ms < 2) {
+				update_rate_in_ms = 2;
+				_update_rate = 500;
+			}
+			/* reject slower than 50 Hz updates */
+			if (update_rate_in_ms > 20) {
+				update_rate_in_ms = 20;
+				_update_rate = 50;
+			}
+
+			orb_set_interval(_t_actuators, update_rate_in_ms);
+			up_pwm_servo_set_rate(_update_rate);
+			_current_update_rate = _update_rate;
+		}
+
+		/* sleep waiting for data, but no more than a second */
+		int ret = ::poll(&fds[0], 2, 1000);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			usleep(1000000);
+			continue;
+		}
+
+		/* do we have a control update? */
+		if (fds[0].revents & POLLIN) {
+
+			/* get controls - must always do this to avoid spinning */
+			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+
+			/* can we mix? */
+			if (_mixers != nullptr) {
+
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				// XXX output actual limited values
+				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+				/* iterate actuators */
+				for (unsigned int i = 0; i < _num_outputs; i++) {
+
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						//outputs.output[i] = 1500 + (600 * outputs.output[i]);
+						//outputs.output[i] = 127 + (127 * outputs.output[i]);
+					} else {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						if(outputs.output[i] < -1.0f) {
+							outputs.output[i] = -1.0f;
+						} else if(outputs.output[i] > 1.0f) {
+							outputs.output[i] = 1.0f;
+						} else {
+							outputs.output[i] = -1.0f;
+						}
+					}
+
+					/* don't go under BLCTRL_MIN_VALUE */
+					if(outputs.output[i] < BLCTRL_MIN_VALUE) {
+						outputs.output[i] = BLCTRL_MIN_VALUE;
+					}
+					//_motortest = true;
+					/* output to BLCtrl's */
+					if(_motortest == true) {
+						mk_servo_test(i);
+					} else {
+						//mk_servo_set(i, outputs.output[i]);
+						mk_servo_set_test(i, outputs.output[i]);	// 8Bit
+					}
+
+
+				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+			}
+		}
+
+		/* how about an arming update? */
+		if (fds[1].revents & POLLIN) {
+			actuator_armed_s aa;
+
+			/* get new value */
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+
+			/* update PWM servo armed status if armed and not locked down */
+			////up_pwm_servo_arm(aa.armed && !aa.lockdown);
+			mk_servo_arm(aa.armed && !aa.lockdown);
+		}
+
+		// see if we have new PPM input data
+		if (ppm_last_valid_decode != rc_in.timestamp) {
+			// we have a new PPM frame. Publish it.
+			rc_in.channel_count = ppm_decoded_channels;
+			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
+				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
+			}
+			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+				rc_in.values[i] = ppm_buffer[i];
+			}
+			rc_in.timestamp = ppm_last_valid_decode;
+
+			/* lazily advertise on first publication */
+			if (to_input_rc == 0) {
+				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
+			} else {
+				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
+			}
+		}
+
+	}
+
+	::close(_t_actuators);
+	::close(_t_actuators_effective);
+	::close(_t_armed);
+
+
+	/* make sure servos are off */
+	up_pwm_servo_deinit();
+
+	log("stopping");
+
+	/* note - someone else is responsible for restoring the GPIO config */
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+
+int 
+MK::mk_servo_arm(bool status)
+{
+	_armed = status;
+	return 0;
+}
+
+
+unsigned int
+MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
+{
+	_retries = 50;
+	uint8_t foundMotorCount = 0;
+
+	for(unsigned i=0; i<MAX_MOTORS; i++) {
+		Motor[i].Version = 0;
+		Motor[i].SetPoint = 0;
+		Motor[i].SetPointLowerBits = 0;
+		Motor[i].State = 0;
+		Motor[i].ReadMode = 0;
+		Motor[i].Current = 0;
+		Motor[i].MaxPWM = 0;
+		Motor[i].Temperature = 0;
+		Motor[i].RoundCount = 0;
+	}
+
+	uint8_t msg = 0;
+	uint8_t result[3];
+
+	for(unsigned i=0; i< count; i++) {
+		result[0] = 0;
+		result[1] = 0;
+		result[2] = 0;
+			
+		set_address( BLCTRL_BASE_ADDR + i );
+		
+		if (OK == transfer(&msg, 1, &result[0], 3)) {
+			Motor[i].Current = result[0];
+			Motor[i].MaxPWM = result[1];
+			Motor[i].Temperature = result[2];
+			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
+			foundMotorCount++;
+			if(Motor[i].MaxPWM == 250) {
+				Motor[i].Version = BLCTRL_NEW;
+			} else {
+				Motor[i].Version = BLCTRL_OLD;
+			}
+		}
+	}
+
+	if(showOutput) {
+		fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
+		for(unsigned i=0; i< foundMotorCount; i++) {
+			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
+		}
+
+		if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+			_task_should_exit = true;
+		}
+	}
+
+	return foundMotorCount;
+}
+
+
+
+
+int
+MK::mk_servo_set(unsigned int chan, float val)
+{
+	float tmpVal = 0;
+	_retries = 0;
+	uint8_t result[3] = { 0,0,0 };
+	uint8_t msg[2] = { 0,0 };
+	uint8_t rod=0;
+	uint8_t bytesToSendBL2 = 2;
+
+
+	tmpVal = (1023 + (1023 * val));
+	if(tmpVal > 2047) {
+		tmpVal = 2047;
+	}
+
+
+	Motor[chan].SetPoint = (uint8_t) tmpVal / 3;	// divide 8
+	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8;	// rest of divide 8
+	//rod = (uint8_t) tmpVal % 8;
+	//Motor[chan].SetPointLowerBits = rod<<1;	// rest of divide 8
+	Motor[chan].SetPointLowerBits = 0;
+
+	if(_armed == false) {
+		Motor[chan].SetPoint = 0;
+		Motor[chan].SetPointLowerBits = 0;
+	}
+
+	//if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
+		set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+
+		if(Motor[chan].Version == BLCTRL_OLD) {
+			/*
+			*	Old BL-Ctrl 8Bit served. Version < 2.0
+			*/
+			msg[0] = Motor[chan].SetPoint;
+			if(Motor[chan].RoundCount >= 16) {
+				// on each 16th cyle we read out the status messages from the blctrl
+				if (OK == transfer(&msg[0], 1, &result[0], 2)) {
+					Motor[chan].Current = result[0];
+					Motor[chan].MaxPWM = result[1];
+					Motor[chan].Temperature = 255;;
+				} else {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+				Motor[chan].RoundCount = 0;
+			} else {
+				if (OK != transfer(&msg[0], 1, nullptr, 0)) {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+			}
+
+		} else {
+			/*
+			*	New BL-Ctrl 11Bit served. Version >= 2.0
+			*/
+			msg[0] = Motor[chan].SetPoint;
+			msg[1] = Motor[chan].SetPointLowerBits;
+
+			if(Motor[chan].SetPointLowerBits == 0) {
+				bytesToSendBL2 = 1;	// if setpoint lower bits are zero, we send only the higher bits - this saves time
+			}
+
+			if(Motor[chan].RoundCount >= 16) {
+				// on each 16th cyle we read out the status messages from the blctrl
+				if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
+					Motor[chan].Current = result[0];
+					Motor[chan].MaxPWM = result[1];
+					Motor[chan].Temperature = result[2];
+				} else {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+				Motor[chan].RoundCount = 0;
+			} else {
+				if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+			}
+
+		}
+
+		Motor[chan].RoundCount++;
+	//}
+
+	if(showDebug == true) {
+		debugCounter++;
+		if(debugCounter == 2000) {
+			debugCounter = 0;
+			for(int i=0; i<_num_outputs; i++){
+				if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
+					fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
+				}
+			}
+			fprintf(stderr, "\n");
+		}
+	}
+	return 0;
+}
+
+int
+MK::mk_servo_set_test(unsigned int chan, float val)
+{
+	_retries = 0;
+	int ret;
+
+	float tmpVal = 0;
+
+	uint8_t msg[2] = { 0,0 };
+
+	tmpVal = (1023 + (1023 * val));
+	if(tmpVal > 2048) {
+		tmpVal = 2048;
+	}
+
+	Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+
+	if(_armed == false) {
+		Motor[chan].SetPoint = 0;
+		Motor[chan].SetPointLowerBits = 0;
+	}
+
+	msg[0] = Motor[chan].SetPoint;
+
+	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+	ret = transfer(&msg[0], 1, nullptr, 0);
+
+	ret = OK;
+
+	return ret;
+}
+
+
+int
+MK::mk_servo_test(unsigned int chan)
+{
+	int ret=0;
+	float tmpVal = 0;
+	float val = -1;
+	_retries = 0;
+	uint8_t msg[2] = { 0,0 };
+
+	if(debugCounter >= MOTOR_SPINUP_COUNTER) {
+		debugCounter = 0;
+		_motor++;
+
+		if(_motor < _num_outputs) {
+			fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
+		}
+
+		if(_motor >= _num_outputs) {
+			_motor = -1;
+			_motortest = false;
+		}
+
+	}
+	debugCounter++;
+
+	if(_motor == chan) {
+		val = BLCTRL_MIN_VALUE;
+	} else {
+		val = -1;
+	}
+
+	tmpVal = (1023 + (1023 * val));
+	if(tmpVal > 2048) {
+		tmpVal = 2048;
+	}
+
+	//Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+	//Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
+	Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
+	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
+
+	if(_motor != chan) {
+		Motor[chan].SetPoint = 0;
+		Motor[chan].SetPointLowerBits = 0;
+	}
+
+	if(Motor[chan].Version == BLCTRL_OLD) {
+		msg[0] = Motor[chan].SetPoint;
+	} else {
+		msg[0] = Motor[chan].SetPoint;
+		msg[1] = Motor[chan].SetPointLowerBits;
+	}
+
+	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+	if(Motor[chan].Version == BLCTRL_OLD) {
+		ret = transfer(&msg[0], 1, nullptr, 0);
+	} else {
+		ret = transfer(&msg[0], 2, nullptr, 0);
+	}
+
+	return ret;
+}
+
+
+int
+MK::control_callback(uintptr_t handle,
+			 uint8_t control_group,
+			 uint8_t control_index,
+			 float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls->control[control_index];
+	return 0;
+}
+
+int
+MK::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret;
+
+	// XXX disabled, confusing users
+	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
+
+	/* try it as a GPIO ioctl first */
+	ret = gpio_ioctl(filp, cmd, arg);
+
+	if (ret != -ENOTTY)
+		return ret;
+
+	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	switch (_mode) {
+	case MODE_2PWM:
+	case MODE_4PWM:
+		ret = pwm_ioctl(filp, cmd, arg);
+		break;
+
+	default:
+		debug("not in a PWM mode");
+		break;
+	}
+
+	/* if nobody wants it, let CDev have it */
+	if (ret == -ENOTTY)
+		ret = CDev::ioctl(filp, cmd, arg);
+
+	return ret;
+}
+
+int
+MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+	int channel;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		////up_pwm_servo_arm(true);
+		mk_servo_arm(true);
+		break;
+
+	case PWM_SERVO_DISARM:
+		////up_pwm_servo_arm(false);
+		mk_servo_arm(false);
+		break;
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		set_pwm_rate(arg);
+		break;
+
+
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+
+		/* fake an update to the selected 'servo' channel */
+		if ((arg >= 0) && (arg <= 255)) {
+			channel = cmd - PWM_SERVO_SET(0);
+			//mk_servo_set(channel, arg);
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
+		/* copy the current output value from the channel */
+		*(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
+		break;
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		/*
+		if (_mode == MODE_4PWM) {
+			*(unsigned *)arg = 4;
+		} else {
+			*(unsigned *)arg = 2;
+		}
+		 */
+
+		*(unsigned *)arg = _num_outputs;
+
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+		}
+
+		break;
+
+	case MIXERIOCADDSIMPLE: {
+			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
+
+			if (mixer->check()) {
+				delete mixer;
+				ret = -EINVAL;
+
+			} else {
+				if (_mixers == nullptr)
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
+
+				_mixers->add_mixer(mixer);
+			}
+
+			break;
+		}
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
+			if (_mixers == nullptr) {
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					debug("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					ret = -EINVAL;
+				}
+			}
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+MK::gpio_reset(void)
+{
+	/*
+	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
+	 * to input mode.
+	 */
+	for (unsigned i = 0; i < _ngpio; i++)
+		stm32_configgpio(_gpio_tab[i].input);
+
+	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+	stm32_configgpio(GPIO_GPIO_DIR);
+}
+
+void
+MK::gpio_set_function(uint32_t gpios, int function)
+{
+	/*
+	 * GPIOs 0 and 1 must have the same direction as they are buffered
+	 * by a shared 2-port driver.  Any attempt to set either sets both.
+	 */
+	if (gpios & 3) {
+		gpios |= 3;
+
+		/* flip the buffer to output mode if required */
+		if (GPIO_SET_OUTPUT == function)
+			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
+	}
+
+	/* configure selected GPIOs as required */
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (gpios & (1 << i)) {
+			switch (function) {
+			case GPIO_SET_INPUT:
+				stm32_configgpio(_gpio_tab[i].input);
+				break;
+
+			case GPIO_SET_OUTPUT:
+				stm32_configgpio(_gpio_tab[i].output);
+				break;
+
+			case GPIO_SET_ALT_1:
+				if (_gpio_tab[i].alt != 0)
+					stm32_configgpio(_gpio_tab[i].alt);
+
+				break;
+			}
+		}
+	}
+
+	/* flip buffer to input mode if required */
+	if ((GPIO_SET_INPUT == function) && (gpios & 3))
+		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+}
+
+void
+MK::gpio_write(uint32_t gpios, int function)
+{
+	int value = (function == GPIO_SET) ? 1 : 0;
+
+	for (unsigned i = 0; i < _ngpio; i++)
+		if (gpios & (1 << i))
+			stm32_gpiowrite(_gpio_tab[i].output, value);
+}
+
+uint32_t
+MK::gpio_read(void)
+{
+	uint32_t bits = 0;
+
+	for (unsigned i = 0; i < _ngpio; i++)
+		if (stm32_gpioread(_gpio_tab[i].input))
+			bits |= (1 << i);
+
+	return bits;
+}
+
+int
+MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int	ret = OK;
+
+	lock();
+
+	switch (cmd) {
+
+	case GPIO_RESET:
+		gpio_reset();
+		break;
+
+	case GPIO_SET_OUTPUT:
+	case GPIO_SET_INPUT:
+	case GPIO_SET_ALT_1:
+		gpio_set_function(arg, cmd);
+		break;
+
+	case GPIO_SET_ALT_2:
+	case GPIO_SET_ALT_3:
+	case GPIO_SET_ALT_4:
+		ret = -EINVAL;
+		break;
+
+	case GPIO_SET:
+	case GPIO_CLEAR:
+		gpio_write(arg, cmd);
+		break;
+
+	case GPIO_GET:
+		*(uint32_t *)arg = gpio_read();
+		break;
+
+	default:
+		ret = -ENOTTY;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+namespace
+{
+
+enum PortMode {
+	PORT_MODE_UNSET = 0,
+	PORT_FULL_GPIO,
+	PORT_FULL_SERIAL,
+	PORT_FULL_PWM,
+	PORT_GPIO_AND_SERIAL,
+	PORT_PWM_AND_SERIAL,
+	PORT_PWM_AND_GPIO,
+};
+
+enum MappingMode {
+	MAPPING_MK = 0,
+	MAPPING_PX4,
+};
+
+	enum FrameType {
+		FRAME_PLUS = 0,
+		FRAME_X,
+	};
+
+PortMode g_port_mode;
+
+int
+mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
+{
+	uint32_t gpio_bits;
+	int shouldStop = 0;
+	MK::Mode servo_mode;
+
+	/* reset to all-inputs */
+	g_mk->ioctl(0, GPIO_RESET, 0);
+
+	gpio_bits = 0;
+	servo_mode = MK::MODE_NONE;
+
+	switch (new_mode) {
+	case PORT_FULL_GPIO:
+	case PORT_MODE_UNSET:
+		/* nothing more to do here */
+		break;
+
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_FULL_PWM:
+		/* select 4-pin PWM mode */
+		servo_mode = MK::MODE_4PWM;
+		break;
+
+	case PORT_GPIO_AND_SERIAL:
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_SERIAL:
+		/* select 2-pin PWM mode */
+		servo_mode = MK::MODE_2PWM;
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_GPIO:
+		/* select 2-pin PWM mode */
+		servo_mode = MK::MODE_2PWM;
+		break;
+	}
+
+	/* adjust GPIO config for serial mode(s) */
+	if (gpio_bits != 0)
+		g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+
+	/* native PX4 addressing) */
+	g_mk->set_px4mode(px4mode);
+
+	/* set frametype (geometry) */
+	g_mk->set_frametype(frametype);
+
+	/* motortest if enabled */
+	g_mk->set_motor_test(motortest);
+
+
+	/* (re)set count of used motors */
+	////g_mk->set_motor_count(motorcount);
+	/* count used motors */
+
+	do {
+		if(g_mk->mk_check_for_blctrl(8, false) != 0) {
+			shouldStop = 4;
+		} else {
+			shouldStop++;
+		}
+		sleep(1);
+	} while ( shouldStop < 3);
+
+	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
+
+	/* (re)set the PWM output mode */
+	g_mk->set_mode(servo_mode);
+
+
+	if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
+		g_mk->set_pwm_rate(update_rate);
+
+	return OK;
+}
+
+int
+mk_start(unsigned bus, unsigned motors)
+{
+	int ret = OK;
+
+	if (g_mk == nullptr) {
+
+		g_mk = new MK(bus);
+
+		if (g_mk == nullptr) {
+			ret = -ENOMEM;
+
+		} else {
+			ret = g_mk->init(motors);
+
+			if (ret != OK) {
+				delete g_mk;
+				g_mk = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+
+} // namespace
+
+extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
+
+int
+mkblctrl_main(int argc, char *argv[])
+{
+	PortMode port_mode = PORT_FULL_PWM;
+	int pwm_update_rate_in_hz = UPDATE_RATE;
+	int motorcount = 8;
+	int bus = 1;
+	int px4mode = MAPPING_PX4;
+	int frametype = FRAME_PLUS;	// + plus is default
+	bool motortest = false;
+	bool showHelp = false;
+	bool newMode = false;
+
+	/*
+	 * optional parameters
+	 */
+	for (int i = 1; i < argc; i++) {
+
+		/* look for the optional i2c bus parameter */
+		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+			if (argc > i + 1) {
+				bus = atoi(argv[i + 1]);
+				newMode = true;
+				} else {
+				errx(1, "missing argument for i2c bus (-b)");
+				return 1;
+			}
+		}
+
+		/* look for the optional frame parameter */
+		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
+			if (argc > i + 1) {
+				if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
+					px4mode = MAPPING_MK;
+					newMode = true;
+					if(strcmp(argv[i + 1], "+") == 0) {
+						frametype = FRAME_PLUS;
+					} else {
+						frametype = FRAME_X;
+					}
+				} else {
+					errx(1, "only + or x for frametype supported !");
+				}
+			} else {
+				errx(1, "missing argument for mkmode (-mkmode)");
+				return 1;
+			}
+		}
+
+		/* look for the optional test parameter */
+		if (strcmp(argv[i], "-t") == 0) {
+			motortest = true;
+			newMode = true;
+		}
+
+		/* look for the optional -h --help parameter */
+		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
+			showHelp == true;
+		}
+
+	}
+
+	if(showHelp) {
+		fprintf(stderr, "mkblctrl: help:\n");
+		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
+		exit(1);
+	}
+
+
+	if (g_mk == nullptr) {
+		if (mk_start(bus, motorcount) != OK) {
+			errx(1, "failed to start the MK-BLCtrl driver");
+		} else {
+			newMode = true;
+		}
+	}
+
+
+	/* parameter set ? */
+	if (newMode) {
+		/* switch parameter */
+		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
+	}
+
+	/* test, etc. here g*/
+
+	exit(1);
+}
diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk
new file mode 100644
index 000000000..3ac263b00
--- /dev/null
+++ b/src/drivers/mkblctrl/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012,2013 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.
+#
+############################################################################
+
+#
+# Interface driver for the Mikrokopter BLCtrl
+#
+
+MODULE_COMMAND		= mkblctrl
+
+SRCS				= mkblctrl.cpp
+
+INCLUDE_DIRS		+= $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk
new file mode 100644
index 000000000..c7d9cd3ef
--- /dev/null
+++ b/src/drivers/mpu6000/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Makefile to build the MPU6000 driver.
+#
+
+MODULE_COMMAND	= mpu6000
+
+# XXX seems excessive, check if 2048 is not sufficient
+MODULE_STACKSIZE	 = 4096
+
+SRCS		= mpu6000.cpp
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
new file mode 100644
index 000000000..df1958186
--- /dev/null
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -0,0 +1,1219 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 mpu6000.cpp
+ *
+ * Driver for the Invensense MPU6000 connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/conversions.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+
+#define DIR_READ			0x80
+#define DIR_WRITE			0x00
+
+// MPU 6000 registers
+#define MPUREG_WHOAMI			0x75
+#define MPUREG_SMPLRT_DIV		0x19
+#define MPUREG_CONFIG			0x1A
+#define MPUREG_GYRO_CONFIG		0x1B
+#define MPUREG_ACCEL_CONFIG		0x1C
+#define MPUREG_FIFO_EN			0x23
+#define MPUREG_INT_PIN_CFG		0x37
+#define MPUREG_INT_ENABLE		0x38
+#define MPUREG_INT_STATUS		0x3A
+#define MPUREG_ACCEL_XOUT_H		0x3B
+#define MPUREG_ACCEL_XOUT_L		0x3C
+#define MPUREG_ACCEL_YOUT_H		0x3D
+#define MPUREG_ACCEL_YOUT_L		0x3E
+#define MPUREG_ACCEL_ZOUT_H		0x3F
+#define MPUREG_ACCEL_ZOUT_L		0x40
+#define MPUREG_TEMP_OUT_H		0x41
+#define MPUREG_TEMP_OUT_L		0x42
+#define MPUREG_GYRO_XOUT_H		0x43
+#define MPUREG_GYRO_XOUT_L		0x44
+#define MPUREG_GYRO_YOUT_H		0x45
+#define MPUREG_GYRO_YOUT_L		0x46
+#define MPUREG_GYRO_ZOUT_H		0x47
+#define MPUREG_GYRO_ZOUT_L		0x48
+#define MPUREG_USER_CTRL		0x6A
+#define MPUREG_PWR_MGMT_1		0x6B
+#define MPUREG_PWR_MGMT_2		0x6C
+#define MPUREG_FIFO_COUNTH		0x72
+#define MPUREG_FIFO_COUNTL		0x73
+#define MPUREG_FIFO_R_W			0x74
+#define MPUREG_PRODUCT_ID		0x0C
+
+// Configuration bits MPU 3000 and MPU 6000 (not revised)?
+#define BIT_SLEEP			0x40
+#define BIT_H_RESET			0x80
+#define BITS_CLKSEL			0x07
+#define MPU_CLK_SEL_PLLGYROX		0x01
+#define MPU_CLK_SEL_PLLGYROZ		0x03
+#define MPU_EXT_SYNC_GYROX		0x02
+#define BITS_FS_250DPS			0x00
+#define BITS_FS_500DPS			0x08
+#define BITS_FS_1000DPS			0x10
+#define BITS_FS_2000DPS			0x18
+#define BITS_FS_MASK			0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2	0x00
+#define BITS_DLPF_CFG_188HZ		0x01
+#define BITS_DLPF_CFG_98HZ		0x02
+#define BITS_DLPF_CFG_42HZ		0x03
+#define BITS_DLPF_CFG_20HZ		0x04
+#define BITS_DLPF_CFG_10HZ		0x05
+#define BITS_DLPF_CFG_5HZ		0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF	0x07
+#define BITS_DLPF_CFG_MASK		0x07
+#define BIT_INT_ANYRD_2CLEAR		0x10
+#define BIT_RAW_RDY_EN			0x01
+#define BIT_I2C_IF_DIS			0x10
+#define BIT_INT_STATUS_DATA		0x01
+
+// Product ID Description for MPU6000
+// high 4 bits 	low 4 bits
+// Product Name	Product Revision
+#define MPU6000ES_REV_C4		0x14
+#define MPU6000ES_REV_C5		0x15
+#define MPU6000ES_REV_D6		0x16
+#define MPU6000ES_REV_D7		0x17
+#define MPU6000ES_REV_D8		0x18
+#define MPU6000_REV_C4			0x54
+#define MPU6000_REV_C5			0x55
+#define MPU6000_REV_D6			0x56
+#define MPU6000_REV_D7			0x57
+#define MPU6000_REV_D8			0x58
+#define MPU6000_REV_D9			0x59
+#define MPU6000_REV_D10			0x5A
+
+
+class MPU6000_gyro;
+
+class MPU6000 : public device::SPI
+{
+public:
+	MPU6000(int bus, spi_dev_e device);
+	virtual ~MPU6000();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+	friend class MPU6000_gyro;
+
+	virtual ssize_t		gyro_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+	MPU6000_gyro		*_gyro;
+	uint8_t			_product;	/** product code */
+
+	struct hrt_call		_call;
+	unsigned		_call_interval;
+
+	struct accel_report	_accel_report;
+	struct accel_scale	_accel_scale;
+	float			_accel_range_scale;
+	float			_accel_range_m_s2;
+	orb_advert_t		_accel_topic;
+
+	struct gyro_report	_gyro_report;
+	struct gyro_scale	_gyro_scale;
+	float			_gyro_range_scale;
+	float			_gyro_range_rad_s;
+	orb_advert_t		_gyro_topic;
+
+	unsigned		_reads;
+	perf_counter_t		_sample_perf;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the MPU6000
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+	uint16_t		read_reg16(unsigned reg);
+
+	/**
+	 * Write a register in the MPU6000
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the MPU6000
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the MPU6000 measurement range.
+	 *
+	 * @param max_g		The maximum G value the range must support.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_range(unsigned max_g);
+
+	/**
+	 * Swap a 16-bit value read from the MPU6000 to native byte order.
+	 */
+	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
+
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
+
+	/*
+	  set low pass filter frequency
+	 */
+	void _set_dlpf_filter(uint16_t frequency_hz);
+
+};
+
+/**
+ * Helper class implementing the gyro driver node.
+ */
+class MPU6000_gyro : public device::CDev
+{
+public:
+	MPU6000_gyro(MPU6000 *parent);
+	~MPU6000_gyro();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+	friend class MPU6000;
+
+	void			parent_poll_notify();
+private:
+	MPU6000			*_parent;
+};
+
+/** driver 'main' command */
+extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
+
+MPU6000::MPU6000(int bus, spi_dev_e device) :
+	SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
+	_gyro(new MPU6000_gyro(this)),
+	_product(0),
+	_call_interval(0),
+	_accel_range_scale(0.0f),
+	_accel_range_m_s2(0.0f),
+	_accel_topic(-1),
+	_gyro_range_scale(0.0f),
+	_gyro_range_rad_s(0.0f),
+	_gyro_topic(-1),
+	_reads(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
+{
+	// disable debug() calls
+	_debug_enabled = false;
+
+	// default accel scale factors
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+
+	// default gyro scale factors
+	_gyro_scale.x_offset = 0;
+	_gyro_scale.x_scale  = 1.0f;
+	_gyro_scale.y_offset = 0;
+	_gyro_scale.y_scale  = 1.0f;
+	_gyro_scale.z_offset = 0;
+	_gyro_scale.z_scale  = 1.0f;
+
+	memset(&_accel_report, 0, sizeof(_accel_report));
+	memset(&_gyro_report, 0, sizeof(_gyro_report));
+	memset(&_call, 0, sizeof(_call));
+}
+
+MPU6000::~MPU6000()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* delete the gyro subdriver */
+	delete _gyro;
+
+	/* delete the perf counter */
+	perf_free(_sample_perf);
+}
+
+int
+MPU6000::init()
+{
+	int ret;
+
+	/* do SPI init (and probe) first */
+	ret = SPI::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("SPI setup failed");
+		return ret;
+	}
+
+	/* advertise sensor topics */
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
+	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
+
+	// Chip reset
+	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
+	up_udelay(10000);
+
+	// Wake up device and select GyroZ clock (better performance)
+	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
+	up_udelay(1000);
+
+	// Disable I2C bus (recommended on datasheet)
+	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+	up_udelay(1000);
+
+	// SAMPLE RATE
+	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	usleep(1000);
+
+	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
+	// was 90 Hz, but this ruins quality and does not improve the
+	// system response
+	_set_dlpf_filter(20);
+	usleep(1000);
+	// Gyro scale 2000 deg/s ()
+	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
+	usleep(1000);
+
+	// correct gyro scale factors
+	// scale to rad/s in SI units
+	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
+	// scaling factor:
+	// 1/(2^15)*(2000/180)*PI
+	_gyro_scale.x_offset = 0;
+	_gyro_scale.x_scale  = 1.0f;
+	_gyro_scale.y_offset = 0;
+	_gyro_scale.y_scale  = 1.0f;
+	_gyro_scale.z_offset = 0;
+	_gyro_scale.z_scale  = 1.0f;
+	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
+
+	// product-specific scaling
+	switch (_product) {
+	case MPU6000ES_REV_C4:
+	case MPU6000ES_REV_C5:
+	case MPU6000_REV_C4:
+	case MPU6000_REV_C5:
+		// Accel scale 8g (4096 LSB/g)
+		// Rev C has different scaling than rev D
+		write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
+		break;
+
+	case MPU6000ES_REV_D6:
+	case MPU6000ES_REV_D7:
+	case MPU6000ES_REV_D8:
+	case MPU6000_REV_D6:
+	case MPU6000_REV_D7:
+	case MPU6000_REV_D8:
+	case MPU6000_REV_D9:
+	case MPU6000_REV_D10:
+	// default case to cope with new chip revisions, which
+	// presumably won't have the accel scaling bug		
+	default:
+		// Accel scale 8g (4096 LSB/g)
+		write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
+		break;
+	}
+
+	// Correct accel scale factors of 4096 LSB/g
+	// scale to m/s^2 ( 1g = 9.81 m/s^2)
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+	_accel_range_scale = (9.81f / 4096.0f);
+	_accel_range_m_s2 = 8.0f * 9.81f;
+
+	usleep(1000);
+
+	// INT CFG => Interrupt on Data Ready
+	write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
+	usleep(1000);
+	write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
+	usleep(1000);
+
+	// Oscillator set
+	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
+	usleep(1000);
+
+	/* do CDev init for the gyro device node, keep it optional */
+	int gyro_ret = _gyro->init();
+
+	if (gyro_ret != OK) {
+		_gyro_topic = -1;
+	}
+
+	return ret;
+}
+
+int
+MPU6000::probe()
+{
+
+	/* look for a product ID we recognise */
+	_product = read_reg(MPUREG_PRODUCT_ID);
+
+	// verify product revision
+	switch (_product) {
+	case MPU6000ES_REV_C4:
+	case MPU6000ES_REV_C5:
+	case MPU6000_REV_C4:
+	case MPU6000_REV_C5:
+	case MPU6000ES_REV_D6:
+	case MPU6000ES_REV_D7:
+	case MPU6000ES_REV_D8:
+	case MPU6000_REV_D6:
+	case MPU6000_REV_D7:
+	case MPU6000_REV_D8:
+	case MPU6000_REV_D9:
+	case MPU6000_REV_D10:
+		debug("ID 0x%02x", _product);
+		return OK;
+	}
+
+	debug("unexpected ID 0x%02x", _product);
+	return -EIO;
+}
+
+/*
+  set the DLPF filter frequency. This affects both accel and gyro.
+ */
+void
+MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
+{
+	uint8_t filter;
+
+	/* 
+	   choose next highest filter frequency available
+	 */
+	if (frequency_hz <= 5) {
+		filter = BITS_DLPF_CFG_5HZ;
+	} else if (frequency_hz <= 10) {
+		filter = BITS_DLPF_CFG_10HZ;
+	} else if (frequency_hz <= 20) {
+		filter = BITS_DLPF_CFG_20HZ;
+	} else if (frequency_hz <= 42) {
+		filter = BITS_DLPF_CFG_42HZ;
+	} else if (frequency_hz <= 98) {
+		filter = BITS_DLPF_CFG_98HZ;
+	} else if (frequency_hz <= 188) {
+		filter = BITS_DLPF_CFG_188HZ;
+	} else if (frequency_hz <= 256) {
+		filter = BITS_DLPF_CFG_256HZ_NOLPF2;
+	} else {
+		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
+	}
+	write_reg(MPUREG_CONFIG, filter);
+}
+
+ssize_t
+MPU6000::read(struct file *filp, char *buffer, size_t buflen)
+{
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (buflen < sizeof(_accel_report))
+		return -ENOSPC;
+
+	/* if automatic measurement is not enabled */
+	if (_call_interval == 0)
+		measure();
+
+	/* copy out the latest reports */
+	memcpy(buffer, &_accel_report, sizeof(_accel_report));
+	ret = sizeof(_accel_report);
+
+	return ret;
+}
+
+int
+MPU6000::self_test()
+{
+	if (_reads == 0) {
+		measure();
+	}
+
+	/* return 0 on success, 1 else */
+	return (_reads > 0) ? 0 : 1;
+}
+
+ssize_t
+MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
+{
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (buflen < sizeof(_gyro_report))
+		return -ENOSPC;
+
+	/* if automatic measurement is not enabled */
+	if (_call_interval == 0)
+		measure();
+
+	/* copy out the latest report */
+	memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
+	ret = sizeof(_gyro_report);
+
+	return ret;
+}
+
+int
+MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_interval = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* XXX 500Hz is just a wild guess */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 500);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_call.period = _call_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_interval;
+
+	case SENSORIOCSQUEUEDEPTH:
+		/* XXX not implemented */
+		return -EINVAL;
+
+	case SENSORIOCGQUEUEDEPTH:
+		/* XXX not implemented */
+		return -EINVAL;
+
+
+	case ACCELIOCSSAMPLERATE:
+	case ACCELIOCGSAMPLERATE:
+		/* XXX not implemented */
+		return -EINVAL;
+
+	case ACCELIOCSLOWPASS:
+	case ACCELIOCGLOWPASS:
+		_set_dlpf_filter((uint16_t)arg);
+		return OK;
+
+	case ACCELIOCSSCALE:
+		{
+			/* copy scale, but only if off by a few percent */
+			struct accel_scale *s = (struct accel_scale *) arg;
+			float sum = s->x_scale + s->y_scale + s->z_scale;
+			if (sum > 2.0f && sum < 4.0f) {
+				memcpy(&_accel_scale, s, sizeof(_accel_scale));
+				return OK;
+			} else {
+				return -EINVAL;
+			}
+		}
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCSRANGE:
+	case ACCELIOCGRANGE:
+		/* XXX not implemented */
+		// XXX change these two values on set:
+		// _accel_range_scale = (9.81f / 4096.0f);
+		// _accel_range_rad_s = 8.0f * 9.81f;
+		return -EINVAL;
+
+	case ACCELIOCSELFTEST:
+		return self_test();
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+		/* these are shared with the accel side */
+	case SENSORIOCSPOLLRATE:
+	case SENSORIOCGPOLLRATE:
+	case SENSORIOCSQUEUEDEPTH:
+	case SENSORIOCGQUEUEDEPTH:
+	case SENSORIOCRESET:
+		return ioctl(filp, cmd, arg);
+
+	case GYROIOCSSAMPLERATE:
+	case GYROIOCGSAMPLERATE:
+		/* XXX not implemented */
+		return -EINVAL;
+
+	case GYROIOCSLOWPASS:
+	case GYROIOCGLOWPASS:
+		_set_dlpf_filter((uint16_t)arg);
+		return OK;
+
+	case GYROIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
+		return OK;
+
+	case GYROIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
+		return OK;
+
+	case GYROIOCSRANGE:
+	case GYROIOCGRANGE:
+		/* XXX not implemented */
+		// XXX change these two values on set:
+		// _gyro_range_scale = xx
+		// _gyro_range_m_s2 = xx
+		return -EINVAL;
+
+	case GYROIOCSELFTEST:
+		return self_test();
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+MPU6000::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+uint16_t
+MPU6000::read_reg16(unsigned reg)
+{
+	uint8_t cmd[3];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return (uint16_t)(cmd[1] << 8) | cmd[2];
+}
+
+void
+MPU6000::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+MPU6000::set_range(unsigned max_g)
+{
+#if 0
+	uint8_t rangebits;
+	float rangescale;
+
+	if (max_g > 16) {
+		return -ERANGE;
+
+	} else if (max_g > 8) {		/* 16G */
+		rangebits = OFFSET_LSB1_RANGE_16G;
+		rangescale = 1.98;
+
+	} else if (max_g > 4) {		/* 8G */
+		rangebits = OFFSET_LSB1_RANGE_8G;
+		rangescale = 0.99;
+
+	} else if (max_g > 3) {		/* 4G */
+		rangebits = OFFSET_LSB1_RANGE_4G;
+		rangescale = 0.5;
+
+	} else if (max_g > 2) {		/* 3G */
+		rangebits = OFFSET_LSB1_RANGE_3G;
+		rangescale = 0.38;
+
+	} else if (max_g > 1) {		/* 2G */
+		rangebits = OFFSET_LSB1_RANGE_2G;
+		rangescale = 0.25;
+
+	} else {			/* 1G */
+		rangebits = OFFSET_LSB1_RANGE_1G;
+		rangescale = 0.13;
+	}
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
+	_range_scale = rangescale;
+#endif
+	return OK;
+}
+
+void
+MPU6000::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
+}
+
+void
+MPU6000::stop()
+{
+	hrt_cancel(&_call);
+}
+
+void
+MPU6000::measure_trampoline(void *arg)
+{
+	MPU6000 *dev = (MPU6000 *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+MPU6000::measure()
+{
+#pragma pack(push, 1)
+	/**
+	 * Report conversation within the MPU6000, including command byte and
+	 * interrupt status.
+	 */
+	struct MPUReport {
+		uint8_t		cmd;
+		uint8_t		status;
+		uint8_t		accel_x[2];
+		uint8_t		accel_y[2];
+		uint8_t		accel_z[2];
+		uint8_t		temp[2];
+		uint8_t		gyro_x[2];
+		uint8_t		gyro_y[2];
+		uint8_t		gyro_z[2];
+	} mpu_report;
+#pragma pack(pop)
+
+	struct Report {
+		int16_t		accel_x;
+		int16_t		accel_y;
+		int16_t		accel_z;
+		int16_t		temp;
+		int16_t		gyro_x;
+		int16_t		gyro_y;
+		int16_t		gyro_z;
+	} report;
+
+	/* start measuring */
+	perf_begin(_sample_perf);
+
+	/*
+	 * Fetch the full set of measurements from the MPU6000 in one pass.
+	 */
+	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
+		return;
+
+	/* count measurement */
+	_reads++;
+
+	/*
+	 * Convert from big to little endian
+	 */
+
+	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
+	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
+	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
+
+	report.temp = int16_t_from_bytes(mpu_report.temp);
+
+	report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
+	report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
+	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
+
+	/*
+	 * Swap axes and negate y
+	 */
+	int16_t accel_xt = report.accel_y;
+	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
+
+	int16_t gyro_xt = report.gyro_y;
+	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
+
+	/*
+	 * Apply the swap
+	 */
+	report.accel_x = accel_xt;
+	report.accel_y = accel_yt;
+	report.gyro_x = gyro_xt;
+	report.gyro_y = gyro_yt;
+
+	/*
+	 * Adjust and scale results to m/s^2.
+	 */
+	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
+
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	/* NOTE: Axes have been swapped to match the board a few lines above. */
+
+	_accel_report.x_raw = report.accel_x;
+	_accel_report.y_raw = report.accel_y;
+	_accel_report.z_raw = report.accel_z;
+
+	_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	_accel_report.scaling = _accel_range_scale;
+	_accel_report.range_m_s2 = _accel_range_m_s2;
+
+	_accel_report.temperature_raw = report.temp;
+	_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
+
+	_gyro_report.x_raw = report.gyro_x;
+	_gyro_report.y_raw = report.gyro_y;
+	_gyro_report.z_raw = report.gyro_z;
+
+	_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	_gyro_report.scaling = _gyro_range_scale;
+	_gyro_report.range_rad_s = _gyro_range_rad_s;
+
+	_gyro_report.temperature_raw = report.temp;
+	_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+	_gyro->parent_poll_notify();
+
+	/* and publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
+	if (_gyro_topic != -1) {
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
+	}
+
+	/* stop measuring */
+	perf_end(_sample_perf);
+}
+
+void
+MPU6000::print_info()
+{
+	printf("reads:          %u\n", _reads);
+}
+
+MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
+	CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
+	_parent(parent)
+{
+}
+
+MPU6000_gyro::~MPU6000_gyro()
+{
+}
+
+void
+MPU6000_gyro::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->gyro_read(filp, buffer, buflen);
+}
+
+int
+MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	return _parent->gyro_ioctl(filp, cmd, arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mpu6000
+{
+
+MPU6000	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd = -1;
+	int fd_gyro = -1;
+	struct accel_report a_report;
+	struct gyro_report g_report;
+	ssize_t sz;
+
+	/* get the driver */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
+		    ACCEL_DEVICE_PATH);
+
+	/* get the driver */
+	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd_gyro < 0)
+		err(1, "%s open failed", GYRO_DEVICE_PATH);
+
+	/* reset to manual polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
+		err(1, "reset to manual polling");
+
+	/* do a simple demand read */
+	sz = read(fd, &a_report, sizeof(a_report));
+
+	if (sz != sizeof(a_report))
+		err(1, "immediate acc read failed");
+
+	warnx("single read");
+	warnx("time:     %lld", a_report.timestamp);
+	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
+	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
+	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
+	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
+	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
+	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
+	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
+	      (double)(a_report.range_m_s2 / 9.81f));
+
+	/* do a simple demand read */
+	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
+	if (sz != sizeof(g_report))
+		err(1, "immediate gyro read failed");
+
+	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
+	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
+	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
+	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
+	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
+	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
+	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
+
+	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
+	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
+
+
+	/* XXX add poll-rate tests here too */
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+mpu6000_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		mpu6000::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		mpu6000::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		mpu6000::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		mpu6000::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
new file mode 100644
index 000000000..3c4b0f093
--- /dev/null
+++ b/src/drivers/ms5611/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# MS5611 driver
+#
+
+MODULE_COMMAND	= ms5611
+
+SRCS		= ms5611.cpp
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
new file mode 100644
index 000000000..59ab5936e
--- /dev/null
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -0,0 +1,1214 @@
+/****************************************************************************
+ *
+ *   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 ms5611.cpp
+ * Driver for the MS5611 barometric pressure sensor connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct ms5611_prom_s {
+	uint16_t factory_setup;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t serial_and_crc;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union ms5611_prom_u {
+	uint16_t c[8];
+	struct ms5611_prom_s s;
+};
+#pragma pack(pop)
+
+class MS5611 : public device::I2C
+{
+public:
+	MS5611(int bus);
+	virtual ~MS5611();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+	union ms5611_prom_u	_prom;
+
+	struct work_s		_work;
+	unsigned		_measure_ticks;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	struct baro_report	*_reports;
+
+	bool			_collect_phase;
+	unsigned		_measure_phase;
+
+	/* intermediate temperature values per MS5611 datasheet */
+	int32_t			_TEMP;
+	int64_t			_OFF;
+	int64_t			_SENS;
+
+	/* altitude conversion calibration */
+	unsigned		_msl_pressure;	/* in kPa */
+
+	orb_advert_t		_baro_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	/**
+	 * Test whether the device supported by the driver is present at a
+	 * specific address.
+	 *
+	 * @param address	The I2C bus address to probe.
+	 * @return		True if the device is present.
+	 */
+	int			probe_address(uint8_t address);
+
+	/**
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void			start_cycle();
+
+	/**
+	 * Stop the automatic measurement state machine.
+	 */
+	void			stop_cycle();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			cycle();
+
+	/**
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * Issue a measurement command for the current state.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int			measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			collect();
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int			cmd_reset();
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int			read_prom();
+
+	/**
+	 * PROM CRC routine ported from MS5611 application note
+	 *
+	 * @param n_prom	Pointer to words read from PROM.
+	 * @return		True if the CRC matches.
+	 */
+	bool			crc4(uint16_t *n_prom);
+
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+/*
+ * MS5611 internal constants and data structures.
+ */
+
+/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
+#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+
+#define MS5611_BUS			PX4_I2C_BUS_ONBOARD
+#define MS5611_ADDRESS_1		PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
+
+#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
+#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
+
+
+MS5611::MS5611(int bus) :
+	I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
+	_measure_ticks(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_collect_phase(false),
+	_measure_phase(0),
+	_TEMP(0),
+	_OFF(0),
+	_SENS(0),
+	_msl_pressure(101325),
+	_baro_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
+	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+MS5611::~MS5611()
+{
+	/* make sure we are truly inactive */
+	stop_cycle();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+MS5611::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct baro_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	_oldest_report = _next_report = 0;
+
+	/* get a publish handle on the baro topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+
+	if (_baro_topic < 0)
+		debug("failed to create sensor_baro object");
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+MS5611::probe()
+{
+	_retries = 10;
+
+	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
+	    (OK == probe_address(MS5611_ADDRESS_2))) {
+		/*
+	    	 * Disable retries; we may enable them selectively in some cases,
+		 * but the device gets confused if we retry some of the commands.
+	    	 */
+		_retries = 0;
+		return OK;
+	}
+
+	return -EIO;
+}
+
+int
+MS5611::probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_address(address);
+
+	/* send reset command */
+	if (OK != cmd_reset())
+		return -EIO;
+
+	/* read PROM */
+	if (OK != read_prom())
+		return -EIO;
+
+	return OK;
+}
+
+ssize_t
+MS5611::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct baro_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_measure_phase = 0;
+		_oldest_report = _next_report = 0;
+
+		/* do temperature first */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop_cycle();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start_cycle();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start_cycle();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* add one to account for the sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct baro_report *buf = new struct baro_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop_cycle();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start_cycle();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case BAROIOCSMSLPRESSURE:
+
+		/* range-check for sanity */
+		if ((arg < 80000) || (arg > 120000))
+			return -EINVAL;
+
+		_msl_pressure = arg;
+		return OK;
+
+	case BAROIOCGMSLPRESSURE:
+		return _msl_pressure;
+
+	default:
+		break;
+	}
+
+	/* give it to the superclass */
+	return I2C::ioctl(filp, cmd, arg);
+}
+
+void
+MS5611::start_cycle()
+{
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_measure_phase = 0;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
+}
+
+void
+MS5611::stop_cycle()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+MS5611::cycle_trampoline(void *arg)
+{
+	MS5611 *dev = (MS5611 *)arg;
+
+	dev->cycle();
+}
+
+void
+MS5611::cycle()
+{
+	int ret;
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		ret = collect();
+		if (ret != OK) {
+			if (ret == -6) {
+				/*
+				 * The ms5611 seems to regularly fail to respond to
+				 * its address; this happens often enough that we'd rather not
+				 * spam the console with the message.
+				 */
+			} else {
+				//log("collection error %d", ret);
+			}
+			/* reset the collection state machine and try again */
+			start_cycle();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 * Don't inject one after temperature measurements, so we can keep
+		 * doing pressure measurements at something close to the desired rate.
+		 */
+		if ((_measure_phase != 0) &&
+		    (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MS5611::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	ret = measure();
+	if (ret != OK) {
+		//log("measure error %d", ret);
+		/* reset the collection state machine and try again */
+		start_cycle();
+		return;
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MS5611::cycle_trampoline,
+		   this,
+		   USEC2TICK(MS5611_CONVERSION_INTERVAL));
+}
+
+int
+MS5611::measure()
+{
+	int ret;
+
+	perf_begin(_measure_perf);
+
+	/*
+	 * In phase zero, request temperature; in other phases, request pressure.
+	 */
+	uint8_t	cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+
+	/*
+	 * Send the command to begin measuring.
+	 *
+	 * Disable retries on this command; we can't know whether failure 
+	 * means the device did or did not see the write.
+	 */
+	_retries = 0;
+	ret = transfer(&cmd_data, 1, nullptr, 0);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	perf_end(_measure_perf);
+
+	return ret;
+}
+
+int
+MS5611::collect()
+{
+	int ret;
+	uint8_t cmd;
+	uint8_t data[3];
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} cvt;
+
+	/* read the most recent measurement */
+	cmd = 0;
+
+	perf_begin(_sample_perf);
+
+	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+
+	ret = transfer(&cmd, 1, &data[0], 3);
+	if (ret != OK) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	/* fetch the raw value */
+	cvt.b[0] = data[2];
+	cvt.b[1] = data[1];
+	cvt.b[2] = data[0];
+	cvt.b[3] = 0;
+	uint32_t raw = cvt.w;
+
+	/* handle a measurement */
+	if (_measure_phase == 0) {
+
+		/* temperature offset (in ADC units) */
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+
+		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
+		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+
+		/* base sensor scale/offset values */
+		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
+		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+
+		/* temperature compensation */
+		if (_TEMP < 2000) {
+
+			int32_t T2 = POW2(dT) >> 31;
+
+			int64_t f = POW2((int64_t)_TEMP - 2000);
+			int64_t OFF2 = 5 * f >> 1;
+			int64_t SENS2 = 5 * f >> 2;
+
+			if (_TEMP < -1500) {
+				int64_t f2 = POW2(_TEMP + 1500);
+				OFF2 += 7 * f2;
+				SENS2 += 11 * f2 >> 1;
+			}
+
+			_TEMP -= T2;
+			_OFF  -= OFF2;
+			_SENS -= SENS2;
+		}
+
+	} else {
+
+		/* pressure calculation, result in Pa */
+		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+
+		/* generate a new report */
+		_reports[_next_report].temperature = _TEMP / 100.0f;
+		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
+
+		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
+
+		/*
+		 * PERFORMANCE HINT:
+		 *
+		 * The single precision calculation is 50 microseconds faster than the double
+		 * precision variant. It is however not obvious if double precision is required.
+		 * Pending more inspection and tests, we'll leave the double precision variant active.
+		 *
+		 * Measurements:
+		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
+		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
+		 */
+#if 0/* USE_FLOAT */
+		/* tropospheric properties (0-11km) for standard atmosphere */
+		const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
+		const float a  = -6.5f / 1000f;	/* temperature gradient in degrees per metre */
+		const float g  = 9.80665f;	/* gravity constant in m/s/s */
+		const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
+
+		/* current pressure at MSL in kPa */
+		float p1 = _msl_pressure / 1000.0f;
+
+		/* measured pressure in kPa */
+		float p = P / 1000.0f;
+
+		/*
+		 * Solve:
+		 *
+		 *     /        -(aR / g)     \
+		 *    | (p / p1)          . T1 | - T1
+		 *     \                      /
+		 * h = -------------------------------  + h1
+		 *                   a
+		 */
+		_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+#else
+		/* tropospheric properties (0-11km) for standard atmosphere */
+		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+		const double g  = 9.80665;	/* gravity constant in m/s/s */
+		const double R  = 287.05;	/* ideal gas constant in J/kg/K */
+
+		/* current pressure at MSL in kPa */
+		double p1 = _msl_pressure / 1000.0;
+
+		/* measured pressure in kPa */
+		double p = P / 1000.0;
+
+		/*
+		 * Solve:
+		 *
+		 *     /        -(aR / g)     \
+		 *    | (p / p1)          . T1 | - T1
+		 *     \                      /
+		 * h = -------------------------------  + h1
+		 *                   a
+		 */
+		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+#endif
+		/* publish it */
+		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
+
+		/* post a report to the ring - note, not locked */
+		INCREMENT(_next_report, _num_reports);
+
+		/* if we are running up against the oldest report, toss it */
+		if (_next_report == _oldest_report) {
+			perf_count(_buffer_overflows);
+			INCREMENT(_oldest_report, _num_reports);
+		}
+
+		/* notify anyone waiting for data */
+		poll_notify(POLLIN);
+	}
+
+	/* update the measurement state machine */
+	INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+int
+MS5611::cmd_reset()
+{
+	unsigned	old_retrycount = _retries;
+	uint8_t		cmd = ADDR_RESET_CMD;
+	int		result;
+
+	/* bump the retry count */
+	_retries = 10;
+	result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int
+MS5611::read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+
+		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
+			break;
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+bool
+MS5611::crc4(uint16_t *n_prom)
+{
+	int16_t cnt;
+	uint16_t n_rem;
+	uint16_t crc_read;
+	uint8_t n_bit;
+
+	n_rem = 0x00;
+
+	/* save the read crc */
+	crc_read = n_prom[7];
+
+	/* remove CRC byte */
+	n_prom[7] = (0xFF00 & (n_prom[7]));
+
+	for (cnt = 0; cnt < 16; cnt++) {
+		/* uneven bytes */
+		if (cnt & 1) {
+			n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
+
+		} else {
+			n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
+		}
+
+		for (n_bit = 8; n_bit > 0; n_bit--) {
+			if (n_rem & 0x8000) {
+				n_rem = (n_rem << 1) ^ 0x3000;
+
+			} else {
+				n_rem = (n_rem << 1);
+			}
+		}
+	}
+
+	/* final 4 bit remainder is CRC value */
+	n_rem = (0x000F & (n_rem >> 12));
+	n_prom[7] = crc_read;
+
+	/* return true if CRCs match */
+	return (0x000F & crc_read) == (n_rem ^ 0x00);
+}
+
+void
+MS5611::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+	printf("TEMP:           %d\n", _TEMP);
+	printf("SENS:           %lld\n", _SENS);
+	printf("OFF:            %lld\n", _OFF);
+	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
+
+	printf("factory_setup             %u\n", _prom.s.factory_setup);
+	printf("c1_pressure_sens          %u\n", _prom.s.c1_pressure_sens);
+	printf("c2_pressure_offset        %u\n", _prom.s.c2_pressure_offset);
+	printf("c3_temp_coeff_pres_sens   %u\n", _prom.s.c3_temp_coeff_pres_sens);
+	printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
+	printf("c5_reference_temp         %u\n", _prom.s.c5_reference_temp);
+	printf("c6_temp_coeff_temp        %u\n", _prom.s.c6_temp_coeff_temp);
+	printf("serial_and_crc            %u\n", _prom.s.serial_and_crc);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ms5611
+{
+
+MS5611	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+void	calibrate(unsigned altitude);
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new MS5611(MS5611_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct baro_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("pressure:    %10.4f", (double)report.pressure);
+	warnx("altitude:    %11.4f", (double)report.altitude);
+	warnx("temperature: %8.4f", (double)report.temperature);
+	warnx("time:        %lld", report.timestamp);
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+		errx(1, "failed to set queue depth");
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("pressure:    %10.4f", (double)report.pressure);
+		warnx("altitude:    %11.4f", (double)report.altitude);
+		warnx("temperature: %8.4f", (double)report.temperature);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+/**
+ * Calculate actual MSL pressure given current altitude
+ */
+void
+calibrate(unsigned altitude)
+{
+	struct baro_report report;
+	float	pressure;
+	float	p1;
+
+	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+
+	/* start the sensor polling at max */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
+		errx(1, "failed to set poll rate");
+
+	/* average a few measurements */
+	pressure = 0.0f;
+
+	for (unsigned i = 0; i < 20; i++) {
+		struct pollfd fds;
+		int ret;
+		ssize_t sz;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 1000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "sensor read failed");
+
+		pressure += report.pressure;
+	}
+
+	pressure /= 20;		/* average */
+	pressure /= 10;		/* scale from millibar to kPa */
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const float T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+	const float a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+	const float g  = 9.80665f;	/* gravity constant in m/s/s */
+	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
+
+	warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
+
+	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
+
+	warnx("calculated MSL pressure %10.4fkPa", p1);
+
+	/* save as integer Pa */
+	p1 *= 1000.0f;
+
+	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
+		err(1, "BAROIOCSMSLPRESSURE");
+
+	exit(0);
+}
+
+} // namespace
+
+int
+ms5611_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		ms5611::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		ms5611::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		ms5611::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		ms5611::info();
+
+	/*
+	 * Perform MSL pressure calibration given an altitude in metres
+	 */
+	if (!strcmp(argv[1], "calibrate")) {
+		if (argc < 2)
+			errx(1, "missing altitude");
+
+		long altitude = strtol(argv[2], nullptr, 10);
+
+		ms5611::calibrate(altitude);
+	}
+
+	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
new file mode 100644
index 000000000..bf72892eb
--- /dev/null
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -0,0 +1,1093 @@
+/****************************************************************************
+ *
+ *   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 fmu.cpp
+ *
+ * Driver/configurator for the PX4 FMU multi-purpose port.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_mixer.h>
+#include <drivers/drv_rc_input.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <systemlib/err.h>
+#include <systemlib/ppm_decode.h>
+
+class PX4FMU : public device::CDev
+{
+public:
+	enum Mode {
+		MODE_2PWM,
+		MODE_4PWM,
+		MODE_NONE
+	};
+	PX4FMU();
+	virtual ~PX4FMU();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
+
+	virtual int	init();
+
+	int		set_mode(Mode mode);
+
+	int		set_pwm_alt_rate(unsigned rate);
+	int		set_pwm_alt_channels(uint32_t channels);
+
+private:
+	static const unsigned _max_actuators = 4;
+
+	Mode		_mode;
+	unsigned	_pwm_default_rate;
+	unsigned	_pwm_alt_rate;
+	uint32_t	_pwm_alt_rate_channels;
+	unsigned	_current_update_rate;
+	int		_task;
+	int		_t_actuators;
+	int		_t_armed;
+	orb_advert_t	_t_outputs;
+	orb_advert_t	_t_actuators_effective;
+	unsigned	_num_outputs;
+	bool		_primary_pwm_device;
+
+	volatile bool	_task_should_exit;
+	bool		_armed;
+
+	MixerGroup	*_mixers;
+
+	actuator_controls_s _controls;
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+	void		task_main() __attribute__((noreturn));
+
+	static int	control_callback(uintptr_t handle,
+					 uint8_t control_group,
+					 uint8_t control_index,
+					 float &input);
+
+	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
+	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+	struct GPIOConfig {
+		uint32_t	input;
+		uint32_t	output;
+		uint32_t	alt;
+	};
+
+	static const GPIOConfig	_gpio_tab[];
+	static const unsigned	_ngpio;
+
+	void		gpio_reset(void);
+	void		gpio_set_function(uint32_t gpios, int function);
+	void		gpio_write(uint32_t gpios, int function);
+	uint32_t	gpio_read(void);
+	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
+
+};
+
+const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
+	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
+	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
+	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
+	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
+	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+};
+
+const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
+
+namespace
+{
+
+PX4FMU	*g_fmu;
+
+} // namespace
+
+PX4FMU::PX4FMU() :
+	CDev("fmuservo", "/dev/px4fmu"),
+	_mode(MODE_NONE),
+	_pwm_default_rate(50),
+	_pwm_alt_rate(50),
+	_pwm_alt_rate_channels(0),
+	_current_update_rate(0),
+	_task(-1),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(0),
+	_t_actuators_effective(0),
+	_num_outputs(0),
+	_primary_pwm_device(false),
+	_task_should_exit(false),
+	_armed(false),
+	_mixers(nullptr)
+{
+	_debug_enabled = true;
+}
+
+PX4FMU::~PX4FMU()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		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;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	g_fmu = nullptr;
+}
+
+int
+PX4FMU::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
+	/* reset GPIOs */
+	gpio_reset();
+
+	/* start the IO interface task */
+	_task = task_spawn("fmuservo",
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_DEFAULT,
+			   2048,
+			   (main_t)&PX4FMU::task_main_trampoline,
+			   nullptr);
+
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+PX4FMU::task_main_trampoline(int argc, char *argv[])
+{
+	g_fmu->task_main();
+}
+
+int
+PX4FMU::set_mode(Mode mode)
+{
+	/*
+	 * Configure for PWM output.
+	 *
+	 * Note that regardless of the configured mode, the task is always
+	 * listening and mixing; the mode just selects which of the channels
+	 * are presented on the output pins.
+	 */
+	switch (mode) {
+	case MODE_2PWM:	// multi-port with flow control lines as PWM
+	case MODE_4PWM: // multi-port as 4 PWM outs
+		debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_NONE:
+		debug("MODE_NONE");
+
+		_pwm_default_rate = 10;	/* artificially reduced output rate */
+		_pwm_alt_rate = 10;
+		_pwm_alt_rate_channels = 0;
+
+		/* disable servo outputs - no need to set rates */
+		up_pwm_servo_deinit();
+
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	_mode = mode;
+	return OK;
+}
+
+int
+PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
+{
+	debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
+
+	for (unsigned pass = 0; pass < 2; pass++) {
+		for (unsigned group = 0; group < _max_actuators; 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 = rate_map & mask;
+
+			if (pass == 0) {
+				// preflight
+				if ((alt != 0) && (alt != mask)) {
+					warn("rate group %u mask %x bad overlap %x", group, mask, alt);
+					// not a legal map, bail
+					return -EINVAL;
+				}
+			} else {
+				// set it - errors here are unexpected
+				if (alt != 0) {
+					if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
+						warn("rate group set alt failed");
+						return -EINVAL;
+					}
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
+						warn("rate group set default failed");
+						return -EINVAL;
+					}
+				}
+			}
+		}
+	}
+	_pwm_alt_rate_channels = rate_map;
+	_pwm_default_rate = default_rate;
+	_pwm_alt_rate = alt_rate;
+
+	return OK;
+}
+
+int
+PX4FMU::set_pwm_alt_rate(unsigned rate)
+{
+	return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate);
+}
+
+int
+PX4FMU::set_pwm_alt_channels(uint32_t channels)
+{
+	return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
+}
+
+void
+PX4FMU::task_main()
+{
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	/* force a reset of the update rate */
+	_current_update_rate = 0;
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	/* advertise the mixed control outputs */
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
+
+	/* advertise the effective control inputs */
+	actuator_controls_effective_s controls_effective;
+	memset(&controls_effective, 0, sizeof(controls_effective));
+	/* advertise the effective control inputs */
+	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+				   &controls_effective);
+
+	pollfd fds[2];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+
+	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
+
+	// rc input, published to ORB
+	struct rc_input_values rc_in;
+	orb_advert_t to_input_rc = 0;
+
+	memset(&rc_in, 0, sizeof(rc_in));
+	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+
+	log("starting");
+
+	/* loop until killed */
+	while (!_task_should_exit) {
+
+		/*
+		 * Adjust actuator topic update rate to keep up with
+		 * the highest servo update rate configured.
+		 *
+		 * We always mix at max rate; some channels may update slower.
+		 */
+		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+		if (_current_update_rate != max_rate) {
+			_current_update_rate = max_rate;
+			int update_rate_in_ms = int(1000 / _current_update_rate);
+
+			/* reject faster than 500 Hz updates */
+			if (update_rate_in_ms < 2) {
+				update_rate_in_ms = 2;
+			}
+			/* reject slower than 10 Hz updates */
+			if (update_rate_in_ms > 100) {
+				update_rate_in_ms = 100;
+			}
+
+			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
+			orb_set_interval(_t_actuators, update_rate_in_ms);
+
+			// set to current max rate, even if we are actually checking slower/faster
+			_current_update_rate = max_rate;
+		}
+
+		/* sleep waiting for data, stopping to check for PPM
+		 * input at 100Hz */
+		int ret = ::poll(&fds[0], 2, 10);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			usleep(1000000);
+			continue;
+		}
+
+		/* do we have a control update? */
+		if (fds[0].revents & POLLIN) {
+
+			/* get controls - must always do this to avoid spinning */
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+			/* can we mix? */
+			if (_mixers != nullptr) {
+
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				// XXX output actual limited values
+				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+				/* iterate actuators */
+				for (unsigned i = 0; i < num_outputs; i++) {
+
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					} else {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = 900;
+					}
+
+					/* output to the servo */
+					up_pwm_servo_set(i, outputs.output[i]);
+				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+			}
+		}
+
+		/* how about an arming update? */
+		if (fds[1].revents & POLLIN) {
+			actuator_armed_s aa;
+
+			/* get new value */
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+
+			/* update PWM servo armed status if armed and not locked down */
+			bool set_armed = aa.armed && !aa.lockdown;
+			if (set_armed != _armed) {
+				_armed = set_armed;
+				up_pwm_servo_arm(set_armed);
+			}
+		}
+
+		// see if we have new PPM input data
+		if (ppm_last_valid_decode != rc_in.timestamp) {
+			// we have a new PPM frame. Publish it.
+			rc_in.channel_count = ppm_decoded_channels;
+			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
+				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
+			}
+			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+				rc_in.values[i] = ppm_buffer[i];
+			}
+			rc_in.timestamp = ppm_last_valid_decode;
+
+			/* lazily advertise on first publication */
+			if (to_input_rc == 0) {
+				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
+			} else { 
+				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
+			}
+		}
+	}
+
+	::close(_t_actuators);
+	::close(_t_actuators_effective);
+	::close(_t_armed);
+
+	/* make sure servos are off */
+	up_pwm_servo_deinit();
+
+	log("stopping");
+
+	/* note - someone else is responsible for restoring the GPIO config */
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+int
+PX4FMU::control_callback(uintptr_t handle,
+			 uint8_t control_group,
+			 uint8_t control_index,
+			 float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls->control[control_index];
+	return 0;
+}
+
+int
+PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret;
+
+	// XXX disabled, confusing users
+	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
+
+	/* try it as a GPIO ioctl first */
+	ret = gpio_ioctl(filp, cmd, arg);
+
+	if (ret != -ENOTTY)
+		return ret;
+
+	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	switch (_mode) {
+	case MODE_2PWM:
+	case MODE_4PWM:
+		ret = pwm_ioctl(filp, cmd, arg);
+		break;
+
+	default:
+		debug("not in a PWM mode");
+		break;
+	}
+
+	/* if nobody wants it, let CDev have it */
+	if (ret == -ENOTTY)
+		ret = CDev::ioctl(filp, cmd, arg);
+
+	return ret;
+}
+
+int
+PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		up_pwm_servo_arm(true);
+		break;
+
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+		// these are no-ops, as no safety switch
+		break;
+
+	case PWM_SERVO_DISARM:
+		up_pwm_servo_arm(false);
+		break;
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
+		break;
+
+	case PWM_SERVO_SET(2):
+	case PWM_SERVO_SET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_SET(0):
+	case PWM_SERVO_SET(1):
+		if (arg < 2100) {
+			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_GET(0):
+	case PWM_SERVO_GET(1):
+		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
+		break;
+
+	case PWM_SERVO_GET_RATEGROUP(0):
+	case PWM_SERVO_GET_RATEGROUP(1):
+	case PWM_SERVO_GET_RATEGROUP(2):
+	case PWM_SERVO_GET_RATEGROUP(3):
+		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
+		break;
+
+	case PWM_SERVO_GET_COUNT:	
+	case MIXERIOCGETOUTPUTCOUNT:
+		if (_mode == MODE_4PWM) {
+			*(unsigned *)arg = 4;
+
+		} else {
+			*(unsigned *)arg = 2;
+		}
+
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+		}
+
+		break;
+
+	case MIXERIOCADDSIMPLE: {
+			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
+
+			if (mixer->check()) {
+				delete mixer;
+				ret = -EINVAL;
+
+			} else {
+				if (_mixers == nullptr)
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
+
+				_mixers->add_mixer(mixer);
+			}
+
+			break;
+		}
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
+			if (_mixers == nullptr) {
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					debug("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					ret = -EINVAL;
+				}
+			}
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+/*
+  this implements PWM output via a write() method, for compatibility
+  with px4io
+ */
+ssize_t
+PX4FMU::write(file *filp, const char *buffer, size_t len)
+{
+	unsigned count = len / 2;
+	uint16_t values[4];
+
+	if (count > 4) {
+		// we only have 4 PWM outputs on the FMU
+		count = 4;
+	}
+
+	// allow for misaligned values
+	memcpy(values, buffer, count*2);
+
+	for (uint8_t i=0; i<count; i++) {
+		up_pwm_servo_set(i, values[i]);
+	}
+	return count * 2;
+}
+
+void
+PX4FMU::gpio_reset(void)
+{
+	/*
+	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
+	 * to input mode.
+	 */
+	for (unsigned i = 0; i < _ngpio; i++)
+		stm32_configgpio(_gpio_tab[i].input);
+
+	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+	stm32_configgpio(GPIO_GPIO_DIR);
+}
+
+void
+PX4FMU::gpio_set_function(uint32_t gpios, int function)
+{
+	/*
+	 * GPIOs 0 and 1 must have the same direction as they are buffered
+	 * by a shared 2-port driver.  Any attempt to set either sets both.
+	 */
+	if (gpios & 3) {
+		gpios |= 3;
+
+		/* flip the buffer to output mode if required */
+		if (GPIO_SET_OUTPUT == function)
+			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
+	}
+
+	/* configure selected GPIOs as required */
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (gpios & (1 << i)) {
+			switch (function) {
+			case GPIO_SET_INPUT:
+				stm32_configgpio(_gpio_tab[i].input);
+				break;
+
+			case GPIO_SET_OUTPUT:
+				stm32_configgpio(_gpio_tab[i].output);
+				break;
+
+			case GPIO_SET_ALT_1:
+				if (_gpio_tab[i].alt != 0)
+					stm32_configgpio(_gpio_tab[i].alt);
+
+				break;
+			}
+		}
+	}
+
+	/* flip buffer to input mode if required */
+	if ((GPIO_SET_INPUT == function) && (gpios & 3))
+		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+}
+
+void
+PX4FMU::gpio_write(uint32_t gpios, int function)
+{
+	int value = (function == GPIO_SET) ? 1 : 0;
+
+	for (unsigned i = 0; i < _ngpio; i++)
+		if (gpios & (1 << i))
+			stm32_gpiowrite(_gpio_tab[i].output, value);
+}
+
+uint32_t
+PX4FMU::gpio_read(void)
+{
+	uint32_t bits = 0;
+
+	for (unsigned i = 0; i < _ngpio; i++)
+		if (stm32_gpioread(_gpio_tab[i].input))
+			bits |= (1 << i);
+
+	return bits;
+}
+
+int
+PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int	ret = OK;
+
+	lock();
+
+	switch (cmd) {
+
+	case GPIO_RESET:
+		gpio_reset();
+		break;
+
+	case GPIO_SET_OUTPUT:
+	case GPIO_SET_INPUT:
+	case GPIO_SET_ALT_1:
+		gpio_set_function(arg, cmd);
+		break;
+
+	case GPIO_SET_ALT_2:
+	case GPIO_SET_ALT_3:
+	case GPIO_SET_ALT_4:
+		ret = -EINVAL;
+		break;
+
+	case GPIO_SET:
+	case GPIO_CLEAR:
+		gpio_write(arg, cmd);
+		break;
+
+	case GPIO_GET:
+		*(uint32_t *)arg = gpio_read();
+		break;
+
+	default:
+		ret = -ENOTTY;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+namespace
+{
+
+enum PortMode {
+	PORT_MODE_UNSET = 0,
+	PORT_FULL_GPIO,
+	PORT_FULL_SERIAL,
+	PORT_FULL_PWM,
+	PORT_GPIO_AND_SERIAL,
+	PORT_PWM_AND_SERIAL,
+	PORT_PWM_AND_GPIO,
+};
+
+PortMode g_port_mode;
+
+int
+fmu_new_mode(PortMode new_mode)
+{
+	uint32_t gpio_bits;
+	PX4FMU::Mode servo_mode;
+
+	/* reset to all-inputs */
+	g_fmu->ioctl(0, GPIO_RESET, 0);
+
+	gpio_bits = 0;
+	servo_mode = PX4FMU::MODE_NONE;
+
+	switch (new_mode) {
+	case PORT_FULL_GPIO:
+	case PORT_MODE_UNSET:
+		/* nothing more to do here */
+		break;
+
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_FULL_PWM:
+		/* select 4-pin PWM mode */
+		servo_mode = PX4FMU::MODE_4PWM;
+		break;
+
+	case PORT_GPIO_AND_SERIAL:
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_SERIAL:
+		/* select 2-pin PWM mode */
+		servo_mode = PX4FMU::MODE_2PWM;
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_GPIO:
+		/* select 2-pin PWM mode */
+		servo_mode = PX4FMU::MODE_2PWM;
+		break;
+	}
+
+	/* adjust GPIO config for serial mode(s) */
+	if (gpio_bits != 0)
+		g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+
+	/* (re)set the PWM output mode */
+	g_fmu->set_mode(servo_mode);
+
+	return OK;
+}
+
+int
+fmu_start(void)
+{
+	int ret = OK;
+
+	if (g_fmu == nullptr) {
+
+		g_fmu = new PX4FMU;
+
+		if (g_fmu == nullptr) {
+			ret = -ENOMEM;
+
+		} else {
+			ret = g_fmu->init();
+
+			if (ret != OK) {
+				delete g_fmu;
+				g_fmu = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void
+test(void)
+{
+	int	fd;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+
+	if (fd < 0)
+		errx(1, "open fail");
+
+	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
+
+	if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+
+	if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+
+	if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+
+	if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+
+	close(fd);
+
+	exit(0);
+}
+
+void
+fake(int argc, char *argv[])
+{
+	if (argc < 5)
+		errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
+
+	actuator_controls_s ac;
+
+	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
+
+	ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
+
+	ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
+
+	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
+
+	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+
+	if (handle < 0)
+		errx(1, "advertise failed");
+
+	actuator_armed_s aa;
+
+	aa.armed = true;
+	aa.lockdown = false;
+
+	handle = orb_advertise(ORB_ID(actuator_armed), &aa);
+
+	if (handle < 0)
+		errx(1, "advertise failed 2");
+
+	exit(0);
+}
+
+} // namespace
+
+extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
+
+int
+fmu_main(int argc, char *argv[])
+{
+	PortMode new_mode = PORT_MODE_UNSET;
+	const char *verb = argv[1];
+
+	if (fmu_start() != OK)
+		errx(1, "failed to start the FMU driver");
+
+	/*
+	 * Mode switches.
+	 */
+	if (!strcmp(verb, "mode_gpio")) {
+		new_mode = PORT_FULL_GPIO;
+
+	} else if (!strcmp(verb, "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm")) {
+		new_mode = PORT_FULL_PWM;
+
+	} else if (!strcmp(verb, "mode_gpio_serial")) {
+		new_mode = PORT_GPIO_AND_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT_PWM_AND_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT_PWM_AND_GPIO;
+	}
+
+	/* was a new mode set? */
+	if (new_mode != PORT_MODE_UNSET) {
+
+		/* yes but it's the same mode */
+		if (new_mode == g_port_mode)
+			return OK;
+
+		/* switch modes */
+		int ret = fmu_new_mode(new_mode);
+		exit(ret == OK ? 0 : 1);
+	}
+
+	if (!strcmp(verb, "test"))
+		test();
+
+	if (!strcmp(verb, "fake"))
+		fake(argc - 1, argv + 1);
+
+	fprintf(stderr, "FMU: unrecognised command, try:\n");
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+	exit(1);
+}
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
new file mode 100644
index 000000000..05bc7a5b3
--- /dev/null
+++ b/src/drivers/px4fmu/module.mk
@@ -0,0 +1,6 @@
+#
+# Interface driver for the PX4FMU board
+#
+
+MODULE_COMMAND	 = fmu
+SRCS		 = fmu.cpp
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
new file mode 100644
index 000000000..328e5a684
--- /dev/null
+++ b/src/drivers/px4io/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Interface driver for the PX4IO board.
+#
+
+MODULE_COMMAND		= px4io
+
+SRCS			= px4io.cpp \
+			  uploader.cpp
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
new file mode 100644
index 000000000..399c003b7
--- /dev/null
+++ b/src/drivers/px4io/px4io.cpp
@@ -0,0 +1,1846 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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.cpp
+ * Driver for the PX4IO board.
+ *
+ * PX4IO is connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <math.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_rc_input.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/scheduling_priorities.h>
+#include <systemlib/param/param.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <debug.h>
+
+#include <mavlink/mavlink_log.h>
+#include "uploader.h"
+#include <modules/px4iofirmware/protocol.h>
+
+#define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
+#define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
+
+class PX4IO : public device::I2C
+{
+public:
+	PX4IO();
+	virtual ~PX4IO();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
+
+	/**
+	* Set the update rate for actuator outputs from FMU to IO.
+	*
+	* @param rate    The rate in Hz actuator outpus are sent to IO.
+	*      Min 10 Hz, max 400 Hz
+	*/
+	int      set_update_rate(int rate);
+
+	/**
+	* Set the battery current scaling and bias
+	*
+	* @param amp_per_volt
+	* @param amp_bias
+	*/
+	void      set_battery_current_scaling(float amp_per_volt, float amp_bias);
+
+	/**
+	* Print the current status of IO
+	*/
+	void			print_status();
+
+private:
+	// XXX
+	unsigned		_max_actuators;
+	unsigned		_max_controls;
+	unsigned		_max_rc_input;
+	unsigned		_max_relays;
+	unsigned		_max_transfer;
+
+	unsigned 		_update_interval; ///< subscription interval limiting send rate
+
+	volatile int		_task;		///< worker task
+	volatile bool		_task_should_exit;
+
+	int			_mavlink_fd;
+
+	perf_counter_t		_perf_update;
+
+	/* cached IO state */
+	uint16_t		_status;
+	uint16_t		_alarms;
+
+	/* subscribed topics */
+	int			_t_actuators;	///< actuator controls topic
+	int			_t_armed;	///< system armed control topic
+	int 			_t_vstatus;	///< system / vehicle status
+	int			_t_param;	///< parameter update topic
+
+	/* advertised topics */
+	orb_advert_t 		_to_input_rc;	///< rc inputs from io
+	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
+	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
+	orb_advert_t		_to_battery;	///< battery status / voltage
+
+	actuator_outputs_s	_outputs;	///< mixed outputs
+	actuator_controls_effective_s _controls_effective; ///< effective controls
+
+	bool			_primary_pwm_device;	///< true if we are the default PWM output
+
+	float			_battery_amp_per_volt;
+	float			_battery_amp_bias;
+	float			_battery_mamphour_total;
+	uint64_t		_battery_last_timestamp;
+
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void		task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * worker task
+	 */
+	void			task_main();
+
+	/**
+	 * Send controls to IO
+	 */
+	int			io_set_control_state();
+
+	/**
+	 * Update IO's arming-related state
+	 */
+	int			io_set_arming_state();
+
+	/**
+	 * Push RC channel configuration to IO.
+	 */
+	int			io_set_rc_config();
+
+	/**
+	 * Fetch status and alarms from IO
+	 *
+	 * Also publishes battery voltage/current.
+	 */
+	int			io_get_status();
+
+	/**
+	 * Fetch RC inputs from IO.
+	 *
+	 * @param input_rc	Input structure to populate.
+	 * @return		OK if data was returned.
+	 */
+	int			io_get_raw_rc_input(rc_input_values &input_rc);
+
+	/**
+	 * Fetch and publish raw RC input data.
+	 */
+	int			io_publish_raw_rc();
+
+	/**
+	 * Fetch and publish the mixed control values.
+	 */
+	int			io_publish_mixed_controls();
+
+	/**
+	 * Fetch and publish the PWM servo outputs.
+	 */
+	int			io_publish_pwm_outputs();
+
+	/**
+	 * write register(s)
+	 *
+	 * @param page		Register page to write to.
+	 * @param offset	Register offset to start writing at.
+	 * @param values	Pointer to array of values to write.
+	 * @param num_values	The number of values to write.
+	 * @return		Zero if all values were successfully written.
+	 */
+	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+
+	/**
+	 * write a register
+	 *
+	 * @param page		Register page to write to.
+	 * @param offset	Register offset to write to.
+	 * @param value		Value to write.
+	 * @return		Zero if the value was written successfully.
+	 */
+	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
+
+	/**
+	 * read register(s)
+	 *
+	 * @param page		Register page to read from.
+	 * @param offset	Register offset to start reading from.
+	 * @param values	Pointer to array where values should be stored.
+	 * @param num_values	The number of values to read.
+	 * @return		Zero if all values were successfully read.
+	 */
+	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
+
+	/**
+	 * read a register
+	 *
+	 * @param page		Register page to read from.
+	 * @param offset	Register offset to start reading from.
+	 * @return		Register value that was read, or _io_reg_get_error on error.
+	 */
+	uint32_t		io_reg_get(uint8_t page, uint8_t offset);
+	static const uint32_t	_io_reg_get_error = 0x80000000;
+
+	/**
+	 * modify a register
+	 *
+	 * @param page		Register page to modify.
+	 * @param offset	Register offset to modify.
+	 * @param clearbits	Bits to clear in the register.
+	 * @param setbits	Bits to set in the register.
+	 */
+	int			io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
+
+	/**
+	 * Send mixer definition text to IO
+	 */
+	int			mixer_send(const char *buf, unsigned buflen);
+
+	/**
+	 * Handle a status update from IO.
+	 *
+	 * Publish IO status information if necessary.
+	 *
+	 * @param status	The status register
+	 */
+	int			io_handle_status(uint16_t status);
+
+	/**
+	 * Handle an alarm update from IO.
+	 *
+	 * Publish IO alarm information if necessary.
+	 *
+	 * @param alarm		The status register
+	 */
+	int			io_handle_alarms(uint16_t alarms);
+
+};
+
+
+namespace
+{
+
+PX4IO	*g_dev;
+
+}
+
+PX4IO::PX4IO() :
+	I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+	_max_actuators(0),
+	_max_controls(0),
+	_max_rc_input(0),
+	_max_relays(0),
+	_max_transfer(16),	/* sensible default */
+	_update_interval(0),
+	_task(-1),
+	_task_should_exit(false),
+	_mavlink_fd(-1),
+	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
+	_status(0),
+	_alarms(0),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_vstatus(-1),
+	_t_param(-1),
+	_to_input_rc(0),
+	_to_actuators_effective(0),
+	_to_outputs(0),
+	_to_battery(0),
+	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
+	_battery_amp_bias(0),
+	_battery_mamphour_total(0),
+	_battery_last_timestamp(0),
+	_primary_pwm_device(false)
+{
+	/* we need this potentially before it could be set in task_main */
+	g_dev = this;
+
+	/* open MAVLink text channel */
+	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+
+	_debug_enabled = true;
+}
+
+PX4IO::~PX4IO()
+{
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
+
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
+	}
+
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
+
+	g_dev = nullptr;
+}
+
+int
+PX4IO::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* do regular cdev init */
+	ret = I2C::init();
+	if (ret != OK)
+		return ret;
+
+	/*
+	 * Enable a couple of retries for operations to IO.
+	 *
+	 * Register read/write operations are intentionally idempotent
+	 * so this is safe as designed.
+	 */
+	_retries = 2;
+
+	/* get some parameters */
+	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
+	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
+	_max_relays    = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
+	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
+	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
+	if ((_max_actuators < 1) || (_max_actuators > 255) ||
+	    (_max_relays < 1)    || (_max_relays > 255)    ||
+	    (_max_transfer < 16)   || (_max_transfer > 255)    ||
+	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
+
+		log("failed getting parameters from PX4IO");
+		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+		return -1;
+	}
+	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
+		_max_rc_input = RC_INPUT_MAX_CHANNELS;
+
+	/*
+	 * Check for IO flight state - if FMU was flagged to be in
+	 * armed state, FMU is recovering from an in-air reset.
+	 * Read back status and request the commander to arm
+	 * in this case.
+	 */
+
+	uint16_t reg;
+
+	/* get IO's last seen FMU state */
+	ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
+	if (ret != OK)
+		return ret;
+
+	/*
+	 * in-air restart is only tried if the IO board reports it is
+	 * already armed, and has been configured for in-air restart
+	 */
+	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
+	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+
+	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+
+		/* WARNING: COMMANDER app/vehicle status must be initialized.
+		 * If this fails (or the app is not started), worst-case IO
+		 * remains untouched (so manual override is still available).
+		 */
+
+		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		/* fill with initial values, clear updated flag */
+		vehicle_status_s status;
+		uint64_t try_start_time = hrt_absolute_time();
+		bool updated = false;
+		
+		/* keep checking for an update, ensure we got a recent state,
+		   not something that was published a long time ago. */
+		do {
+			orb_check(vstatus_sub, &updated);
+
+			if (updated) {
+				/* got data, copy and exit loop */
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				break;
+			}
+
+			/* wait 10 ms */
+			usleep(10000);
+
+			/* abort after 5s */
+			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+				log("failed to recover from in-air restart (1), aborting IO driver init.");
+				return 1;
+			}
+
+		} while (true);
+
+		/* send command to arm system via command API */
+		vehicle_command_s cmd;
+		/* request arming */
+		cmd.param1 = 1.0f;
+		cmd.param2 = 0;
+		cmd.param3 = 0;
+		cmd.param4 = 0;
+		cmd.param5 = 0;
+		cmd.param6 = 0;
+		cmd.param7 = 0;
+		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
+		cmd.target_system = status.system_id;
+		cmd.target_component = status.component_id;
+		cmd.source_system = status.system_id;
+		cmd.source_component = status.component_id;
+		/* ask to confirm command */
+		cmd.confirmation =  1;
+
+		/* send command once */
+		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+
+		/* spin here until IO's state has propagated into the system */
+		do {
+			orb_check(vstatus_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+			}
+
+			/* wait 10 ms */
+			usleep(10000);
+
+			/* abort after 5s */
+			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+				log("failed to recover from in-air restart (2), aborting IO driver init.");
+				return 1;
+			}
+
+		/* keep waiting for state change for 10 s */
+		} while (!status.flag_fmu_armed);
+
+	/* regular boot, no in-air restart, init IO */
+	} else {
+
+
+		/* dis-arm IO before touching anything */
+		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
+			PX4IO_P_SETUP_ARMING_FMU_ARMED |
+			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+
+		/* publish RC config to IO */
+		ret = io_set_rc_config();
+		if (ret != OK) {
+			log("failed to update RC input config");
+			mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+			return ret;
+		}
+
+	}
+
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
+	/* start the IO interface task */
+	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	mavlink_log_info(_mavlink_fd, "[IO] init ok");
+
+	return OK;
+}
+
+void
+PX4IO::task_main_trampoline(int argc, char *argv[])
+{
+	g_dev->task_main();
+}
+
+void
+PX4IO::task_main()
+{
+	hrt_abstime last_poll_time = 0;
+
+	log("starting");
+
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
+	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
+
+	_t_param = orb_subscribe(ORB_ID(parameter_update));
+	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
+
+	/* poll descriptor */
+	pollfd fds[4];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+	fds[2].fd = _t_vstatus;
+	fds[2].events = POLLIN;
+	fds[3].fd = _t_param;
+	fds[3].events = POLLIN;
+
+	debug("ready");
+
+	/* lock against the ioctl handler */
+	lock();
+
+	/* loop talking to IO */
+	while (!_task_should_exit) {
+
+		/* adjust update interval */
+		if (_update_interval != 0) {
+			if (_update_interval < 5)
+				_update_interval = 5;
+			if (_update_interval > 100)
+				_update_interval = 100;
+			orb_set_interval(_t_actuators, _update_interval);
+			_update_interval = 0;
+		}
+
+		/* sleep waiting for topic updates, but no more than 20ms */
+		unlock();
+		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20);
+		lock();
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			continue;
+		}
+
+		perf_begin(_perf_update);
+		hrt_abstime now = hrt_absolute_time();
+
+		/* if we have new control data from the ORB, handle it */
+		if (fds[0].revents & POLLIN)
+			io_set_control_state();
+
+		/* if we have an arming state update, handle it */
+		if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
+			io_set_arming_state();
+
+		/*
+		 * If it's time for another tick of the polling status machine,
+		 * try it now.
+		 */
+		if ((now - last_poll_time) >= 20000) {
+
+			/*
+			 * Pull status and alarms from IO.
+			 */
+			io_get_status();
+
+			/*
+			 * Get raw R/C input from IO.
+			 */
+			io_publish_raw_rc();
+
+			/*
+			 * Fetch mixed servo controls and PWM outputs from IO.
+			 *
+			 * XXX We could do this at a reduced rate in many/most cases.
+			 */
+			io_publish_mixed_controls();
+			io_publish_pwm_outputs();
+
+			/*
+			 * If parameters have changed, re-send RC mappings to IO
+			 *
+			 * XXX this may be a bit spammy
+			 */
+			if (fds[3].revents & POLLIN) {
+				parameter_update_s pupdate;
+
+				/* copy to reset the notification */
+				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
+
+				/* re-upload RC input config as it may have changed */
+				io_set_rc_config();
+			}
+		}
+
+		perf_end(_perf_update);
+	}
+
+	unlock();
+
+	debug("exiting");
+
+	/* 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;
+	_exit(0);
+}
+
+int
+PX4IO::io_set_control_state()
+{
+	actuator_controls_s	controls;	///< actuator outputs
+	uint16_t 		regs[_max_actuators];
+
+	/* get controls */
+	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+	     ORB_ID(actuator_controls_1), _t_actuators, &controls);
+
+	for (unsigned i = 0; i < _max_controls; i++)
+		regs[i] = FLOAT_TO_REG(controls.control[i]);
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
+}
+
+int
+PX4IO::io_set_arming_state()
+{
+	actuator_armed_s	armed;		///< system armed state
+	vehicle_status_s	vstatus;	///< overall system state
+
+	orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
+	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+
+	uint16_t set = 0;
+	uint16_t clear = 0;
+
+	if (armed.armed && !armed.lockdown) {
+		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
+	}
+	// TODO fix this
+//	if (armed.ready_to_arm) {
+//		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+//	} else {
+//		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+//	}
+
+	if (vstatus.flag_external_manual_override_ok) {
+		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+	}
+
+	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
+int
+PX4IO::io_set_rc_config()
+{
+	unsigned offset = 0;
+	int input_map[_max_rc_input];
+	int32_t ichan;
+	int ret = OK;
+
+	/*
+	 * Generate the input channel -> control channel mapping table;
+	 * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
+	 * controls.
+	 */
+	for (unsigned i = 0; i < _max_rc_input; i++)
+		input_map[i] = -1;
+
+	/*
+	 * NOTE: The indices for mapped channels are 1-based
+	 *       for compatibility reasons with existing
+	 *       autopilots / GCS'.
+	 */
+	param_get(param_find("RC_MAP_ROLL"), &ichan);
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 0;
+
+	param_get(param_find("RC_MAP_PITCH"), &ichan);
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 1;
+
+	param_get(param_find("RC_MAP_YAW"), &ichan);
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 2;
+
+	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 3;
+
+	ichan = 4;
+	for (unsigned i = 0; i < _max_rc_input; i++)
+		if (input_map[i] == -1)
+			input_map[i] = ichan++;
+
+	/*
+	 * Iterate all possible RC inputs.
+	 */
+	for (unsigned i = 0; i < _max_rc_input; i++) {
+		uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+		char pname[16];
+		float fval;
+
+		/*
+		 * RC params are floats, but do only
+		 * contain integer values. Do not scale
+		 * or cast them, let the auto-typeconversion
+		 * do its job here.
+		 * Channels: 500 - 2500
+		 * Inverted flag: -1 (inverted) or 1 (normal)
+		 */
+
+		sprintf(pname, "RC%d_MIN", i + 1);
+		param_get(param_find(pname), &fval);
+		regs[PX4IO_P_RC_CONFIG_MIN] = fval;
+
+		sprintf(pname, "RC%d_TRIM", i + 1);
+		param_get(param_find(pname), &fval);
+		regs[PX4IO_P_RC_CONFIG_CENTER] = fval;
+
+		sprintf(pname, "RC%d_MAX", i + 1);
+		param_get(param_find(pname), &fval);
+		regs[PX4IO_P_RC_CONFIG_MAX] = fval;
+
+		sprintf(pname, "RC%d_DZ", i + 1);
+		param_get(param_find(pname), &fval);
+		regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval;
+
+		regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i];
+
+		regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+		sprintf(pname, "RC%d_REV", i + 1);
+		param_get(param_find(pname), &fval);
+
+		/*
+		 * This has been taken for the sake of compatibility
+		 * with APM's setup / mission planner: normal: 1,
+		 * inverted: -1
+		 */
+		if (fval < 0) {
+			regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+		}
+
+		/* send channel config to IO */
+		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+		if (ret != OK) {
+			log("rc config upload failed");
+			break;
+		}
+
+		/* check the IO initialisation flag */
+		if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
+			log("config for RC%d rejected by IO", i + 1);
+			break;
+		}
+
+		offset += PX4IO_P_RC_CONFIG_STRIDE;
+	}
+
+	return ret;
+}
+
+int
+PX4IO::io_handle_status(uint16_t status)
+{
+	int ret = 1;
+	/**
+	 * WARNING: This section handles in-air resets.
+	 */
+
+	/* check for IO reset - force it back to armed if necessary */
+	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+		/* set the arming flag */
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+
+		/* set new status */
+		_status = status;
+		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+
+		/* set the sync flag */
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		/* set new status */
+		_status = status;
+
+	} else {
+		ret = 0;
+
+		/* set new status */
+		_status = status;
+	}
+
+	return ret;
+}
+
+int
+PX4IO::io_handle_alarms(uint16_t alarms)
+{
+
+	/* XXX handle alarms */
+
+
+	/* set new alarms state */
+	_alarms = alarms;
+
+	return 0;
+}
+
+int
+PX4IO::io_get_status()
+{
+	uint16_t	regs[4];
+	int		ret;
+
+	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
+	if (ret != OK)
+		return ret;
+
+	io_handle_status(regs[0]);
+	io_handle_alarms(regs[1]);
+
+	/* only publish if battery has a valid minimum voltage */
+	if (regs[2] > 3300) {
+		battery_status_s	battery_status;
+
+		battery_status.timestamp = hrt_absolute_time();
+
+		/* voltage is scaled to mV */
+		battery_status.voltage_v = regs[2] / 1000.0f;
+
+		/*
+		  regs[3] contains the raw ADC count, as 12 bit ADC
+		  value, with full range being 3.3v
+		 */
+		battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
+		battery_status.current_a += _battery_amp_bias;
+
+		/*
+		  integrate battery over time to get total mAh used
+		 */
+		if (_battery_last_timestamp != 0) {
+			_battery_mamphour_total += battery_status.current_a * 
+				(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+		}
+		battery_status.discharged_mah = _battery_mamphour_total;
+		_battery_last_timestamp = battery_status.timestamp;
+
+		/* lazily publish the battery voltage */
+		if (_to_battery > 0) {
+			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+		} else {
+			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		}
+	}
+	return ret;
+}
+
+int
+PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
+{
+	uint32_t channel_count;
+	int	ret = OK;
+
+	/* we don't have the status bits, so input_source has to be set elsewhere */
+	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
+	
+	/*
+	 * XXX Because the channel count and channel data are fetched
+	 *     separately, there is a risk of a race between the two
+	 *     that could leave us with channel data and a count that 
+	 *     are out of sync.
+	 *     Fixing this would require a guarantee of atomicity from
+	 *     IO, and a single fetch for both count and channels.
+	 *
+	 * XXX Since IO has the input calibration info, we ought to be
+	 *     able to get the pre-fixed-up controls directly.
+	 *
+	 * XXX can we do this more cheaply? If we knew we had DMA, it would
+	 *     almost certainly be better to just get all the inputs...
+	 */
+	channel_count =  io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
+	if (channel_count == _io_reg_get_error)
+		return -EIO;
+	if (channel_count > RC_INPUT_MAX_CHANNELS)
+		channel_count = RC_INPUT_MAX_CHANNELS;
+	input_rc.channel_count = channel_count;
+
+	if (channel_count > 0) {
+		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
+		if (ret == OK)
+			input_rc.timestamp = hrt_absolute_time();
+	}
+
+	return ret;
+}
+
+int
+PX4IO::io_publish_raw_rc()
+{
+	/* if no raw RC, just don't publish */
+	if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		return OK;
+
+	/* fetch values from IO */
+	rc_input_values	rc_val;
+	rc_val.timestamp = hrt_absolute_time();
+
+	int ret = io_get_raw_rc_input(rc_val);
+	if (ret != OK)
+		return ret;
+
+	/* sort out the source of the values */
+	if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+	} else {
+		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
+	}
+
+	/* lazily advertise on first publication */
+	if (_to_input_rc == 0) {
+		_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
+	} else { 
+		orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
+	}
+
+	return OK;
+}
+
+int
+PX4IO::io_publish_mixed_controls()
+{
+	/* if no FMU comms(!) just don't publish */
+	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
+		return OK;
+
+	/* if not taking raw PPM from us, must be mixing */
+	if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		return OK;
+
+	/* data we are going to fetch */
+	actuator_controls_effective_s controls_effective;
+	controls_effective.timestamp = hrt_absolute_time();
+
+	/* get actuator controls from IO */
+	uint16_t act[_max_actuators];
+	int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
+	if (ret != OK)
+		return ret;
+
+	/* convert from register format to float */
+	for (unsigned i = 0; i < _max_actuators; i++)
+		controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
+
+	/* laxily advertise on first publication */
+	if (_to_actuators_effective == 0) {
+		_to_actuators_effective = 
+			orb_advertise((_primary_pwm_device ? 
+				ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
+				ORB_ID(actuator_controls_effective_1)),
+					   &controls_effective);
+	} else {
+		orb_publish((_primary_pwm_device ? 
+			ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
+			ORB_ID(actuator_controls_effective_1)),
+			_to_actuators_effective, &controls_effective);
+	}
+
+	return OK;
+}
+
+int
+PX4IO::io_publish_pwm_outputs()
+{
+	/* if no FMU comms(!) just don't publish */
+	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
+		return OK;
+
+	/* data we are going to fetch */
+	actuator_outputs_s outputs;
+	outputs.timestamp = hrt_absolute_time();
+
+	/* get servo values from IO */
+	uint16_t ctl[_max_actuators];
+	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
+	if (ret != OK)
+		return ret;
+
+	/* convert from register format to float */
+	for (unsigned i = 0; i < _max_actuators; i++)
+		outputs.output[i] = ctl[i];
+	outputs.noutputs = _max_actuators;
+
+	/* lazily advertise on first publication */
+	if (_to_outputs == 0) {
+		_to_outputs = orb_advertise((_primary_pwm_device ?
+			ORB_ID_VEHICLE_CONTROLS :
+			ORB_ID(actuator_outputs_1)),
+			&outputs);
+	} else {
+		orb_publish((_primary_pwm_device ?
+			ORB_ID_VEHICLE_CONTROLS :
+			ORB_ID(actuator_outputs_1)),
+			_to_outputs,
+			&outputs);
+	}
+
+	return OK;
+}
+
+int
+PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+	/* range check the transfer */
+	if (num_values > ((_max_transfer) / sizeof(*values))) {
+		debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+		return -EINVAL;
+	}
+
+	/* set up the transfer */
+	uint8_t 	addr[2] = {
+		page,
+		offset
+	};
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+	msgv[1].flags = I2C_M_NORESTART;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = num_values * sizeof(*values);
+
+	/* perform the transfer */
+	int ret = transfer(msgv, 2);
+	if (ret != OK)
+		debug("io_reg_set: error %d", ret);
+	return ret;
+}
+
+int
+PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
+{
+	return io_reg_set(page, offset, &value, 1);
+}
+
+int
+PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+{
+	/* set up the transfer */
+	uint8_t		addr[2] = {
+		page,
+		offset
+	};
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+	msgv[1].flags = I2C_M_READ;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = num_values * sizeof(*values);
+
+	/* perform the transfer */
+	int ret = transfer(msgv, 2);
+	if (ret != OK)
+		debug("io_reg_get: data error %d", ret);
+	return ret;
+}
+
+uint32_t
+PX4IO::io_reg_get(uint8_t page, uint8_t offset)
+{
+	uint16_t value;
+
+	if (io_reg_get(page, offset, &value, 1))
+		return _io_reg_get_error;
+
+	return value;
+}
+
+int
+PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits)
+{
+	int ret;
+	uint16_t value;
+
+	ret = io_reg_get(page, offset, &value, 1);
+	if (ret)
+		return ret;
+	value &= ~clearbits;
+	value |= setbits;
+
+	return io_reg_set(page, offset, value);
+}
+
+int
+PX4IO::mixer_send(const char *buf, unsigned buflen)
+{
+	uint8_t	frame[_max_transfer];
+	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+	unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
+
+	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+	msg->action = F2I_MIXER_ACTION_RESET;
+
+	do {
+		unsigned count = buflen;
+
+		if (count > max_len)
+			count = max_len;
+
+		if (count > 0) {
+			memcpy(&msg->text[0], buf, count);
+			buf += count;
+			buflen -= count;
+		}
+
+		/*
+		 * We have to send an even number of bytes.  This
+		 * will only happen on the very last transfer of a
+		 * mixer, and we are guaranteed that there will be
+		 * space left to round up as _max_transfer will be
+		 * even.
+		 */
+		unsigned total_len = sizeof(px4io_mixdata) + count;
+		if (total_len % 1) {
+			msg->text[count] = '\0';
+			total_len++;
+		}
+
+		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+		if (ret) {
+			log("mixer send error %d", ret);
+			return ret;
+		}
+
+		msg->action = F2I_MIXER_ACTION_APPEND;
+
+	} while (buflen > 0);
+
+	/* check for the mixer-OK flag */
+	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
+		debug("mixer upload OK");
+		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
+		return 0;
+	} else {
+		debug("mixer rejected by IO");
+		mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+	}
+
+	/* load must have failed for some reason */
+	return -EINVAL;
+}
+
+void
+PX4IO::print_status()
+{
+	/* basic configuration */
+	printf("protocol %u software %u bootloader %u buffer %uB\n",
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
+	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
+
+	/* status */
+	printf("%u bytes free\n",
+		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
+	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
+		flags,
+		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
+		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
+		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
+		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
+		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"));
+	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
+	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
+		alarms,
+		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+	printf("vbatt %u ibatt %u vbatt scale %u\n",
+	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+	printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
+	       (double)_battery_amp_per_volt,
+	       (double)_battery_amp_bias,
+	       (double)_battery_mamphour_total);
+	printf("actuators");
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
+	printf("\n");
+	printf("servos");
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));
+	printf("\n");
+	uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
+	printf("%d raw R/C inputs", raw_inputs);
+	for (unsigned i = 0; i < raw_inputs; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
+	printf("\n");
+	uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
+	printf("mapped R/C inputs 0x%04x", mapped_inputs);
+	for (unsigned i = 0; i < _max_rc_input; i++) {
+		if (mapped_inputs & (1 << i))
+			printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)));
+	}
+	printf("\n");
+	uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT);
+	printf("ADC inputs");
+	for (unsigned i = 0; i < adc_inputs; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i));
+	printf("\n");
+
+	/* setup and state */
+	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
+	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
+	printf("arming 0x%04x%s%s%s%s\n",
+		arming,
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
+	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
+	printf("controls");
+	for (unsigned i = 0; i < _max_controls; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
+	printf("\n");
+	for (unsigned i = 0; i < _max_rc_input; i++) {
+		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
+		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
+			i,
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+			options,
+			((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+			((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+	}
+	printf("failsafe");
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+	printf("\n");
+}
+
+int
+PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	/* regular ioctl? */
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		/* set the 'armed' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
+		break;
+
+	case PWM_SERVO_SET_ARM_OK:
+		/* set the 'OK to arm' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_ARM_OK:
+		/* clear the 'OK to arm' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
+		break;
+
+	case PWM_SERVO_DISARM:
+		/* clear the 'armed' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
+		break;
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		/* set the requested alternate rate */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE: {
+
+		/* blindly clear the PWM update alarm - might be set for some other reason */
+		io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+
+		/* attempt to set the rate map */
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+
+		/* check that the changes took */
+		uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
+		if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
+			ret = -EINVAL;
+			io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+		}
+		break;
+	}
+
+	case PWM_SERVO_GET_COUNT:
+		*(unsigned *)arg = _max_actuators;
+		break;
+
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
+
+		/* TODO: we could go lower for e.g. TurboPWM */
+		unsigned channel = cmd - PWM_SERVO_SET(0);
+		if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
+			ret = -EINVAL;
+		} else {
+			/* send a direct PWM value */
+			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+		}
+
+		break;
+	}
+
+	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
+
+		unsigned channel = cmd - PWM_SERVO_GET(0);
+
+		if (channel >= _max_actuators) {
+			ret = -EINVAL;
+		} else {
+			/* fetch a current PWM value */
+			uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
+			if (value == _io_reg_get_error) {
+				ret = -EIO;
+			} else {
+				*(servo_position_t *)arg = value;
+			}
+		}
+		break;
+	}
+
+	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+
+		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+		uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+
+		*(uint32_t *)arg = value;
+		break;
+	}
+
+	case GPIO_RESET:
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
+		break;
+
+	case GPIO_SET:
+		arg &= ((1 << _max_relays) - 1);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+		break;
+
+	case GPIO_CLEAR:
+		arg &= ((1 << _max_relays) - 1);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+		break;
+
+	case GPIO_GET:
+		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
+		if (*(uint32_t *)arg == _io_reg_get_error)
+			ret = -EIO;
+		break;
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		*(unsigned *)arg = _max_actuators;
+		break;
+
+	case MIXERIOCRESET:
+		ret = 0;	/* load always resets */
+		break;
+
+	case MIXERIOCLOADBUF: {
+		const char *buf = (const char *)arg;
+		ret = mixer_send(buf, strnlen(buf, 1024));
+		break;
+	}
+
+	case RC_INPUT_GET: {
+		uint16_t status;
+		rc_input_values *rc_val = (rc_input_values *)arg;
+
+		ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1);
+		if (ret != OK)
+			break;
+
+		/* if no R/C input, don't try to fetch anything */
+		if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+			ret = -ENOTCONN;
+			break;
+		}
+
+		/* sort out the source of the values */
+		if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
+			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+		} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
+			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+		} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+		} else {
+			rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
+		}
+
+		/* read raw R/C input values */
+		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input);
+		break;
+	}
+
+	case PX4IO_SET_DEBUG:
+		/* set the debug level */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
+		break;
+
+	case PX4IO_INAIR_RESTART_ENABLE:
+		/* set/clear the 'in-air restart' bit */
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
+		}
+		break;
+
+	default:
+		/* not a recognized value */
+		ret = -ENOTTY;
+	}
+
+	return ret;
+}
+
+ssize_t
+PX4IO::write(file *filp, const char *buffer, size_t len)
+{
+	unsigned count = len / 2;
+
+	if (count > _max_actuators)
+		count = _max_actuators;
+	if (count > 0) {
+		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+		if (ret != OK)
+			return ret;
+	}
+	return count * 2;
+}
+
+int
+PX4IO::set_update_rate(int rate)
+{
+	int interval_ms = 1000 / rate;
+	if (interval_ms < 5) {
+		interval_ms = 5;
+		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
+	}
+
+	if (interval_ms > 100) {
+		interval_ms = 100;
+		warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
+	}
+
+	_update_interval = interval_ms;
+	return 0;
+}
+
+void
+PX4IO::set_battery_current_scaling(float amp_per_volt, float amp_bias)
+{
+	_battery_amp_per_volt = amp_per_volt;
+	_battery_amp_bias = amp_bias;
+}
+
+extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
+
+namespace
+{
+
+void
+start(int argc, char *argv[])
+{
+	if (g_dev != nullptr)
+		errx(1, "already loaded");
+
+	/* create the driver - it will set g_dev */
+	(void)new PX4IO();
+
+	if (g_dev == nullptr)
+		errx(1, "driver alloc failed");
+
+	if (OK != g_dev->init()) {
+		delete g_dev;
+		errx(1, "driver init failed");
+	}
+
+	exit(0);
+}
+
+void
+test(void)
+{
+	int		fd;
+	unsigned	servo_count = 0;
+	unsigned	pwm_value = 1000;
+	int		direction = 1;
+	int		ret;
+
+	fd = open("/dev/px4io", O_WRONLY);
+
+	if (fd < 0)
+		err(1, "failed to open device");
+
+	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
+		err(1, "failed to get servo count");
+
+	if (ioctl(fd, PWM_SERVO_ARM, 0))
+		err(1, "failed to arm servos");
+
+	for (;;) {
+
+		/* sweep all servos between 1000..2000 */
+		servo_position_t servos[servo_count];
+		for (unsigned i = 0; i < servo_count; i++)
+			servos[i] = pwm_value;
+
+		ret = write(fd, servos, sizeof(servos));
+		if (ret != (int)sizeof(servos))
+			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+
+		if (direction > 0) {
+			if (pwm_value < 2000) {
+				pwm_value++;
+			} else {
+				direction = -1;
+			}
+		} else {
+			if (pwm_value > 1000) {
+				pwm_value--;
+			} else {
+				direction = 1;
+			}
+		}
+
+		/* readback servo values */
+		for (unsigned i = 0; i < servo_count; i++) {
+			servo_position_t value;
+
+			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+				err(1, "error reading PWM servo %d", i);
+			if (value != servos[i])
+				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+		}
+	}
+}
+
+void
+monitor(void)
+{
+	unsigned cancels = 3;
+	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);
+		}
+
+#warning implement this
+
+//		if (g_dev != nullptr)
+//			g_dev->dump_one = true;
+	}
+}
+
+}
+
+int
+px4io_main(int argc, char *argv[])
+{
+	/* check for sufficient number of arguments */
+	if (argc < 2)
+		goto out;
+
+	if (!strcmp(argv[1], "start"))
+		start(argc - 1, argv + 1);
+
+	if (!strcmp(argv[1], "limit")) {
+
+		if (g_dev != nullptr) {
+
+			if ((argc > 2)) {
+				g_dev->set_update_rate(atoi(argv[2]));
+			} else {
+				errx(1, "missing argument (50 - 200 Hz)");
+				return 1;
+			}
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "current")) {
+		if (g_dev != nullptr) {
+			if ((argc > 3)) {
+				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+			} else {
+				errx(1, "missing argument (apm_per_volt, amp_offset)");
+				return 1;
+			}
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "recovery")) {
+
+		if (g_dev != nullptr) {
+			/*
+			 * Enable in-air restart support.
+			 * We can cheat and call the driver directly, as it
+		 	 * doesn't reference filp in ioctl()
+			 */
+			g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		if (g_dev != nullptr) {
+			/* stop the driver */
+			delete g_dev;
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
+
+	if (!strcmp(argv[1], "status")) {
+
+		if (g_dev != nullptr) {
+			printf("[px4io] loaded\n");
+			g_dev->print_status();
+		} else {
+			printf("[px4io] not loaded\n");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "debug")) {
+		if (argc <= 2) {
+			printf("usage: px4io debug LEVEL\n");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			printf("px4io is not started\n");
+			exit(1);
+		}
+		uint8_t level = atoi(argv[2]);
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
+		if (ret != 0) {
+			printf("SET_DEBUG failed - %d\n", ret);
+			exit(1);
+		}
+		printf("SET_DEBUG %u OK\n", (unsigned)level);
+		exit(0);
+	}
+
+	/* note, stop not currently implemented */
+
+	if (!strcmp(argv[1], "update")) {
+
+		if (g_dev != nullptr) {
+			printf("[px4io] loaded, detaching first\n");
+			/* stop the driver */
+			delete g_dev;
+		}
+
+		PX4IO_Uploader *up;
+		const char *fn[3];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+
+		} else {
+			fn[0] = "/fs/microsd/px4io.bin";
+			fn[1] =	"/etc/px4io.bin";
+			fn[2] =	nullptr;
+		}
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+		default:
+			errx(1, "unexpected error %d", ret);
+		}
+
+		return ret;
+	}
+
+	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();
+
+	if (!strcmp(argv[1], "monitor"))
+		monitor();
+
+	out:
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
+}
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp
new file mode 100644
index 000000000..15524c3ae
--- /dev/null
+++ b/src/drivers/px4io/uploader.cpp
@@ -0,0 +1,619 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 uploader.cpp
+ * Firmware uploader for PX4IO
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <sys/stat.h>
+
+#include "uploader.h"
+
+static const uint32_t crctab[] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+static uint32_t
+crc32(const uint8_t *src, unsigned len, unsigned state)
+{
+	for (unsigned i = 0; i < len; i++)
+		state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
+	return state;
+}
+
+PX4IO_Uploader::PX4IO_Uploader() :
+	_io_fd(-1),
+	_fw_fd(-1)
+{
+}
+
+PX4IO_Uploader::~PX4IO_Uploader()
+{
+}
+
+int
+PX4IO_Uploader::upload(const char *filenames[])
+{
+	int	ret;
+	const char *filename = NULL;
+	size_t fw_size;
+
+	_io_fd = open("/dev/ttyS2", O_RDWR);
+
+	if (_io_fd < 0) {
+		log("could not open interface");
+		return -errno;
+	}
+
+	/* look for the bootloader */
+	ret = sync();
+
+	if (ret != OK) {
+		/* this is immediately fatal */
+		log("bootloader not responding");
+		close(_io_fd);
+		_io_fd = -1;
+		return -EIO;
+	}
+
+	for (unsigned i = 0; filenames[i] != nullptr; i++) {
+		_fw_fd = open(filenames[i], O_RDONLY);
+
+		if (_fw_fd < 0) {
+			log("failed to open %s", filenames[i]);
+			continue;
+		}
+
+		log("using firmware from %s", filenames[i]);
+		filename = filenames[i];
+		break;
+	}
+
+	if (filename == NULL) {
+		log("no firmware found");
+		close(_io_fd);
+		_io_fd = -1;
+		return -ENOENT;
+	}
+
+	struct stat st;
+	if (stat(filename, &st) != 0) {
+		log("Failed to stat %s - %d\n", filename, (int)errno);
+		close(_io_fd);
+		_io_fd = -1;
+		return -errno;
+	}
+	fw_size = st.st_size;
+
+	if (_fw_fd == -1) {
+		close(_io_fd);
+		_io_fd = -1;
+		return -ENOENT;
+	}
+
+	/* do the usual program thing - allow for failure */
+	for (unsigned retries = 0; retries < 1; retries++) {
+		if (retries > 0) {
+			log("retrying update...");
+			ret = sync();
+
+			if (ret != OK) {
+				/* this is immediately fatal */
+				log("bootloader not responding");
+				close(_io_fd);
+				_io_fd = -1;
+				return -EIO;
+			}
+		}
+
+		ret = get_info(INFO_BL_REV, bl_rev);
+
+		if (ret == OK) {
+			if (bl_rev <= BL_REV) {
+				log("found bootloader revision: %d", bl_rev);
+			} else {
+				log("found unsupported bootloader revision %d, exiting", bl_rev);
+				close(_io_fd);
+				_io_fd = -1;
+				return OK;
+			}
+		}
+
+		ret = erase();
+
+		if (ret != OK) {
+			log("erase failed");
+			continue;
+		}
+
+		ret = program(fw_size);
+
+		if (ret != OK) {
+			log("program failed");
+			continue;
+		}
+
+		if (bl_rev <= 2)
+			ret = verify_rev2(fw_size);
+		else if(bl_rev == 3) {
+			ret = verify_rev3(fw_size);
+		}
+
+		if (ret != OK) {
+			log("verify failed");
+			continue;
+		}
+
+		ret = reboot();
+
+		if (ret != OK) {
+			log("reboot failed");
+			return ret;
+		}
+
+		log("update complete");
+
+		ret = OK;
+		break;
+	}
+
+	close(_fw_fd);
+	close(_io_fd);
+	_io_fd = -1;
+	return ret;
+}
+
+int
+PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
+{
+	struct pollfd fds[1];
+
+	fds[0].fd = _io_fd;
+	fds[0].events = POLLIN;
+
+	/* wait 100 ms for a character */
+	int ret = ::poll(&fds[0], 1, timeout);
+
+	if (ret < 1) {
+		log("poll timeout %d", ret);
+		return -ETIMEDOUT;
+	}
+
+	read(_io_fd, &c, 1);
+	//log("recv 0x%02x", c);
+	return OK;
+}
+
+int
+PX4IO_Uploader::recv(uint8_t *p, unsigned count)
+{
+	while (count--) {
+		int ret = recv(*p++, 5000);
+
+		if (ret != OK)
+			return ret;
+	}
+
+	return OK;
+}
+
+void
+PX4IO_Uploader::drain()
+{
+	uint8_t c;
+	int ret;
+
+	do {
+		ret = recv(c, 1000);
+
+		if (ret == OK) {
+			//log("discard 0x%02x", c);
+		}
+	} while (ret == OK);
+}
+
+int
+PX4IO_Uploader::send(uint8_t c)
+{
+	//log("send 0x%02x", c);
+	if (write(_io_fd, &c, 1) != 1)
+		return -errno;
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::send(uint8_t *p, unsigned count)
+{
+	while (count--) {
+		int ret = send(*p++);
+
+		if (ret != OK)
+			return ret;
+	}
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::get_sync(unsigned timeout)
+{
+	uint8_t c[2];
+	int ret;
+
+	ret = recv(c[0], timeout);
+
+	if (ret != OK)
+		return ret;
+
+	ret = recv(c[1], timeout);
+
+	if (ret != OK)
+		return ret;
+
+	if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
+		log("bad sync 0x%02x,0x%02x", c[0], c[1]);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::sync()
+{
+	drain();
+
+	/* complete any pending program operation */
+	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
+		send(0);
+
+	send(PROTO_GET_SYNC);
+	send(PROTO_EOC);
+	return get_sync();
+}
+
+int
+PX4IO_Uploader::get_info(int param, uint32_t &val)
+{
+	int ret;
+
+	send(PROTO_GET_DEVICE);
+	send(param);
+	send(PROTO_EOC);
+
+	ret = recv((uint8_t *)&val, sizeof(val));
+
+	if (ret != OK)
+		return ret;
+
+	return get_sync();
+}
+
+int
+PX4IO_Uploader::erase()
+{
+	log("erase...");
+	send(PROTO_CHIP_ERASE);
+	send(PROTO_EOC);
+	return get_sync(10000);		/* allow 10s timeout */
+}
+
+
+static int read_with_retry(int fd, void *buf, size_t n)
+{
+	int ret;
+	uint8_t retries = 0;
+	do {
+		ret = read(fd, buf, n);
+	} while (ret == -1 && retries++ < 100);
+	if (retries != 0) {
+		printf("read of %u bytes needed %u retries\n",
+		       (unsigned)n,
+		       (unsigned)retries);
+	}
+	return ret;
+}
+
+int
+PX4IO_Uploader::program(size_t fw_size)
+{
+	uint8_t	file_buf[PROG_MULTI_MAX];
+	ssize_t count;
+	int ret;
+	size_t sent = 0;
+
+	log("programming %u bytes...", (unsigned)fw_size);
+
+	ret = lseek(_fw_fd, 0, SEEK_SET);
+
+	while (sent < fw_size) {
+		/* get more bytes to program */
+		size_t n = fw_size - sent;
+		if (n > sizeof(file_buf)) {
+			n = sizeof(file_buf);
+		}
+		count = read_with_retry(_fw_fd, file_buf, n);
+
+		if (count != (ssize_t)n) {
+			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			    (unsigned)n,
+			    (unsigned)sent,
+			    (int)count,
+			    (int)errno);
+		}
+
+		if (count == 0)
+			return OK;
+
+		sent += count;
+
+		if (count < 0)
+			return -errno;
+
+		ASSERT((count % 4) == 0);
+
+		send(PROTO_PROG_MULTI);
+		send(count);
+		send(&file_buf[0], count);
+		send(PROTO_EOC);
+
+		ret = get_sync(1000);
+
+		if (ret != OK)
+			return ret;
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::verify_rev2(size_t fw_size)
+{
+	uint8_t	file_buf[4];
+	ssize_t count;
+	int ret;
+	size_t sent = 0;
+
+	log("verify...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	send(PROTO_CHIP_VERIFY);
+	send(PROTO_EOC);
+	ret = get_sync();
+
+	if (ret != OK)
+		return ret;
+
+	while (sent < fw_size) {
+		/* get more bytes to verify */
+		size_t n = fw_size - sent;
+		if (n > sizeof(file_buf)) {
+			n = sizeof(file_buf);
+		}
+		count = read_with_retry(_fw_fd, file_buf, n);
+
+		if (count != (ssize_t)n) {
+			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			    (unsigned)n,
+			    (unsigned)sent,
+			    (int)count,
+			    (int)errno);
+		}
+
+		if (count == 0)
+			break;
+
+		sent += count;
+
+		if (count < 0)
+			return -errno;
+
+		ASSERT((count % 4) == 0);
+
+		send(PROTO_READ_MULTI);
+		send(count);
+		send(PROTO_EOC);
+
+		for (ssize_t i = 0; i < count; i++) {
+			uint8_t c;
+
+			ret = recv(c, 5000);
+
+			if (ret != OK) {
+				log("%d: got %d waiting for bytes", sent + i, ret);
+				return ret;
+			}
+
+			if (c != file_buf[i]) {
+				log("%d: got 0x%02x expected 0x%02x", sent + i, c, file_buf[i]);
+				return -EINVAL;
+			}
+		}
+
+		ret = get_sync();
+
+		if (ret != OK) {
+			log("timeout waiting for post-verify sync");
+			return ret;
+		}
+	}
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::verify_rev3(size_t fw_size_local)
+{
+	int ret;
+	uint8_t	file_buf[4];
+	ssize_t count;
+	uint32_t sum = 0;
+	uint32_t bytes_read = 0;
+	uint32_t crc = 0;
+	uint32_t fw_size_remote;
+	uint8_t fill_blank = 0xff;
+
+	log("verify...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	ret = get_info(INFO_FLASH_SIZE, fw_size_remote);
+	send(PROTO_EOC);
+
+	if (ret != OK) {
+		log("could not read firmware size");
+		return ret;
+	}
+
+	/* read through the firmware file again and calculate the checksum*/
+	while (bytes_read < fw_size_local) {
+		size_t n = fw_size_local - bytes_read;
+		if (n > sizeof(file_buf)) {
+			n = sizeof(file_buf);
+		}
+		count = read_with_retry(_fw_fd, file_buf, n);
+
+		if (count != (ssize_t)n) {
+			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			    (unsigned)n,
+			    (unsigned)bytes_read,
+			    (int)count,
+			    (int)errno);
+		}
+
+		/* set the rest to ff */
+		if (count == 0) {
+			break;
+		}
+		/* stop if the file cannot be read */
+		if (count < 0)
+			return -errno;
+
+		/* calculate crc32 sum */
+		sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+
+		bytes_read += count;
+	}
+
+	/* fill the rest with 0xff */
+	while (bytes_read < fw_size_remote) {
+		sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+		bytes_read += sizeof(fill_blank);
+	}
+
+	/* request CRC from IO */
+	send(PROTO_GET_CRC);
+	send(PROTO_EOC);
+
+	ret = recv((uint8_t*)(&crc), sizeof(crc));
+
+	if (ret != OK) {
+		log("did not receive CRC checksum");
+		return ret;
+	}
+
+	/* compare the CRC sum from the IO with the one calculated */
+	if (sum != crc) {
+		log("CRC wrong: received: %d, expected: %d", crc, sum);
+		return -EINVAL;
+	}
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::reboot()
+{
+	send(PROTO_REBOOT);
+	send(PROTO_EOC);
+
+	return OK;
+}
+
+void
+PX4IO_Uploader::log(const char *fmt, ...)
+{
+	va_list	ap;
+
+	printf("[PX4IO] ");
+	va_start(ap, fmt);
+	vprintf(fmt, ap);
+	va_end(ap);
+	printf("\n");
+	fflush(stdout);
+}
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
new file mode 100644
index 000000000..135202dd1
--- /dev/null
+++ b/src/drivers/px4io/uploader.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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 uploader.h
+ * Firmware uploader definitions for PX4IO.
+ */
+
+#ifndef _PX4IO_UPLOADER_H
+#define _PX4IO_UPLOADER_H value
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+class PX4IO_Uploader
+{
+public:
+	PX4IO_Uploader();
+	virtual ~PX4IO_Uploader();
+
+	int			upload(const char *filenames[]);
+
+private:
+	enum {
+
+		PROTO_NOP		= 0x00,
+		PROTO_OK		= 0x10,
+		PROTO_FAILED		= 0x11,
+		PROTO_INSYNC		= 0x12,
+		PROTO_EOC		= 0x20,
+		PROTO_GET_SYNC		= 0x21,
+		PROTO_GET_DEVICE	= 0x22,
+		PROTO_CHIP_ERASE	= 0x23,
+		PROTO_CHIP_VERIFY	= 0x24,
+		PROTO_PROG_MULTI	= 0x27,
+		PROTO_READ_MULTI	= 0x28,
+		PROTO_GET_CRC		= 0x29,
+		PROTO_REBOOT		= 0x30,
+
+		INFO_BL_REV		= 1,		/**< bootloader protocol revision */
+		BL_REV			= 3,		/**< supported bootloader protocol  */
+		INFO_BOARD_ID		= 2,		/**< board type */
+		INFO_BOARD_REV		= 3,		/**< board revision */
+		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
+
+		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
+		READ_MULTI_MAX		= 60,		/**< protocol max is 255, something overflows with >= 64 */
+
+	};
+
+	int			_io_fd;
+	int			_fw_fd;
+
+	uint32_t	bl_rev; /**< bootloader revision */
+
+	void			log(const char *fmt, ...);
+
+	int			recv(uint8_t &c, unsigned timeout);
+	int			recv(uint8_t *p, unsigned count);
+	void			drain();
+	int			send(uint8_t c);
+	int			send(uint8_t *p, unsigned count);
+	int			get_sync(unsigned timeout = 1000);
+	int			sync();
+	int			get_info(int param, uint32_t &val);
+	int			erase();
+	int			program(size_t fw_size);
+	int			verify_rev2(size_t fw_size);
+	int			verify_rev3(size_t fw_size);
+	int			reboot();
+};
+
+#endif
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
new file mode 100644
index 000000000..911def943
--- /dev/null
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ *
+ * Driver for the STM32 ADC.
+ *
+ * This is a low-rate driver, designed for sampling things like voltages
+ * and so forth. It avoids the gross complexity of the NuttX ADC driver.
+ */
+
+#include <nuttx/config.h>
+#include <drivers/device/device.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_adc.h>
+
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.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)
+
+#ifdef STM32_ADC_CCR
+# define rCCR		REG(STM32_ADC_CCR_OFFSET)
+#endif
+
+class ADC : public device::CDev
+{
+public:
+	ADC(uint32_t channels);
+	~ADC();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t		read(file *filp, char *buffer, size_t len);
+
+protected:
+	virtual int		open_first(struct file *filp);
+	virtual int		close_last(struct file *filp);
+
+private:
+	static const hrt_abstime _tickrate = 10000;	/**< 100Hz base rate */
+	
+	hrt_call		_call;
+	perf_counter_t		_sample_perf;
+
+	unsigned		_channel_count;
+	adc_msg_s		*_samples;		/**< sample buffer */
+
+	/** work trampoline */
+	static void		_tick_trampoline(void *arg);
+
+	/** worker function */
+	void			_tick();
+
+	/**
+	 * Sample a single channel and return the measured value.
+	 *
+	 * @param channel		The channel to sample.
+	 * @return			The sampled value, or 0xffff if
+	 *				sampling failed.
+	 */
+	uint16_t		_sample(unsigned channel);
+
+};
+
+ADC::ADC(uint32_t channels) :
+	CDev("adc", ADC_DEVICE_PATH),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+	_channel_count(0),
+	_samples(nullptr)
+{
+	_debug_enabled = true;
+
+	/* always enable the temperature sensor */
+	channels |= 1 << 16;
+
+	/* allocate the sample array */
+	for (unsigned i = 0; i < 32; i++) {
+		if (channels & (1 << i)) {
+			_channel_count++;
+		}
+	}
+	_samples = new adc_msg_s[_channel_count];
+
+	/* prefill the channel numbers in the sample array */
+	if (_samples != nullptr) {
+		unsigned index = 0;
+		for (unsigned i = 0; i < 32; i++) {
+			if (channels & (1 << i)) {
+				_samples[index].am_channel = i;
+				_samples[index].am_data = 0;
+				index++;
+			}
+		}
+	}
+}
+
+ADC::~ADC()
+{
+	if (_samples != nullptr)
+		delete _samples;
+}
+
+int
+ADC::init()
+{
+	/* do calibration if supported */
+#ifdef ADC_CR2_CAL
+	rCR2 |= ADC_CR2_CAL;
+	usleep(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;
+	usleep(10);
+	rCR2 |= ADC_CR2_ADON;
+	usleep(10);
+	rCR2 |= ADC_CR2_ADON;
+	usleep(10);
+
+	/* kick off a sample and wait for it to complete */
+	hrt_abstime now = hrt_absolute_time();
+	rCR2 |= ADC_CR2_SWSTART;
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 500) {
+			log("sample timeout");
+			return -1;
+			return 0xffff;
+		}
+	}
+
+
+	debug("init done");
+
+	/* create the device node */
+	return CDev::init();
+}
+
+int
+ADC::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	return -ENOTTY;
+}
+
+ssize_t
+ADC::read(file *filp, char *buffer, size_t len)
+{
+	const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
+
+	if (len > maxsize)
+		len = maxsize;
+
+	/* block interrupts while copying samples to avoid racing with an update */
+	irqstate_t flags = irqsave();
+	memcpy(buffer, _samples, len);
+	irqrestore(flags);
+
+	return len;
+}
+
+int
+ADC::open_first(struct file *filp)
+{
+	/* get fresh data */
+	_tick();
+
+	/* and schedule regular updates */
+	hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
+
+	return 0;
+}
+
+int
+ADC::close_last(struct file *filp)
+{
+	hrt_cancel(&_call);
+	return 0;
+}
+
+void
+ADC::_tick_trampoline(void *arg)
+{
+	((ADC *)arg)->_tick();
+}
+
+void
+ADC::_tick()
+{
+	/* scan the channel set and sample each */
+	for (unsigned i = 0; i < _channel_count; i++)
+		_samples[i].am_data = _sample(_samples[i].am_channel);
+}
+
+uint16_t
+ADC::_sample(unsigned channel)
+{
+	perf_begin(_sample_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_SWSTART;
+
+	/* wait for the conversion to complete */
+	hrt_abstime now = hrt_absolute_time();
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 50) {
+			log("sample timeout");
+			return 0xffff;
+		}
+	}
+
+	/* read the result and clear EOC */
+	uint16_t result = rDR;
+
+	perf_end(_sample_perf);
+	return result;
+}
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int adc_main(int argc, char *argv[]);
+
+namespace
+{
+ADC	*g_adc;
+
+void
+test(void)
+{
+
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+	if (fd < 0)
+		err(1, "can't open ADC device");
+
+	for (unsigned i = 0; i < 50; i++) {
+		adc_msg_s data[8];
+		ssize_t count = read(fd, data, sizeof(data));
+
+		if (count < 0)
+			errx(1, "read error");
+
+		unsigned channels = count / sizeof(data[0]);
+
+		for (unsigned j = 0; j < channels; j++) {
+			printf ("%d: %u  ", data[j].am_channel, data[j].am_data);
+		}
+
+		printf("\n");
+		usleep(500000);
+	}
+
+	exit(0);
+}
+}
+
+int
+adc_main(int argc, char *argv[])
+{
+	if (g_adc == nullptr) {
+		/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+
+		if (g_adc == nullptr)
+			errx(1, "couldn't allocate the ADC driver");
+
+		if (g_adc->init() != OK) {
+			delete g_adc;
+			errx(1, "ADC init failed");
+		}
+	}
+
+	if (argc > 1) {
+		if (!strcmp(argv[1], "test"))
+			test();
+	}
+
+	exit(0);
+}
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
new file mode 100644
index 000000000..48620feea
--- /dev/null
+++ b/src/drivers/stm32/adc/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# STM32 ADC driver
+#
+
+MODULE_COMMAND	= adc
+
+SRCS		= adc.cpp
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
new file mode 100644
index 000000000..cec0c49ff
--- /dev/null
+++ b/src/drivers/stm32/drv_hrt.c
@@ -0,0 +1,908 @@
+/****************************************************************************
+ *
+ *   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_hrt.c
+ *
+ * High-resolution timer callouts and timekeeping.
+ *
+ * This can use any general or advanced STM32 timer.
+ *
+ * Note that really, this could use systick too, but that's
+ * monopolised by NuttX and stealing it would just be awkward.
+ *
+ * We don't use the NuttX STM32 driver per se; rather, we
+ * claim the timer and then drive it directly.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+#ifdef CONFIG_HRT_TIMER
+
+/* HRT configuration */
+#if   HRT_TIMER == 1
+# define HRT_TIMER_BASE		STM32_TIM1_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
+# if CONFIG_STM32_TIM1
+#  error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
+# endif
+#elif HRT_TIMER == 2
+# define HRT_TIMER_BASE		STM32_TIM2_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM2
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
+# if CONFIG_STM32_TIM2
+#  error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2
+# endif
+#elif HRT_TIMER == 3
+# define HRT_TIMER_BASE		STM32_TIM3_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM3EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM3
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
+# if CONFIG_STM32_TIM3
+#  error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3
+# endif
+#elif HRT_TIMER == 4
+# define HRT_TIMER_BASE		STM32_TIM4_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM4EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM4
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
+# if CONFIG_STM32_TIM4
+#  error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4
+# endif
+#elif HRT_TIMER == 5
+# define HRT_TIMER_BASE		STM32_TIM5_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM5
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
+# if CONFIG_STM32_TIM5
+#  error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5
+# endif
+#elif HRT_TIMER == 8
+# define HRT_TIMER_BASE		STM32_TIM8_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM8CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
+# if CONFIG_STM32_TIM8
+#  error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8
+# endif
+#elif HRT_TIMER == 9
+# define HRT_TIMER_BASE		STM32_TIM9_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1BRK
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
+# if CONFIG_STM32_TIM9
+#  error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9
+# endif
+#elif HRT_TIMER == 10
+# define HRT_TIMER_BASE		STM32_TIM10_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1UP
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM10_CLKIN
+# if CONFIG_STM32_TIM10
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
+# endif
+#elif HRT_TIMER == 11
+# define HRT_TIMER_BASE		STM32_TIM11_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM11_CLKIN
+# if CONFIG_STM32_TIM11
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
+# endif
+#else
+# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
+#endif
+
+/*
+ * HRT clock must be a multiple of 1MHz greater than 1MHz
+ */
+#if (HRT_TIMER_CLOCK % 1000000) != 0
+# error HRT_TIMER_CLOCK must be a multiple of 1MHz
+#endif
+#if HRT_TIMER_CLOCK <= 1000000
+# error HRT_TIMER_CLOCK must be greater than 1MHz
+#endif
+
+/*
+ * Minimum/maximum deadlines.
+ *
+ * These are suitable for use with a 16-bit timer/counter clocked
+ * at 1MHz.  The high-resolution timer need only guarantee that it
+ * not wrap more than once in the 50ms period for absolute time to
+ * be consistently maintained.
+ *
+ * The minimum deadline must be such that the time taken between
+ * reading a time and writing a deadline to the timer cannot
+ * result in missing the deadline.
+ */
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
+/*
+ * Period of the free-running counter, in microseconds.
+ */
+#define HRT_COUNTER_PERIOD	65536
+
+/*
+ * Scaling factor(s) for the free-running counter; convert an input
+ * in counts to a time in microseconds.
+ */
+#define HRT_COUNTER_SCALE(_c)	(_c)
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if HRT_TIMER_CHANNEL == 1
+# define rCCR_HRT	rCCR1			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC1IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC1IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 2
+# define rCCR_HRT	rCCR2			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC2IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC2IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 3
+# define rCCR_HRT	rCCR3			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC3IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC3IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 4
+# define rCCR_HRT	rCCR4			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC4IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC4IF		/* interrupt status for HRT */
+#else
+# error HRT_TIMER_CHANNEL must be a value between 1 and 4
+#endif
+
+/*
+ * Queue of callout entries.
+ */
+static struct sq_queue_s	callout_queue;
+
+/* latency baseline (last compare value applied) */
+static uint16_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint16_t			latency_actual;
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT	8
+static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+/* timer-specific functions */
+static void		hrt_tim_init(void);
+static int		hrt_tim_isr(int irq, void *context);
+static void		hrt_latency_update(void);
+
+/* callout list manipulation */
+static void		hrt_call_internal(struct hrt_call *entry,
+		hrt_abstime deadline,
+		hrt_abstime interval,
+		hrt_callout callout,
+		void *arg);
+static void		hrt_call_enter(struct hrt_call *entry);
+static void		hrt_call_reschedule(void);
+static void		hrt_call_invoke(void);
+
+/*
+ * Specific registers and bits used by PPM sub-functions
+ */
+#ifdef CONFIG_HRT_PPM
+/* 
+ * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
+ */
+# ifndef GTIM_CCER_CC1NP
+#  define GTIM_CCER_CC1NP 0
+#  define GTIM_CCER_CC2NP 0
+#  define GTIM_CCER_CC3NP 0
+#  define GTIM_CCER_CC4NP 0
+#  define PPM_EDGE_FLIP
+# endif
+
+# if HRT_PPM_CHANNEL == 1
+#  define rCCR_PPM	rCCR1			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC1IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC1IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC1OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	1			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC1P
+# elif HRT_PPM_CHANNEL == 2
+#  define rCCR_PPM	rCCR2			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC2IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC2IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC2OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	2			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC2P
+# elif HRT_PPM_CHANNEL == 3
+#  define rCCR_PPM	rCCR3			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC3IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC3IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC3OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	1			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC3P
+# elif HRT_PPM_CHANNEL == 4
+#  define rCCR_PPM	rCCR4			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC4IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC4IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC4OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	2			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC4P
+# else
+#  error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
+# endif
+
+/*
+ * PPM decoder tuning parameters
+ */
+# define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+# define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* decoded PPM buffer */
+#define PPM_MAX_CHANNELS	12
+__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT unsigned ppm_decoded_channels = 0;
+__EXPORT uint64_t ppm_last_valid_decode = 0;
+
+/* PPM edge history */
+__EXPORT uint16_t ppm_edge_history[32];
+unsigned ppm_edge_next;
+
+/* PPM pulse history */
+__EXPORT uint16_t ppm_pulse_history[32];
+unsigned ppm_pulse_next;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+static void	hrt_ppm_decode(uint32_t status);
+
+#else
+/* disable the PPM configuration */
+# define rCCR_PPM	0
+# define DIER_PPM	0
+# define SR_INT_PPM	0
+# define SR_OVF_PPM	0
+# define CCMR1_PPM	0
+# define CCMR2_PPM	0
+# define CCER_PPM	0
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Initialise the timer we are going to use.
+ *
+ * We expect that we'll own one of the reduced-function STM32 general
+ * timers, and that we can use channel 1 in compare mode.
+ */
+static void
+hrt_tim_init(void)
+{
+	/* claim our interrupt vector */
+	irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
+
+	/* clock/power on our timer */
+	modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
+
+	/* disable and configure the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = DIER_HRT | DIER_PPM;
+	rCCER = 0;		/* unlock CCMR* registers */
+	rCCMR1 = CCMR1_PPM;
+	rCCMR2 = CCMR2_PPM;
+	rCCER = CCER_PPM;
+	rDCR = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC = (HRT_TIMER_CLOCK / 1000000) - 1;	/* this really only works for whole-MHz clocks */
+
+	/* run the full span of the counter */
+	rARR = 0xffff;
+
+	/* set an initial capture a little ways off */
+	rCCR_HRT = 1000;
+
+	/* generate an update event; reloads the counter, all registers */
+	rEGR = GTIM_EGR_UG;
+
+	/* enable the timer */
+	rCR1 = GTIM_CR1_CEN;
+
+	/* enable interrupts */
+	up_enable_irq(HRT_TIMER_VECTOR);
+}
+
+#ifdef CONFIG_HRT_PPM
+/*
+ * Handle the PPM decoder state machine.
+ */
+static void
+hrt_ppm_decode(uint32_t status)
+{
+	uint16_t count = rCCR_PPM;
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (status & SR_OVF_PPM)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+	ppm.last_edge = count;
+
+	ppm_edge_history[ppm_edge_next++] = width;
+
+	if (ppm_edge_next >= 32)
+		ppm_edge_next = 0;
+
+	/*
+	 * if this looks like a start pulse, then push the last set of values
+	 * and reset the state machine
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/* export the last set of samples if we got something sensible */
+		if (ppm.next_channel > 4) {
+			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
+				ppm_buffer[i] = ppm_temp_buffer[i];
+
+			ppm_decoded_channels = i;
+			ppm_last_valid_decode = hrt_absolute_time();
+
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+		return;
+
+	case ACTIVE:
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		ppm_pulse_history[ppm_pulse_next++] = interval;
+
+		if (ppm_pulse_next >= 32)
+			ppm_pulse_next = 0;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+
+}
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Handle the compare interupt by calling the callout dispatcher
+ * and then re-scheduling the next deadline.
+ */
+static int
+hrt_tim_isr(int irq, void *context)
+{
+	uint32_t status;
+
+	/* grab the timer for latency tracking purposes */
+	latency_actual = rCNT;
+
+	/* copy interrupt status */
+	status = rSR;
+
+	/* ack the interrupts we just read */
+	rSR = ~status;
+
+#ifdef CONFIG_HRT_PPM
+
+	/* was this a PPM edge? */
+	if (status & (SR_INT_PPM | SR_OVF_PPM)) {
+		/* if required, flip edge sensitivity */
+# ifdef PPM_EDGE_FLIP
+		rCCER ^= CCER_PPM_FLIP;
+# endif
+
+		hrt_ppm_decode(status);
+	}
+#endif
+
+	/* was this a timer tick? */
+	if (status & SR_INT_HRT) {
+
+		/* do latency calculations */
+		hrt_latency_update();
+
+		/* run any callouts that have met their deadline */
+		hrt_call_invoke();
+
+		/* and schedule the next interrupt */
+		hrt_call_reschedule();
+	}
+
+	return OK;
+}
+
+/*
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime	abstime;
+	uint32_t	count;
+	irqstate_t	flags;
+
+	/*
+	 * Counter state.  Marked volatile as they may change
+	 * inside this routine but outside the irqsave/restore
+	 * pair.  Discourage the compiler from moving loads/stores
+	 * to these outside of the protected range.
+	 */
+	static volatile hrt_abstime base_time;
+	static volatile uint32_t last_count;
+
+	/* prevent re-entry */
+	flags = irqsave();
+
+	/* get the current counter value */
+	count = rCNT;
+
+	/*
+	 * Determine whether the counter has wrapped since the
+	 * last time we're called.
+	 *
+	 * This simple test is sufficient due to the guarantee that
+	 * we are always called at least once per counter period.
+	 */
+	if (count < last_count)
+		base_time += HRT_COUNTER_PERIOD;
+
+	/* save the count for next time */
+	last_count = count;
+
+	/* compute the current time */
+	abstime = HRT_COUNTER_SCALE(base_time + count);
+
+	irqrestore(flags);
+
+	return abstime;
+}
+
+/*
+ * Convert a timespec to absolute time
+ */
+hrt_abstime
+ts_to_abstime(struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/*
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/*
+ * Compare a time value with the current time.
+ */
+hrt_abstime
+hrt_elapsed_time(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	irqrestore(flags);
+
+	return delta;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion
+ */
+hrt_abstime
+hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime ts = hrt_absolute_time();
+
+	irqrestore(flags);
+
+	return ts;
+}
+
+/*
+ * Initalise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	sq_init(&callout_queue);
+	hrt_tim_init();
+
+#ifdef CONFIG_HRT_PPM
+	/* configure the PPM input pin */
+	stm32_configgpio(GPIO_PPM_IN);
+#endif
+}
+
+/*
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+/*
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	irqstate_t flags = irqsave();
+
+	/* if the entry is currently queued, remove it */
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+
+	irqrestore(flags);
+}
+
+/*
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	irqstate_t flags = irqsave();
+
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+
+	irqrestore(flags);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+static void
+hrt_call_invoke(void)
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	while (true) {
+		/* get the current time */
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == NULL)
+			break;
+
+		if (call->deadline > now)
+			break;
+
+		sq_rem(&call->link, &callout_queue);
+		//lldbg("call pop\n");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline;
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* invoke the callout (if there is one) */
+		if (call->callout) {
+			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			call->callout(call->arg);
+		}
+
+		/* if the callout has a non-zero period, it has to be re-entered */
+		if (call->period != 0) {
+			call->deadline = deadline + call->period;
+			hrt_call_enter(call);
+		}
+	}
+}
+
+/*
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+
+	//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
+
+	/* set the new compare value and remember it for latency tracking */
+	rCCR_HRT = latency_baseline = deadline & 0xffff;
+}
+
+static void
+hrt_latency_update(void)
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	/* bounded buckets */
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	/* catch-all at the end */
+	latency_counters[index]++;
+}
+
+
+#endif /* CONFIG_HRT_TIMER */
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
new file mode 100644
index 000000000..c1efb8515
--- /dev/null
+++ b/src/drivers/stm32/drv_pwm_servo.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 drv_pwm_servo.c
+ *
+ * Servo driver supporting PWM servos connected to STM32 timer blocks.
+ *
+ * Works with any of the 'generic' or 'advanced' STM32 timers that
+ * have output pins, does not require an interrupt.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "drv_pwm_servo.h"
+
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#define REG(_tmr, _reg)	(*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
+
+#define rCR1(_tmr)    	REG(_tmr, STM32_GTIM_CR1_OFFSET)
+#define rCR2(_tmr)    	REG(_tmr, STM32_GTIM_CR2_OFFSET)
+#define rSMCR(_tmr)   	REG(_tmr, STM32_GTIM_SMCR_OFFSET)
+#define rDIER(_tmr)   	REG(_tmr, STM32_GTIM_DIER_OFFSET)
+#define rSR(_tmr)     	REG(_tmr, STM32_GTIM_SR_OFFSET)
+#define rEGR(_tmr)    	REG(_tmr, STM32_GTIM_EGR_OFFSET)
+#define rCCMR1(_tmr)  	REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2(_tmr)  	REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
+#define rCCER(_tmr)   	REG(_tmr, STM32_GTIM_CCER_OFFSET)
+#define rCNT(_tmr)    	REG(_tmr, STM32_GTIM_CNT_OFFSET)
+#define rPSC(_tmr)    	REG(_tmr, STM32_GTIM_PSC_OFFSET)
+#define rARR(_tmr)    	REG(_tmr, STM32_GTIM_ARR_OFFSET)
+#define rCCR1(_tmr)   	REG(_tmr, STM32_GTIM_CCR1_OFFSET)
+#define rCCR2(_tmr)   	REG(_tmr, STM32_GTIM_CCR2_OFFSET)
+#define rCCR3(_tmr)   	REG(_tmr, STM32_GTIM_CCR3_OFFSET)
+#define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
+#define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
+#define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+
+static void		pwm_timer_init(unsigned timer);
+static void		pwm_timer_set_rate(unsigned timer, unsigned rate);
+static void		pwm_channel_init(unsigned channel);
+
+static void
+pwm_timer_init(unsigned timer)
+{
+	/* enable the timer clock before we try to talk to it */
+	modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit);
+
+	/* disable and configure the timer */
+	rCR1(timer) = 0;
+	rCR2(timer) = 0;
+	rSMCR(timer) = 0;
+	rDIER(timer) = 0;
+	rCCER(timer) = 0;
+	rCCMR1(timer) = 0;
+	rCCMR2(timer) = 0;
+	rCCER(timer) = 0;
+	rDCR(timer) = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
+
+	/* default to updating at 50Hz */
+	pwm_timer_set_rate(timer, 50);
+
+	/* note that the timer is left disabled - arming is performed separately */
+}
+
+static void
+pwm_timer_set_rate(unsigned timer, unsigned rate)
+{
+	/* configure the timer to update at the desired rate */
+	rARR(timer) = 1000000 / rate;
+
+	/* generate an update event; reloads the counter and all registers */
+	rEGR(timer) = GTIM_EGR_UG;
+}
+
+static void
+pwm_channel_init(unsigned channel)
+{
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* configure the GPIO first */
+	stm32_configgpio(pwm_channels[channel].gpio);
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
+		rCCR1(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC1E;
+		break;
+
+	case 2:
+		rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
+		rCCR2(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC2E;
+		break;
+
+	case 3:
+		rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
+		rCCR3(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC3E;
+		break;
+
+	case 4:
+		rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
+		rCCR4(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC4E;
+		break;
+	}
+}
+
+int
+up_pwm_servo_set(unsigned channel, servo_position_t value)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS)
+		return -1;
+
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].gpio == 0))
+		return -1;
+
+	/* configure the channel */
+	if (value > 0)
+		value--;
+
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		rCCR1(timer) = value;
+		break;
+
+	case 2:
+		rCCR2(timer) = value;
+		break;
+
+	case 3:
+		rCCR3(timer) = value;
+		break;
+
+	case 4:
+		rCCR4(timer) = value;
+		break;
+
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+servo_position_t
+up_pwm_servo_get(unsigned channel)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS)
+		return 0;
+
+	unsigned timer = pwm_channels[channel].timer_index;
+	servo_position_t value = 0;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].timer_channel == 0))
+		return 0;
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		value = rCCR1(timer);
+		break;
+
+	case 2:
+		value = rCCR2(timer);
+		break;
+
+	case 3:
+		value = rCCR3(timer);
+		break;
+
+	case 4:
+		value = rCCR4(timer);
+		break;
+	}
+
+	return value + 1;
+}
+
+int
+up_pwm_servo_init(uint32_t channel_mask)
+{
+	/* do basic timer initialisation first */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			pwm_timer_init(i);
+	}
+
+	/* now init channels */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		/* don't do init for disabled channels; this leaves the pin configs alone */
+		if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0))
+			pwm_channel_init(i);
+	}
+
+	return OK;
+}
+
+void
+up_pwm_servo_deinit(void)
+{
+	/* disable the timers */
+	up_pwm_servo_arm(false);
+}
+
+int
+up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
+{
+	/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
+	if (rate < 1)
+		return -ERANGE;
+	if (rate > 10000)
+		return -ERANGE;
+
+	if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
+		return ERROR;
+
+	pwm_timer_set_rate(group, rate);
+
+	return OK;
+}
+
+int
+up_pwm_servo_set_rate(unsigned rate)
+{
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
+		up_pwm_servo_set_rate_group_update(i, rate);
+
+	return 0;
+}
+
+uint32_t
+up_pwm_servo_get_rate_group(unsigned group)
+{
+	unsigned channels = 0;
+
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group))
+			channels |= (1 << i);
+	}
+	return channels;
+}
+
+void
+up_pwm_servo_arm(bool armed)
+{
+	/* iterate timers and arm/disarm appropriately */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0) {
+			if (armed) {
+				/* force an update to preload all registers */
+				rEGR(i) = GTIM_EGR_UG;
+
+				/* arm requires the timer be enabled */
+				rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
+
+			} else {
+				// XXX This leads to FMU PWM being still active
+				// but uncontrollable. Just disable the timer
+				// and risk a runt.
+				///* on disarm, just stop auto-reload so we don't generate runts */
+				//rCR1(i) &= ~GTIM_CR1_ARPE;
+				rCR1(i) = 0;
+			}
+		}
+	}
+}
diff --git a/src/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h
new file mode 100644
index 000000000..5dd4cf70c
--- /dev/null
+++ b/src/drivers/stm32/drv_pwm_servo.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   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_pwm_servo.h
+ *
+ * stm32-specific PWM output data.
+ */
+
+#pragma once
+
+#include <drivers/drv_pwm_output.h>
+
+/* configuration limits */
+#define PWM_SERVO_MAX_TIMERS	4
+#define PWM_SERVO_MAX_CHANNELS	8
+
+/* array of timers dedicated to PWM servo use */
+struct pwm_servo_timer {
+	uint32_t	base;
+	uint32_t	clock_register;
+	uint32_t	clock_bit;
+	uint32_t	clock_freq;
+};
+
+/* array of channels in logical order */
+struct pwm_servo_channel {
+	uint32_t	gpio;
+	uint8_t		timer_index;
+	uint8_t		timer_channel;
+	servo_position_t default_value;
+};
+
+/* supplied by board-specific code */
+__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
+__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];
diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk
new file mode 100644
index 000000000..bb751c7f6
--- /dev/null
+++ b/src/drivers/stm32/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# STM32 driver support code
+#
+# Modules in this directory are compiled for all STM32 targets.
+#
+
+SRCS		= drv_hrt.c \
+		  drv_pwm_servo.c
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
new file mode 100644
index 000000000..827cf30b2
--- /dev/null
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Tone alarm driver
+#
+
+MODULE_COMMAND	= tone_alarm
+
+SRCS		= tone_alarm.cpp
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
new file mode 100644
index 000000000..ac5511e60
--- /dev/null
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -0,0 +1,1018 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Driver for the PX4 audio alarm port, /dev/tone_alarm.
+ *
+ * The tone_alarm driver supports a set of predefined "alarm"
+ * tunes and one user-supplied tune.
+ *
+ * The TONE_SET_ALARM ioctl can be used to select a predefined
+ * alarm tune, from 1 - <TBD>.  Selecting tune zero silences
+ * the alarm.
+ *
+ * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY
+ * statement, with some exceptions and extensions.
+ *
+ * From Wikibooks:
+ *
+ * PLAY "[string expression]"
+ * 
+ * Used to play notes and a score ... The tones are indicated by letters A through G.
+ * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) 
+ * immediately after the note letter. See this example:
+ * 
+ *   PLAY "C C# C C#"
+ *
+ * Whitespaces are ignored inside the string expression. There are also codes that
+ * set the duration, octave and tempo. They are all case-insensitive. PLAY executes 
+ * the commands or notes the order in which they appear in the string. Any indicators 
+ * that change the properties are effective for the notes following that indicator.
+ *
+ * Ln     Sets the duration (length) of the notes. The variable n does not indicate an actual duration
+ *        amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc.
+ *        (L8, L16, L32, L64, ...). By default, n = 4.
+ *        For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively.
+ *        The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB"
+ *        can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes.
+ * On     Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B.
+ *        Remember that C- is equivalent to B. 
+ * < >    Changes the current octave respectively down or up one level.
+ * Nn     Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.)
+ *        Cannot use with sharp and flat. Cannot use with the shorthand notation neither.
+ * MN     Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode.
+ * ML     Stand for Music Legato. Note duration is full length of that indicated by Ln.
+ * MS     Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln.
+ * Pn     Causes a silence (pause) for the length of note indicated (same as Ln). 
+ * Tn     Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. 
+ * .      When placed after a note, it causes the duration of the note to be 3/2 of the set duration.
+ *        This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note.
+ *        It can be used for a pause as well.
+ *
+ * Extensions/variations:
+ *
+ * MB MF  The MF command causes the tune to play once and then stop. The MB command causes the
+ *        tune to repeat when it ends.
+ *
+ */
+
+#include <nuttx/config.h>
+#include <debug.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <math.h>
+#include <ctype.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include <arch/stm32/chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <systemlib/err.h>
+
+#ifndef CONFIG_HRT_TIMER
+# error This driver requires CONFIG_HRT_TIMER
+#endif
+
+/* Tone alarm configuration */
+#if   TONE_ALARM_TIMER == 2
+# define TONE_ALARM_BASE		STM32_TIM2_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM2_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM2EN
+# ifdef CONFIG_STM32_TIM2
+#  error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2
+# endif
+#elif TONE_ALARM_TIMER == 3
+# define TONE_ALARM_BASE		STM32_TIM3_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM3_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM3EN
+# ifdef CONFIG_STM32_TIM3
+#  error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3
+# endif
+#elif TONE_ALARM_TIMER == 4
+# define TONE_ALARM_BASE		STM32_TIM4_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM4_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM4EN
+# ifdef CONFIG_STM32_TIM4
+#  error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4
+# endif
+#elif TONE_ALARM_TIMER == 5
+# define TONE_ALARM_BASE		STM32_TIM5_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM5_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM5EN
+# ifdef CONFIG_STM32_TIM5
+#  error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
+# endif
+#elif TONE_ALARM_TIMER == 9
+# define TONE_ALARM_BASE		STM32_TIM9_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM9_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM9EN
+# ifdef CONFIG_STM32_TIM9
+#  error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9
+# endif
+#elif TONE_ALARM_TIMER == 10
+# define TONE_ALARM_BASE		STM32_TIM10_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM10_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM10EN
+# ifdef CONFIG_STM32_TIM10
+#  error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10
+# endif
+#elif TONE_ALARM_TIMER == 11
+# define TONE_ALARM_BASE		STM32_TIM11_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM11_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM11EN
+# ifdef CONFIG_STM32_TIM11
+#  error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
+# endif
+#else
+# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver.
+#endif
+
+#if TONE_ALARM_CHANNEL == 1
+# define TONE_CCMR1	(3 << 4)
+# define TONE_CCMR2	0
+# define TONE_CCER	(1 << 0)
+# define TONE_rCCR	rCCR1
+#elif TONE_ALARM_CHANNEL == 2
+# define TONE_CCMR1	(3 << 12)
+# define TONE_CCMR2	0
+# define TONE_CCER	(1 << 4)
+# define TONE_rCCR	rCCR2
+#elif TONE_ALARM_CHANNEL == 3
+# define TONE_CCMR1	0
+# define TONE_CCMR2	(3 << 4)
+# define TONE_CCER	(1 << 8)
+# define TONE_rCCR	rCCR3
+#elif TONE_ALARM_CHANNEL == 4
+# define TONE_CCMR1	0
+# define TONE_CCMR2	(3 << 12)
+# define TONE_CCER	(1 << 12)
+# define TONE_rCCR	rCCR4
+#else
+# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver.
+#endif
+
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+class ToneAlarm : public device::CDev
+{
+public:
+	ToneAlarm();
+	~ToneAlarm();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
+
+private:
+	static const unsigned	_tune_max = 1024; // be reasonable about user tunes
+	static const char	* const _default_tunes[];
+	static const unsigned	_default_ntunes;
+	static const uint8_t	_note_tab[];
+
+	const char		*_user_tune;
+
+	const char		*_tune;		// current tune string
+	const char		*_next;		// next note in the string
+
+	unsigned		_tempo;
+	unsigned		_note_length;
+	enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode;
+	unsigned		_octave;
+	unsigned		_silence_length; // if nonzero, silence before next note
+	bool			_repeat;	// if true, tune restarts at end
+
+	hrt_call		_note_call;	// HRT callout for note completion
+
+	// Convert a note value in the range C1 to B7 into a divisor for
+	// the configured timer's clock.
+	//
+	unsigned		note_to_divisor(unsigned note);
+
+	// Calculate the duration in microseconds of play and silence for a 
+	// note given the current tempo, length and mode and the number of 
+	// dots following in the play string.
+	//
+	unsigned		note_duration(unsigned &silence, unsigned note_length, unsigned dots);
+
+	// Calculate the duration in microseconds of a rest corresponding to
+	// a given note length.
+	//
+	unsigned		rest_duration(unsigned rest_length, unsigned dots);
+
+	// Start playing the note
+	//
+	void			start_note(unsigned note);
+
+	// Stop playing the current note and make the player 'safe'
+	//
+	void			stop_note();
+
+	// Start playing the tune
+	//
+	void			start_tune(const char *tune);
+
+	// Parse the next note out of the string and play it
+	//
+	void			next_note();
+
+	// Find the next character in the string, discard any whitespace and
+	// return the canonical (uppercase) version.
+	//
+	int			next_char();
+
+	// Extract a number from the string, consuming all the digit characters.
+	//
+	unsigned		next_number();
+
+	// Consume dot characters from the string, returning the number consumed.
+	//
+	unsigned		next_dots();
+
+	// hrt_call trampoline for next_note
+	//
+	static void		next_trampoline(void *arg);
+
+};
+
+// predefined tune array
+const char * const ToneAlarm::_default_tunes[] = {
+	"MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc",		// startup tune
+	"MBT200a8a8a8PaaaP",					// ERROR tone
+	"MFT200e8a8a",						// NotifyPositive tone
+	"MFT200e8e",						// NotifyNeutral tone
+	"MFT200e8c8e8c8e8c8",					// NotifyNegative tone
+	"MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge!
+	"MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16",	// dixie
+	"MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8",		// cucuracha
+	"MNT150L8O2GGABGBADGGABL4GL8F+",			// yankee
+	"MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy
+	"T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8"	// william tell
+	"O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16"
+	"O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16"
+	"O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8"
+	"P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8"
+	"O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16"
+	"O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4"
+	"O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8"
+	"O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16"
+	"O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8"
+	"O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16"
+	"O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8"
+	"O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8"
+	"O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
+	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
+	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
+	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
+	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16"
+	"O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
+	"O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8"
+	"O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16"
+	"O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16"
+	"O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16"
+	"O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16"
+	"O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8"
+	"O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8"
+	"O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16"
+	"O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64"
+	"O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16"
+	"O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16"
+	"O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16"
+	"O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16"
+	"O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16"
+	"O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
+	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
+	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
+	"O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16"
+	"O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16"
+	"O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16"
+	"O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16"
+	"O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16"
+	"O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16"
+	"O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16"
+	"O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16"
+	"O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16"
+	"O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16"
+	"O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16"
+	"O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16"
+	"O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16"
+	"O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16"
+	"O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16"
+	"O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16"
+	"O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16"
+	"O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16"
+	"O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16"
+	"O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16"
+	"O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16"
+	"O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16"
+	"O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16"
+	"O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16"
+	"O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
+	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
+	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
+	"O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16"
+	"O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4."
+	"O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16"
+	"O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16"
+	"O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16"
+	"O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16"
+	"O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8"
+	"O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16"
+	"O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
+	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
+	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
+	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
+	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8"
+	"O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
+	"O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8"
+	"O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8"
+	"O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16"
+	"O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16"
+	"O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8"
+	"O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8"
+	"O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16"
+	"O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16"
+	"O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16"
+	"O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16"
+	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
+	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16"
+	"O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16"
+	"O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16"
+	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
+	"O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16"
+	"O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16"
+	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
+	"P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16"
+	"O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8"
+	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16"
+	"O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16"
+	"O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16"
+	"O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16"
+	"O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16"
+	"P16O4E16P16D+16P16C+16P16O3B16P16A+16P16"
+	"O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16"
+	"O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16"
+	"O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16"
+	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
+	"O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16"
+	"O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16"
+	"O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
+	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
+	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
+	"P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16"
+	"O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16"
+	"O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16"
+	"O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16"
+	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8"
+	"O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16"
+	"O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16"
+	"O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8"
+	"O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16"
+	"O4E16P16D+16P16C+16P16O3B16P16A+16P16A16"
+	"P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16"
+	"O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8"
+	"O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8"
+	"O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16"
+	"O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16"
+	"O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16"
+	"O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16"
+	"O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16"
+	"O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8"
+	"O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16"
+	"O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16"
+	"O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8"
+	"O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8"
+	"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
+	"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
+	"O2E2P64",
+};
+
+const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
+
+// semitone offsets from C for the characters 'A'-'G'
+const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
+
+
+ToneAlarm::ToneAlarm() :
+	CDev("tone_alarm", "/dev/tone_alarm"),
+	_user_tune(nullptr),
+	_tune(nullptr),
+	_next(nullptr)
+{
+	// enable debug() calls
+	//_debug_enabled = true;
+}
+
+ToneAlarm::~ToneAlarm()
+{
+}
+
+int
+ToneAlarm::init()
+{
+	int ret;
+
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	/* configure the GPIO to the idle state */
+	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
+
+	/* clock/power on our timer */
+	modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
+
+	/* initialise the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = 0;
+	rCCER &= TONE_CCER;		/* unlock CCMR* registers */
+	rCCMR1 = TONE_CCMR1;
+	rCCMR2 = TONE_CCMR2;
+	rCCER = TONE_CCER;
+	rDCR = 0;
+
+	/* toggle the CC output each time the count passes 1 */
+	TONE_rCCR = 1;
+
+	/* default the timer to a prescale value of 1; playing notes will change this */
+	rPSC = 0;
+
+	/* make sure the timer is running */
+	rCR1 = GTIM_CR1_CEN;
+
+	debug("ready");
+	return OK;
+}
+
+unsigned
+ToneAlarm::note_to_divisor(unsigned note)
+{
+	// compute the frequency first (Hz)
+	float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f);
+
+	float period = 0.5f / freq;
+
+	// and the divisor, rounded to the nearest integer
+	unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f;
+
+	return divisor;
+}
+
+unsigned
+ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots)
+{
+	unsigned whole_note_period = (60 * 1000000 * 4) / _tempo;
+
+	if (note_length == 0)
+		note_length = 1;
+	unsigned note_period = whole_note_period / note_length;
+
+	switch (_note_mode) {
+	case MODE_NORMAL:
+		silence = note_period / 8;
+		break;
+	case MODE_STACCATO:
+		silence = note_period / 4;
+		break;
+	default:
+	case MODE_LEGATO:
+		silence = 0;
+		break;
+	}
+	note_period -= silence;
+
+	unsigned dot_extension = note_period / 2;
+	while (dots--) {
+		note_period += dot_extension;
+		dot_extension /= 2;
+	}
+
+	return note_period;
+}
+
+unsigned
+ToneAlarm::rest_duration(unsigned rest_length, unsigned dots)
+{
+	unsigned whole_note_period = (60 * 1000000 * 4) / _tempo;
+
+	if (rest_length == 0)
+		rest_length = 1;
+
+	unsigned rest_period = whole_note_period / rest_length;
+
+	unsigned dot_extension = rest_period / 2;
+	while (dots--) {
+		rest_period += dot_extension;
+		dot_extension /= 2;
+	}
+
+	return rest_period;
+}
+
+void
+ToneAlarm::start_note(unsigned note)
+{
+	// compute the divisor
+	unsigned divisor = note_to_divisor(note);
+
+	// pick the lowest prescaler value that we can use
+	// (note that the effective prescale value is 1 greater)
+	unsigned prescale = divisor / 65536;
+
+	// calculate the timer period for the selected prescaler value
+	unsigned period = (divisor / (prescale + 1)) - 1;
+
+	rPSC = prescale;	// load new prescaler
+	rARR = period;		// load new toggle period
+	rEGR = GTIM_EGR_UG;	// force a reload of the period
+	rCCER |= TONE_CCER;	// enable the output
+
+	// configure the GPIO to enable timer output
+	stm32_configgpio(GPIO_TONE_ALARM);
+}
+
+void
+ToneAlarm::stop_note()
+{
+	/* stop the current note */
+	rCCER &= ~TONE_CCER;
+
+	/*
+	 * Make sure the GPIO is not driving the speaker.
+	 */
+	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
+}
+
+void
+ToneAlarm::start_tune(const char *tune)
+{
+	// kill any current playback
+	hrt_cancel(&_note_call);
+
+	// record the tune
+	_tune = tune;
+	_next = tune;
+
+	// initialise player state
+	_tempo = 120;
+	_note_length = 4;
+	_note_mode = MODE_NORMAL;
+	_octave = 4;
+	_silence_length = 0;
+	_repeat = false;		// otherwise command-line tunes repeat forever...
+
+	// schedule a callback to start playing
+	hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this);
+}
+
+void
+ToneAlarm::next_note()
+{
+	// do we have an inter-note gap to wait for?
+	if (_silence_length > 0) {
+		stop_note();
+		hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this);
+		_silence_length = 0;
+		return;
+	}
+
+	// make sure we still have a tune - may be removed by the write / ioctl handler
+	if ((_next == nullptr) || (_tune == nullptr)) {
+		stop_note();
+		return;
+	}
+
+	// parse characters out of the string until we have resolved a note
+	unsigned note = 0;
+	unsigned note_length = _note_length;
+	unsigned duration;
+
+	while (note == 0) {
+		// we always need at least one character from the string
+		int c = next_char();
+		if (c == 0)
+			goto tune_end;
+		_next++;
+
+		switch (c) {
+		case 'L':	// select note length
+			_note_length = next_number();
+			if (_note_length < 1)
+				goto tune_error;
+			break;
+
+		case 'O':	// select octave
+			_octave = next_number();
+			if (_octave > 6)
+				_octave = 6;
+			break;
+
+		case '<':	// decrease octave
+			if (_octave > 0)
+				_octave--;
+			break;
+
+		case '>':	// increase octave
+			if (_octave < 6)
+				_octave++;
+			break;
+
+		case 'M':	// select inter-note gap
+			c = next_char();
+			if (c == 0)
+				goto tune_error;
+			_next++;
+			switch (c) {
+			case 'N':
+				_note_mode = MODE_NORMAL;
+				break;
+			case 'L':
+				_note_mode = MODE_LEGATO;
+				break;
+			case 'S':
+				_note_mode = MODE_STACCATO;
+				break;
+			case 'F':
+				_repeat = false;
+				break;
+			case 'B':
+				_repeat = true;
+				break;
+			default:
+				goto tune_error;
+			}
+			break;
+
+		case 'P':	// pause for a note length
+			stop_note();
+			hrt_call_after(&_note_call, 
+				(hrt_abstime)rest_duration(next_number(), next_dots()),
+				(hrt_callout)next_trampoline, 
+				this);
+			return;
+
+		case 'T': {	// change tempo
+			unsigned nt = next_number();
+
+			if ((nt >= 32) && (nt <= 255)) {
+				_tempo = nt;
+			} else {
+				goto tune_error;
+			}
+			break;
+		}
+
+		case 'N':	// play an arbitrary note
+			note = next_number();
+			if (note > 84)
+				goto tune_error;
+			if (note == 0) {
+				// this is a rest - pause for the current note length
+				hrt_call_after(&_note_call,
+					(hrt_abstime)rest_duration(_note_length, next_dots()),
+					(hrt_callout)next_trampoline, 
+					this);
+				return;				
+			}
+			break;
+
+		case 'A'...'G':	// play a note in the current octave
+			note = _note_tab[c - 'A'] + (_octave * 12) + 1;
+			c = next_char();
+			switch (c) {
+			case '#':	// up a semitone
+			case '+':
+				if (note < 84)
+					note++;
+				_next++;
+				break;
+			case '-':	// down a semitone
+				if (note > 1)
+					note--;
+				_next++;
+				break;
+			default:
+				// 0 / no next char here is OK
+				break;
+			}
+			// shorthand length notation
+			note_length = next_number();
+			if (note_length == 0)
+				note_length = _note_length;
+			break;
+
+		default:
+			goto tune_error;
+		}
+	}
+
+	// compute the duration of the note and the following silence (if any)
+	duration = note_duration(_silence_length, note_length, next_dots());
+
+	// start playing the note
+	start_note(note);
+
+	// and arrange a callback when the note should stop
+	hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this);
+	return;
+
+	// tune looks bad (unexpected EOF, bad character, etc.)
+tune_error:
+	lowsyslog("tune error\n");
+	_repeat = false;		// don't loop on error
+
+	// stop (and potentially restart) the tune
+tune_end:
+	stop_note();
+	if (_repeat)
+		start_tune(_tune);
+	return;
+}
+
+int
+ToneAlarm::next_char()
+{
+	while (isspace(*_next)) {
+		_next++;
+	}
+	return toupper(*_next);
+}
+
+unsigned
+ToneAlarm::next_number()
+{
+	unsigned number = 0;
+	int c;
+
+	for (;;) {
+		c = next_char();
+		if (!isdigit(c))
+			return number;
+		_next++;
+		number = (number * 10) + (c - '0');
+	}
+}
+
+unsigned
+ToneAlarm::next_dots()
+{
+	unsigned dots = 0;
+
+	while (next_char() == '.') {
+		_next++;
+		dots++;
+	}
+	return dots;
+}
+
+void
+ToneAlarm::next_trampoline(void *arg)
+{
+	ToneAlarm *ta = (ToneAlarm *)arg;
+
+	ta->next_note();
+}
+
+
+int
+ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int result = OK;
+
+	debug("ioctl %i %u", cmd, arg);
+
+//	irqstate_t flags = irqsave();
+
+	/* decide whether to increase the alarm level to cmd or leave it alone */
+	switch (cmd) {
+	case TONE_SET_ALARM:
+		debug("TONE_SET_ALARM %u", arg);
+
+		if (arg <= _default_ntunes) {
+			if (arg == 0) {
+				// stop the tune
+				_tune = nullptr;
+				_next = nullptr;
+			} else {
+				// play the selected tune
+				start_tune(_default_tunes[arg - 1]);
+			}
+		} else {
+			result = -EINVAL;
+		}
+
+		break;
+
+	default:
+		result = -ENOTTY;
+		break;
+	}
+
+//	irqrestore(flags);
+
+	/* give it to the superclass if we didn't like it */
+	if (result == -ENOTTY)
+		result = CDev::ioctl(filp, cmd, arg);
+
+	return result;
+}
+
+int
+ToneAlarm::write(file *filp, const char *buffer, size_t len)
+{
+	// sanity-check the buffer for length and nul-termination
+	if (len > _tune_max)
+		return -EFBIG;
+
+	// if we have an existing user tune, free it
+	if (_user_tune != nullptr) {
+
+		// if we are playing the user tune, stop
+		if (_tune == _user_tune) {
+			_tune = nullptr;
+			_next = nullptr;
+		}
+
+		// free the old user tune
+		free((void *)_user_tune);
+		_user_tune = nullptr;
+	}
+
+	// if the new tune is empty, we're done
+	if (buffer[0] == '\0')
+		return OK;
+
+	// allocate a copy of the new tune
+	_user_tune = strndup(buffer, len);
+	if (_user_tune == nullptr)
+		return -ENOMEM;
+
+	// and play it
+	start_tune(_user_tune);
+
+	return len;
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace
+{
+
+ToneAlarm	*g_dev;
+
+int
+play_tune(unsigned tune)
+{
+	int	fd, ret;
+
+	fd = open("/dev/tone_alarm", 0);
+
+	if (fd < 0)
+		err(1, "/dev/tone_alarm");
+
+	ret = ioctl(fd, TONE_SET_ALARM, tune);
+	close(fd);
+
+	if (ret != 0)
+		err(1, "TONE_SET_ALARM");
+
+	exit(0);
+}
+
+int
+play_string(const char *str)
+{
+	int	fd, ret;
+
+	fd = open("/dev/tone_alarm", O_WRONLY);
+
+	if (fd < 0)
+		err(1, "/dev/tone_alarm");
+
+	ret = write(fd, str, strlen(str) + 1);
+	close(fd);
+
+	if (ret < 0)
+		err(1, "play tune");
+	exit(0);
+}
+
+} // namespace
+
+int
+tone_alarm_main(int argc, char *argv[])
+{
+	unsigned tune;
+
+	/* start the driver lazily */
+	if (g_dev == nullptr) {
+		g_dev = new ToneAlarm;
+
+		if (g_dev == nullptr)
+			errx(1, "couldn't allocate the ToneAlarm driver");
+
+		if (g_dev->init() != OK) {
+			delete g_dev;
+			errx(1, "ToneAlarm init failed");
+		}
+	}
+
+	if ((argc > 1) && !strcmp(argv[1], "start"))
+		play_tune(1);
+
+	if ((argc > 1) && !strcmp(argv[1], "stop"))
+		play_tune(0);
+
+	if ((tune = strtol(argv[1], nullptr, 10)) != 0)
+		play_tune(tune);
+
+	/* if it looks like a PLAY string... */
+	if (strlen(argv[1]) > 2) {
+		const char *str = argv[1];
+		if (str[0] == 'M') {
+			play_string(str);
+		}
+	}
+
+	errx(1, "unrecognised command, try 'start', 'stop' or an alarm number");
+}