Rename the 'device' directory back to 'drivers', it's less confusing that way.

Move the fmuv2 board driver out into the new world.

13 files changed, 3887 insertions, 0 deletions

diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk
new file mode 100644
index 000000000..eb8841e4d
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/module.mk
@@ -0,0 +1,9 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmuv2/px4fmu_can.c
new file mode 100644
index 000000000..86ba183b8
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/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
\ No newline at end of file
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c
new file mode 100644
index 000000000..1d99f15bf
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ *   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_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
+
+/****************************************************************************
+ * 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 */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+
+#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)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);
+	stm32_configgpio(GPIO_ADC1_IN11);
+	stm32_configgpio(GPIO_ADC1_IN12);
+
+	/* configure power supply control pins */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN);
+	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+	/* 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
+	// XXX need to make this work again
+//	drv_led_start();
+	up_ledoff(LED_AMBER);
+
+	/* 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_MAG, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\r\n");
+
+	/* Get the SPI port for the FRAM */
+
+	message("[boot] Initializing SPI port 2\n");
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully initialized SPI port 2\n");
+
+	/* XXX need a driver to bind the FRAM to */
+
+	//message("[boot] Successfully bound SPI port 2 to the FRAM driver\n");
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
new file mode 100644
index 000000000..cc4e9529d
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 ************************************************************************************/
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_2V8_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* 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)
+
+/****************************************************************************************************
+ * 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/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..14e4052be
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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_TIM1_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_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_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c
new file mode 100644
index 000000000..f90f25071
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_spi.c
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   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)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__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_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(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_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c
new file mode 100644
index 000000000..b0b669fbe
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/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/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 000000000..32030a1f7
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,1290 @@
+/****************************************************************************
+ *
+ *   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 lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer 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_accel.h>
+#include <drivers/drv_mag.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: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM				0x49
+
+#define ADDR_OUT_L_T         	0x05
+#define ADDR_OUT_H_T        	0x06
+#define ADDR_STATUS_M           0x07
+#define ADDR_OUT_X_L_M          0x08
+#define ADDR_OUT_X_H_M          0x09
+#define ADDR_OUT_Y_L_M          0x0A
+#define ADDR_OUT_Y_H_M          0x0B
+#define ADDR_OUT_Z_L_M          0x0C
+#define ADDR_OUT_Z_H_M          0x0D
+
+#define ADDR_OUT_TEMP_A			0x26
+#define ADDR_STATUS_A			0x27
+#define ADDR_OUT_X_L_A			0x28
+#define ADDR_OUT_X_H_A			0x29
+#define ADDR_OUT_Y_L_A			0x2A
+#define ADDR_OUT_Y_H_A			0x2B
+#define ADDR_OUT_Z_L_A			0x2C
+#define ADDR_OUT_Z_H_A			0x2D
+
+#define ADDR_CTRL_REG0			0x1F
+#define ADDR_CTRL_REG1			0x20
+#define ADDR_CTRL_REG2			0x21
+#define ADDR_CTRL_REG3			0x22
+#define ADDR_CTRL_REG4			0x23
+#define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG7			0x26
+
+#define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A		((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A		((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A		((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_CONT_UPDATE_A		(0<<3)
+#define REG1_Z_ENABLE_A			(1<<2)
+#define REG1_Y_ENABLE_A			(1<<1)
+#define REG1_X_ENABLE_A			(1<<0)
+
+#define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A	((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A	((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T			(1<<7)
+
+#define REG5_RES_HIGH_M			((1<<6) | (1<<5))
+#define REG5_RES_LOW_M			((0<<6) | (0<<5))
+
+#define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M		((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M		((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_2GA_M	((0<<7) | (0<<6))
+#define REG6_FULL_SCALE_4GA_M	((0<<7) | (1<<6))
+#define REG6_FULL_SCALE_8GA_M	((1<<7) | (0<<6))
+#define REG6_FULL_SCALE_12GA_M	((1<<7) | (1<<6))
+
+#define REG7_CONT_MODE_M		((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M              0x12
+#define INT_SRC_M               0x13
+
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+	LSM303D(int bus, const char* path, spi_dev_e device);
+	virtual ~LSM303D();
+
+	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 LSM303D_mag;
+
+	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+	LSM303D_mag		*_mag;
+
+	struct hrt_call		_accel_call;
+	struct hrt_call		_mag_call;
+
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
+
+	unsigned		_num_accel_reports;
+	volatile unsigned	_next_accel_report;
+	volatile unsigned	_oldest_accel_report;
+	struct accel_report	*_accel_reports;
+
+	struct accel_scale	_accel_scale;
+	float			_accel_range_scale;
+	float			_accel_range_m_s2;
+	orb_advert_t		_accel_topic;
+
+	unsigned		_num_mag_reports;
+	volatile unsigned	_next_mag_report;
+	volatile unsigned	_oldest_mag_report;
+	struct mag_report	*_mag_reports;
+
+	struct mag_scale	_mag_scale;
+	float			_mag_range_scale;
+	float			_mag_range_ga;
+	orb_advert_t		_mag_topic;
+
+	unsigned		_current_accel_rate;
+	unsigned		_current_accel_range;
+
+	unsigned		_current_mag_rate;
+	unsigned		_current_mag_range;
+
+	perf_counter_t		_accel_sample_perf;
+	perf_counter_t		_mag_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);
+
+	/**
+	 * Static trampoline for the mag because it runs at a lower rate
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		mag_measure_trampoline(void *arg);
+
+	/**
+	 * Fetch accel measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Fetch mag measurements from the sensor and update the report ring.
+	 */
+	void			mag_measure();
+
+	/**
+	 * Read a register from the LSM303D
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the LSM303D
+	 *
+	 * @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 LSM303D
+	 *
+	 * 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 LSM303D 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 LSM303D 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 class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+	LSM303D_mag(LSM303D *parent);
+	~LSM303D_mag();
+
+	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 LSM303D;
+
+	void			parent_poll_notify();
+private:
+	LSM303D			*_parent;
+
+	void			measure();
+
+	void			measure_trampoline(void *arg);
+};
+
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+	_mag(new LSM303D_mag(this)),
+	_call_accel_interval(0),
+	_call_mag_interval(0),
+	_num_accel_reports(0),
+	_next_accel_report(0),
+	_oldest_accel_report(0),
+	_accel_reports(nullptr),
+	_accel_range_scale(0.0f),
+	_accel_range_m_s2(0.0f),
+	_accel_topic(-1),
+	_num_mag_reports(0),
+	_next_mag_report(0),
+	_oldest_mag_report(0),
+	_mag_reports(nullptr),
+	_mag_range_scale(0.0f),
+	_mag_range_ga(0.0f),
+	_current_accel_rate(0),
+	_current_accel_range(0),
+	_current_mag_rate(0),
+	_current_mag_range(0),
+	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	/* XXX fix this default values */
+	_accel_range_scale = 1.0f;
+	_mag_range_scale = 1.0f;
+
+	// 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;
+
+	_mag_scale.x_offset = 0;
+	_mag_scale.x_scale = 1.0f;
+	_mag_scale.y_offset = 0;
+	_mag_scale.y_scale = 1.0f;
+	_mag_scale.z_offset = 0;
+	_mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete[] _accel_reports;
+	if (_mag_reports != nullptr)
+		delete[] _mag_reports;
+
+	delete _mag;
+
+	/* delete the perf counter */
+	perf_free(_accel_sample_perf);
+	perf_free(_mag_sample_perf);
+}
+
+int
+LSM303D::init()
+{
+	int ret = ERROR;
+	int mag_ret;
+	int fd_mag;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_accel_reports = 2;
+	_oldest_accel_report = _next_accel_report = 0;
+	_accel_reports = new struct accel_report[_num_accel_reports];
+
+	if (_accel_reports == nullptr)
+		goto out;
+
+	/* advertise accel topic */
+	memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+
+	_num_mag_reports = 2;
+	_oldest_mag_report = _next_mag_report = 0;
+	_mag_reports = new struct mag_report[_num_mag_reports];
+
+	if (_mag_reports == nullptr)
+		goto out;
+
+	/* advertise mag topic */
+	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+
+	/* XXX do this with ioctls */
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M);
+
+	/* XXX should we enable FIFO */
+
+	set_range(500);				/* default to 500dps */
+	set_samplerate(0);			/* max sample rate */
+
+//	_current_accel_rate = 100;
+
+	/* XXX test this when another mag is used */
+	/* do CDev init for the mag device node, keep it optional */
+	mag_ret = _mag->init();
+
+	if (mag_ret != OK) {
+		_mag_topic = -1;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+LSM303D::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
+LSM303D::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_accel_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_accel_report != _next_accel_report) {
+				memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
+				ret += sizeof(_accel_reports[0]);
+				INCREMENT(_oldest_accel_report, _num_accel_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_accel_report = _next_accel_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
+	ret = sizeof(*_accel_reports);
+
+	return ret;
+}
+
+ssize_t
+LSM303D::mag_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 (_call_mag_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_mag_report != _next_mag_report) {
+				memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
+				ret += sizeof(_mag_reports[0]);
+				INCREMENT(_oldest_mag_report, _num_mag_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_mag_report = _next_mag_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
+	ret = sizeof(*_mag_reports);
+
+	return ret;
+}
+
+int
+LSM303D::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_accel_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_accel_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... */
+					_accel_call.period = _call_accel_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_accel_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_accel_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[] _accel_reports;
+			_num_accel_reports = arg;
+			_accel_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_accel_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::mag_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_mag_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:
+				/* 50 Hz is max for mag */
+				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50);
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_mag_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... */
+					_mag_call.period = _call_mag_interval = ticks;
+
+
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_mag_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_mag_interval;
+	case SENSORIOCSQUEUEDEPTH:
+	case SENSORIOCGQUEUEDEPTH:
+	case SENSORIOCRESET:
+		return ioctl(filp, cmd, arg);
+
+	case MAGIOCSSAMPLERATE:
+//	case MAGIOCGSAMPLERATE:
+		/* XXX not implemented */
+		return -EINVAL;
+
+	case MAGIOCSLOWPASS:
+//	case MAGIOCGLOWPASS:
+		/* XXX not implemented */
+//		_set_dlpf_filter((uint16_t)arg);
+		return -EINVAL;
+
+	case MAGIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCSRANGE:
+//	case MAGIOCGRANGE:
+		/* XXX not implemented */
+		// XXX change these two values on set:
+		// _mag_range_scale = xx
+		// _mag_range_ga = xx
+		return -EINVAL;
+
+	case MAGIOCSELFTEST:
+		/* XXX not implemented */
+//		return self_test();
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+LSM303D::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
+LSM303D::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
+LSM303D::set_range(unsigned max_dps)
+{
+	/* XXX implement this */
+//	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
+LSM303D::set_samplerate(unsigned frequency)
+{
+	/* XXX implement this */
+//	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
+LSM303D::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_accel_report = _next_accel_report = 0;
+	_oldest_mag_report = _next_mag_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+	hrt_cancel(&_accel_call);
+	hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+	/* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_accel_report;
+#pragma pack(pop)
+
+	accel_report		*accel_report = &_accel_reports[_next_accel_report];
+
+	/* start the performance counter */
+	perf_begin(_accel_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+	/* XXX adapt the comment to specs */
+	/*
+	 * 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.
+	 */
+
+
+	accel_report->timestamp = hrt_absolute_time();
+	/* XXX adjust for sensor alignment to board here */
+	accel_report->x_raw = raw_accel_report.x;
+	accel_report->y_raw = raw_accel_report.y;
+	accel_report->z_raw = raw_accel_report.z;
+
+	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_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_accel_report, _num_accel_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_accel_report == _oldest_accel_report)
+		INCREMENT(_oldest_accel_report, _num_accel_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+
+	/* stop the perf counter */
+	perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+	/* status register and data as read back from the device */
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_mag_report;
+#pragma pack(pop)
+
+	mag_report		*mag_report = &_mag_reports[_next_mag_report];
+
+	/* start the performance counter */
+	perf_begin(_mag_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+	/* XXX adapt the comment to specs */
+	/*
+	 * 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.
+	 */
+
+
+	mag_report->timestamp = hrt_absolute_time();
+	/* XXX adjust for sensor alignment to board here */
+	mag_report->x_raw = raw_mag_report.x;
+	mag_report->y_raw = raw_mag_report.y;
+	mag_report->z_raw = raw_mag_report.z;
+	mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_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_mag_report, _num_mag_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_mag_report == _oldest_mag_report)
+		INCREMENT(_oldest_mag_report, _num_mag_reports);
+
+	/* XXX please check this poll_notify, is it the right one? */
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+
+	/* stop the perf counter */
+	perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+	perf_print_counter(_accel_sample_perf);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
+	perf_print_counter(_mag_sample_perf);
+		printf("report queue:   %u (%u/%u @ %p)\n",
+		   _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+	CDev("LSM303D_mag", MAG_DEVICE_PATH),
+	_parent(parent)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+	_parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+	_parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd, fd_mag;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+
+	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;
+
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	/* don't fail if open cannot be opened */
+	if (0 <= fd_mag) {
+		if (ioctl(fd_mag, 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_accel = -1;
+	struct accel_report a_report;
+	ssize_t sz;
+
+	/* get the driver */
+	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd_accel < 0)
+		err(1, "%s open failed", ACCEL_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd_accel, &a_report, sizeof(a_report));
+
+	if (sz != sizeof(a_report))
+		err(1, "immediate read failed");
+
+	/* XXX fix the test output */
+//	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
+//	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
+//	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
+	warnx("accel x: \t%d\traw", (int)a_report.x_raw);
+	warnx("accel y: \t%d\traw", (int)a_report.y_raw);
+	warnx("accel z: \t%d\traw", (int)a_report.z_raw);
+//	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
+
+
+
+	int fd_mag = -1;
+	struct mag_report m_report;
+
+	/* get the driver */
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd_mag < 0)
+		err(1, "%s open failed", MAG_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd_mag, &m_report, sizeof(m_report));
+
+	if (sz != sizeof(m_report))
+		err(1, "immediate read failed");
+
+	/* XXX fix the test output */
+	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+	warnx("mag z: \t%d\traw", (int)m_report.z_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\n");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		lsm303d::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		lsm303d::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		lsm303d::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		lsm303d::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
new file mode 100644
index 000000000..e93b1e331
--- /dev/null
+++ b/src/drivers/lsm303d/module.mk
@@ -0,0 +1,6 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND	 = lsm303d
+SRCS		 = lsm303d.cpp
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
new file mode 100644
index 000000000..d3865f053
--- /dev/null
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -0,0 +1,1217 @@
+/****************************************************************************
+ *
+ *   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 on v1 and v2 boards.
+ */
+
+#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>
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+# include <drivers/boards/px4fmu/px4fmu_internal.h>
+# define FMU_HAVE_PPM
+#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+# include <drivers/boards/px4fmuv2/px4fmu_internal.h>
+# undef FMU_HAVE_PPM
+#else
+# error Unrecognised FMU board.
+#endif
+
+#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>
+#ifdef FMU_HAVE_PPM
+# include <systemlib/ppm_decode.h>
+#endif
+
+class PX4FMU : public device::CDev
+{
+public:
+	enum Mode {
+		MODE_NONE,
+		MODE_2PWM,
+		MODE_4PWM,
+		MODE_6PWM,
+	};
+	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[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	{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},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
+	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
+	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
+	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
+	{0, GPIO_VDD_5V_PERIPH_EN, 0},
+	{GPIO_5V_HIPOWER_OC, 0, 0},
+	{GPIO_5V_PERIPH_OC, 0, 0},
+#endif
+};
+
+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:	// v1 multi-port with flow control lines as PWM
+		debug("MODE_2PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x3);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_4PWM: // v1 multi-port as 4 PWM outs
+		debug("MODE_4PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xf);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_6PWM: // v2 PWMs as 6 PWM outs
+		debug("MODE_6PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x3f);
+		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;
+
+#ifdef FMU_HAVE_PPM
+	// 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;
+#endif
+
+	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) {
+
+				unsigned num_outputs;
+
+				switch (_mode) {
+				case MODE_2PWM:
+					num_outputs = 2;
+					break;
+				case MODE_4PWM:
+					num_outputs = 4;
+					break;
+				case MODE_6PWM:
+					num_outputs = 6;
+					break;
+				default:
+					num_outputs = 0;
+					break;
+				}
+
+				/* 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 */
+			up_pwm_servo_arm(aa.armed && !aa.lockdown);
+		}
+
+#ifdef FMU_HAVE_PPM
+		// 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);
+			}
+		}
+#endif
+
+	}
+
+	::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:
+	case MODE_6PWM:
+		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_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(5):
+	case PWM_SERVO_SET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_SET(3):
+	case PWM_SERVO_SET(2):
+		if (_mode < MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_SET(1):
+	case PWM_SERVO_SET(0):
+		if (arg < 2100) {
+			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PWM_SERVO_GET(5):
+	case PWM_SERVO_GET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_GET(3):
+	case PWM_SERVO_GET(2):
+		if (_mode < MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
+	case PWM_SERVO_GET(1):
+	case PWM_SERVO_GET(0):
+		*(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):
+	case PWM_SERVO_GET_RATEGROUP(4):
+	case PWM_SERVO_GET_RATEGROUP(5):
+		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
+		break;
+
+	case PWM_SERVO_GET_COUNT:	
+	case MIXERIOCGETOUTPUTCOUNT:
+		switch (_mode) {
+		case MODE_6PWM:
+			*(unsigned *)arg = 6;
+			break;
+		case MODE_4PWM:
+			*(unsigned *)arg = 4;
+			break;
+		case MODE_2PWM:
+			*(unsigned *)arg = 2;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		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[6];
+
+	if (count > 6) {
+		// we have at most 6 outputs
+		count = 6;
+	}
+
+	// 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, input if
+	 * possible otherwise output if possible.
+	 */
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (_gpio_tab[i].input != 0) {
+			stm32_configgpio(_gpio_tab[i].input);
+		} else if (_gpio_tab[i].output != 0) {
+			stm32_configgpio(_gpio_tab[i].output);
+		}
+	}
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	/* if we have a GPIO direction control, set it to zero (input) */
+	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+	stm32_configgpio(GPIO_GPIO_DIR);
+#endif
+}
+
+void
+PX4FMU::gpio_set_function(uint32_t gpios, int function)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	/*
+	 * 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);
+	}
+#endif
+
+	/* 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;
+			}
+		}
+	}
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	/* flip buffer to input mode if required */
+	if ((GPIO_SET_INPUT == function) && (gpios & 3))
+		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+#endif
+}
+
+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_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+		/* select 4-pin PWM mode */
+		servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+		servo_mode = PX4FMU::MODE_6PWM;
+#endif
+		break;
+
+	/* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	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_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;
+#endif
+	default:
+		return -1;
+	}
+
+	/* 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");
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+	if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed");
+
+	if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed");
+#endif
+
+	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_pwm")) {
+		new_mode = PORT_FULL_PWM;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	} else if (!strcmp(verb, "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
+
+	} 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;
+#endif
+	}
+
+	/* 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");
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+	fprintf(stderr, "  mode_gpio, mode_pwm\n");
+#endif
+	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/rgbled/module.mk b/src/drivers/rgbled/module.mk
new file mode 100644
index 000000000..39b53ec9e
--- /dev/null
+++ b/src/drivers/rgbled/module.mk
@@ -0,0 +1,6 @@
+#
+# TCA62724FMG driver for RGB LED
+#
+
+MODULE_COMMAND	 = rgbled
+SRCS		 = rgbled.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
new file mode 100644
index 000000000..dc1e469c0
--- /dev/null
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -0,0 +1,497 @@
+/****************************************************************************
+ *
+ *   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 rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ *
+ */
+
+#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 <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include "device/rgbled.h"
+
+#define LED_ONTIME 120
+#define LED_OFFTIME 120
+
+#define ADDR			PX4_I2C_OBDEV_LED	/**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
+#define SETTING_ENABLE   	0x02	/**< on */
+
+
+enum ledModes {
+	LED_MODE_TEST,
+	LED_MODE_SYSTEMSTATE,
+	LED_MODE_OFF
+};
+
+class RGBLED : public device::I2C
+{
+public:
+	RGBLED(int bus, int rgbled);
+	virtual ~RGBLED();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		setMode(enum ledModes mode);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+	enum ledColors {
+		LED_COLOR_OFF,
+		LED_COLOR_RED,
+		LED_COLOR_YELLOW,
+		LED_COLOR_PURPLE,
+		LED_COLOR_GREEN,
+		LED_COLOR_BLUE,
+		LED_COLOR_WHITE,
+		LED_COLOR_AMBER,
+	};
+
+	enum ledBlinkModes {
+		LED_BLINK_ON,
+		LED_BLINK_OFF
+	};
+
+	work_s			_work;
+
+	int led_colors[8];
+	int led_blink;
+
+	int mode;
+	int running;
+
+	void 			setLEDColor(int ledcolor);
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int				set(bool on, uint8_t r, uint8_t g, uint8_t b);
+	int				set_on(bool on);
+	int				set_rgb(uint8_t r, uint8_t g, uint8_t b);
+	int				get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+	RGBLED *g_rgbled;
+}
+
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
+	led_colors({0,0,0,0,0,0,0,0}),
+	led_blink(LED_BLINK_OFF),
+	mode(LED_MODE_OFF),
+	running(false)
+{
+	memset(&_work, 0, sizeof(_work));
+}
+
+RGBLED::~RGBLED()
+{
+}
+
+int
+RGBLED::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	/* start off */
+	set(false, 0, 0, 0);
+
+	return OK;
+}
+
+int
+RGBLED::setMode(enum ledModes new_mode)
+{
+	switch (new_mode) {
+		case LED_MODE_SYSTEMSTATE:
+		case LED_MODE_TEST:
+			mode = new_mode;
+			if (!running) {
+				running = true;
+				set_on(true);
+				work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+			}
+			break;
+		case LED_MODE_OFF:
+
+		default:
+			if (running) {
+				running = false;
+				set_on(false);
+			}
+			mode = LED_MODE_OFF;
+			break;
+	}
+
+	return OK;
+}
+
+int
+RGBLED::probe()
+{
+	int ret;
+	bool on, not_powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, not_powersave, r, g, b);
+
+	return ret;
+}
+
+int
+RGBLED::info()
+{
+	int ret;
+	bool on, not_powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, not_powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		log("state: %s", on ? "ON" : "OFF");
+		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+	} else {
+		warnx("failed to read led");
+	}
+
+	return ret;
+}
+
+int
+RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+	RGBLED *rgbl = (RGBLED *)arg;
+
+	rgbl->led();
+}
+
+
+
+void
+RGBLED::led()
+{
+	static int led_thread_runcount=0;
+	static int led_interval = 1000;
+
+	switch (mode) {
+		case LED_MODE_TEST:
+			/* Demo LED pattern for now */
+			led_colors[0] = LED_COLOR_YELLOW;
+			led_colors[1] = LED_COLOR_AMBER;
+			led_colors[2] = LED_COLOR_RED;
+			led_colors[3] = LED_COLOR_PURPLE;
+			led_colors[4] = LED_COLOR_BLUE;
+			led_colors[5] = LED_COLOR_GREEN;
+			led_colors[6] = LED_COLOR_WHITE;
+			led_colors[7] = LED_COLOR_OFF;
+			led_blink = LED_BLINK_ON;
+			break;
+
+		case LED_MODE_SYSTEMSTATE:
+			/* XXX TODO set pattern */
+			led_colors[0] = LED_COLOR_OFF;
+			led_colors[1] = LED_COLOR_OFF;
+			led_colors[2] = LED_COLOR_OFF;
+			led_colors[3] = LED_COLOR_OFF;
+			led_colors[4] = LED_COLOR_OFF;
+			led_colors[5] = LED_COLOR_OFF;
+			led_colors[6] = LED_COLOR_OFF;
+			led_colors[7] = LED_COLOR_OFF;
+			led_blink = LED_BLINK_OFF;
+
+			break;
+
+		case LED_MODE_OFF:
+		default:
+			return;
+			break;
+	}
+
+
+	if (led_thread_runcount & 1) {
+		if (led_blink == LED_BLINK_ON)
+			setLEDColor(LED_COLOR_OFF);
+		led_interval = LED_OFFTIME;
+	} else {
+		setLEDColor(led_colors[(led_thread_runcount/2) % 8]);
+		led_interval = LED_ONTIME;
+	}
+
+	led_thread_runcount++;
+
+
+	if(running) {
+		/* re-queue ourselves to run again later */
+		work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval);
+	} else {
+		set_on(false);
+	}
+}
+
+void RGBLED::setLEDColor(int ledcolor) {
+	switch (ledcolor) {
+		case LED_COLOR_OFF:	// off
+			set_rgb(0,0,0);
+			break;
+		case LED_COLOR_RED:	// red
+			set_rgb(255,0,0);
+			break;
+		case LED_COLOR_YELLOW:	// yellow
+			set_rgb(255,70,0);
+			break;
+		case LED_COLOR_PURPLE:	// purple
+			set_rgb(255,0,255);
+			break;
+		case LED_COLOR_GREEN:	// green
+			set_rgb(0,255,0);
+			break;
+		case LED_COLOR_BLUE:	// blue
+			set_rgb(0,0,255);
+			break;
+		case LED_COLOR_WHITE:	// white
+			set_rgb(255,255,255);
+			break;
+		case LED_COLOR_AMBER:	// amber
+			set_rgb(255,20,0);
+			break;
+	}
+}
+
+int
+RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
+{
+	uint8_t settings_byte = 0;
+
+	if (on)
+		settings_byte |= SETTING_ENABLE;
+/* powersave not used */
+//	if (not_powersave)
+		settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_on(bool on)
+{
+	uint8_t settings_byte = 0;
+
+	if (on)
+		settings_byte |= SETTING_ENABLE;
+
+/* powersave not used */
+//	if (not_powersave)
+		settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+
+int
+RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	uint8_t result[2];
+	int ret;
+
+	ret = transfer(nullptr, 0, &result[0], 2);
+
+	if (ret == OK) {
+		on = result[0] & SETTING_ENABLE;
+		not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+		/* XXX check, looks wrong */
+		r = (result[0] & 0x0f)*255/15;
+		g = (result[1] & 0xf0)*255/15;
+		b = (result[1] & 0x0f)*255/15;
+	}
+
+	return ret;
+}
+
+void rgbled_usage();
+
+
+void rgbled_usage() {
+	warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (3)");
+	warnx("\t-a --ddr addr (9)");
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+
+	int i2cdevice = PX4_I2C_BUS_LED;
+	int rgbledadr = ADDR; /* 7bit */
+
+	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], "--addr") == 0) {
+			if (argc > x + 1) {
+				rgbledadr = atoi(argv[x + 1]);
+			}
+		}
+
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (g_rgbled != nullptr)
+			errx(1, "already started");
+
+		g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
+		if (g_rgbled == nullptr)
+			errx(1, "new failed");
+
+		if (OK != g_rgbled->init()) {
+			delete g_rgbled;
+			g_rgbled = nullptr;
+			errx(1, "init failed");
+		}
+
+		exit(0);
+	}
+
+
+	if (g_rgbled == nullptr) {
+		fprintf(stderr, "not started\n");
+		rgbled_usage();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "test")) {
+		g_rgbled->setMode(LED_MODE_TEST);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "systemstate")) {
+		g_rgbled->setMode(LED_MODE_SYSTEMSTATE);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "info")) {
+		g_rgbled->info();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "off")) {
+		g_rgbled->setMode(LED_MODE_OFF);
+		exit(0);
+	}
+
+
+	/* things that require access to the device */
+	int fd = open(RGBLED_DEVICE_PATH, 0);
+	if (fd < 0)
+		err(1, "can't open RGBLED device");
+
+	rgbled_usage();
+	exit(0);
+}