Merge branch 'master' of github.com:PX4/Firmware into gps

89 files changed, 4026 insertions, 579 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index 833425aa6..a8ce3ea77 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -51,7 +51,7 @@
 #include <time.h>
 #include <systemlib/err.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index c68a26df9..70b8ad6de 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -42,7 +42,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <drivers/drv_gpio.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <systemlib/err.h>
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 7bb8490e5..3a432c3eb 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -59,7 +59,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/parameter_update.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/systemlib.h>
 
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 121b6d162..a7550b54b 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -62,8 +62,8 @@
 #include <v1.0/common/mavlink.h>
 #include <string.h>
 #include <arch/board/drv_led.h>
-#include <arch/board/up_hrt.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 #include "state_machine_helper.h"
 #include "systemlib/systemlib.h"
@@ -95,7 +95,7 @@
 PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
 //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
 
-#include <arch/board/up_cpuload.h>
+#include <systemlib/cpuload.h>
 extern struct system_load_s system_load;
 
 /* Decouple update interval and hysteris counters, all depends on intervals */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index b21f3858f..794fb679c 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -45,7 +45,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/systemlib.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp
index c554df9ae..bc4d4b3bf 100644
--- a/apps/drivers/bma180/bma180.cpp
+++ b/apps/drivers/bma180/bma180.cpp
@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
 #include <drivers/device/spi.h>
@@ -289,6 +289,7 @@ BMA180::init()
 	_num_reports = 2;
 	_oldest_report = _next_report = 0;
 	_reports = new struct accel_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
 
@@ -321,13 +322,16 @@ BMA180::init()
 	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
 
 	if (set_range(4)) warnx("Failed setting range");
+
 	if (set_lowpass(75)) warnx("Failed setting lowpass");
 
 	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
 		ret = OK;
+
 	} else {
 		ret = ERROR;
 	}
+
 out:
 	return ret;
 }
@@ -441,6 +445,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_call_interval == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -468,7 +473,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports -1;
+		return _num_reports - 1;
 
 	case SENSORIOCRESET:
 		/* XXX implement */
@@ -488,12 +493,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
+		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSRANGE:
@@ -549,24 +554,30 @@ BMA180::set_range(unsigned max_g)
 
 	if (max_g == 0)
 		max_g = 16;
+
 	if (max_g > 16)
 		return -ERANGE;
 
 	if (max_g <= 2) {
 		_current_range = 2;
 		rangebits = OFFSET_LSB1_RANGE_2G;
+
 	} else if (max_g <= 3) {
 		_current_range = 3;
 		rangebits = OFFSET_LSB1_RANGE_3G;
+
 	} else if (max_g <= 4) {
 		_current_range = 4;
 		rangebits = OFFSET_LSB1_RANGE_4G;
+
 	} else if (max_g <= 8) {
 		_current_range = 8;
 		rangebits = OFFSET_LSB1_RANGE_8G;
+
 	} else if (max_g <= 16) {
 		_current_range = 16;
 		rangebits = OFFSET_LSB1_RANGE_16G;
+
 	} else {
 		return -EINVAL;
 	}
@@ -586,7 +597,7 @@ BMA180::set_range(unsigned max_g)
 
 	/* check if wanted value is now in register */
 	return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
-		(OFFSET_LSB1_RANGE_MASK & rangebits));
+		 (OFFSET_LSB1_RANGE_MASK & rangebits));
 }
 
 int
@@ -633,7 +644,7 @@ BMA180::set_lowpass(unsigned frequency)
 
 	/* check if wanted value is now in register */
 	return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
-		(BW_TCS_BW_MASK & bwbits));
+		 (BW_TCS_BW_MASK & bwbits));
 }
 
 void
@@ -703,9 +714,9 @@ BMA180::measure()
 	 * perform only the axis assignment here.
 	 * Two non-value bits are discarded directly
 	 */
-	report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+1)) << 8) | (read_reg(ADDR_ACC_X_LSB));// XXX PX4DEV raw_report.x;
-	report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+3)) << 8) | (read_reg(ADDR_ACC_X_LSB+2));// XXX PX4DEV raw_report.y;
-	report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+5)) << 8) | (read_reg(ADDR_ACC_X_LSB+4));// XXX PX4DEV raw_report.z;
+	report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x;
+	report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y;
+	report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z;
 
 	/* discard two non-value bits in the 16 bit measurement */
 	report->x_raw = (report->x_raw >> 2);
@@ -781,17 +792,21 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
 	exit(0);
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -809,16 +824,18 @@ test()
 
 	/* get the driver */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
-		err(1, "%s open failed (try 'bma180 start' if the driver is not running)", 
-			ACCEL_DEVICE_PATH);
+		err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
+		    ACCEL_DEVICE_PATH);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
 		err(1, "reset to manual polling");
-	
+
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
+
 	if (sz != sizeof(a_report))
 		err(1, "immediate acc read failed");
 
@@ -831,7 +848,7 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-		(double)(a_report.range_m_s2 / 9.81f));
+	      (double)(a_report.range_m_s2 / 9.81f));
 
 	/* XXX add poll-rate tests here too */
 
@@ -846,10 +863,13 @@ 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");
 
diff --git a/apps/drivers/boards/px4fmu/Makefile b/apps/drivers/boards/px4fmu/Makefile
new file mode 100644
index 000000000..393e96e32
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/Makefile
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Board-specific startup code for the PX4FMU
+#
+
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c
new file mode 100644
index 000000000..987894163
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   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_adc.c
+ *
+ * Board-specific ADC functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+#include <stdio.h>
+
+#include <nuttx/analog/adc.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32_adc.h"
+#include "px4fmu_internal.h"
+
+#define ADC3_NCHANNELS 4
+
+/************************************************************************************
+ * Private Data
+ ************************************************************************************/
+/* The PX4FMU board has four ADC channels: ADC323 IN10-13
+ */
+
+/* Identifying number of each ADC channel: Variable Resistor. */
+
+#ifdef CONFIG_STM32_ADC3
+static const uint8_t  g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards
+
+/* Configurations of pins used byte each ADC channels */
+static const uint32_t g_pinlist[ADC3_NCHANNELS]  = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13};
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: adc_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/adc.
+ *
+ ************************************************************************************/
+
+int adc_devinit(void)
+{
+	static bool initialized = false;
+	struct adc_dev_s *adc[ADC3_NCHANNELS];
+	int ret;
+	int i;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		char name[11];
+
+		for (i = 0; i < ADC3_NCHANNELS; i++) {
+			stm32_configgpio(g_pinlist[i]);
+		}
+
+		for (i = 0; i < 1; i++) {
+			/* Configure the pins as analog inputs for the selected channels */
+			//stm32_configgpio(g_pinlist[i]);
+
+			/* Call stm32_adcinitialize() to get an instance of the ADC interface */
+			//multiple channels only supported with dma!
+			adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
+
+			if (adc == NULL) {
+				adbg("ERROR: Failed to get ADC interface\n");
+				return -ENODEV;
+			}
+
+
+			/* Register the ADC driver at "/dev/adc0" */
+			sprintf(name, "/dev/adc%d", i);
+			ret = adc_register(name, adc[i]);
+
+			if (ret < 0) {
+				adbg("adc_register failed for adc %s: %d\n", name, ret);
+				return ret;
+			}
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_can.c b/apps/drivers/boards/px4fmu/px4fmu_can.c
new file mode 100644
index 000000000..0d0b5fcd3
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_can.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_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"
+
+/************************************************************************************
+ * 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;
+}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
new file mode 100644
index 000000000..e5ded7328
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_init.c
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file 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 <nuttx/arch.h>
+
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+#include <arch/board/drv_led.h>
+#include <arch/board/drv_eeprom.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lib_lowprintf(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lib_lowprintf
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+extern int adc_devinit(void);
+
+/****************************************************************************
+ * 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 *spi3;
+static struct i2c_dev_s *i2c1;
+static struct i2c_dev_s *i2c2;
+static struct i2c_dev_s *i2c3;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+	int result;
+
+	/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
+
+	/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
+
+	/* configure the high-resolution time/callout interface */
+#ifdef CONFIG_HRT_TIMER
+	hrt_init();
+#endif
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+#ifdef SERIAL_HAVE_DMA
+	{
+		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);
+	}
+#endif
+
+	message("\r\n");
+
+	up_ledoff(LED_BLUE);
+	up_ledoff(LED_AMBER);
+
+	up_ledon(LED_BLUE);
+
+	/* Configure user-space led driver */
+	px4fmu_led_init();
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\r\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	// Default SPI1 to 1MHz and de-assert the known chip selects.
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\r\n");
+
+	/* initialize I2C2 bus */
+
+	i2c2 = up_i2cinitialize(2);
+
+	if (!i2c2) {
+		message("[boot] FAILED to initialize I2C bus 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* set I2C2 speed */
+	I2C_SETFREQUENCY(i2c2, 400000);
+
+
+	i2c3 = up_i2cinitialize(3);
+
+	if (!i2c3) {
+		message("[boot] FAILED to initialize I2C bus 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* set I2C3 speed */
+	I2C_SETFREQUENCY(i2c3, 400000);
+
+	/* try to attach, don't fail if device is not responding */
+	(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
+			    FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
+			    FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
+			    FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
+
+#if defined(CONFIG_STM32_SPI3)
+	/* Get the SPI port */
+
+	message("[boot] Initializing SPI port 3\n");
+	spi3 = up_spiinitialize(3);
+
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully initialized SPI port 3\n");
+
+	/* Now bind the SPI interface to the MMCSD driver */
+	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
+
+	if (result != OK) {
+		message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
+#endif /* SPI3 */
+
+	/* initialize I2C1 bus */
+
+	i2c1 = up_i2cinitialize(1);
+
+	if (!i2c1) {
+		message("[boot] FAILED to initialize I2C bus 1\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* set I2C1 speed */
+	I2C_SETFREQUENCY(i2c1, 400000);
+
+	/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
+
+	/* Get board information if available */
+
+	/* Initialize the user GPIOs */
+	px4fmu_gpio_init();
+
+#ifdef CONFIG_ADC
+	int adc_state = adc_devinit();
+
+	if (adc_state != OK) {
+		/* Try again */
+		adc_state = adc_devinit();
+
+		if (adc_state != OK) {
+			/* Give up */
+			message("[boot] FAILED adc_devinit: %d\n", adc_state);
+			return -ENODEV;
+		}
+	}
+
+#endif
+
+	return OK;
+}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_internal.h b/apps/drivers/boards/px4fmu/px4fmu_internal.h
new file mode 100644
index 000000000..c58a8a5c4
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_internal.h
@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+#ifdef CONFIG_STM32_SPI2
+#  error "SPI2 is not supported on this board"
+#endif
+
+#if defined(CONFIG_STM32_CAN1)
+#  warning "CAN1 is not supported on this board"
+#endif
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+
+/* External interrupts */
+#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+// XXX MPU6000 DRDY?
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+
+/* User GPIOs
+ *
+ * GPIO0-1 are the buffered high-power GPIOs.
+ * GPIO2-5 are the USART2 pins.
+ * GPIO6-7 are the CAN2 pins.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* PWM
+ *
+ * The PX4FMU has five PWM outputs, of which only the output on
+ * pin PC8 is fixed assigned to this function. The other four possible
+ * pwm sources are the TX, RX, RTS and CTS pins of USART2
+ *
+ * Alternate function mapping:
+ * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+ */
+
+#ifdef CONFIG_PWM
+#  if defined(CONFIG_STM32_TIM3_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#	 define BUZZER_PWMTIMER 3
+#  elif defined(CONFIG_STM32_TIM8_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#    define BUZZER_PWMTIMER 8
+#  endif
+#endif
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+/****************************************************************************************************
+ * Name: px4fmu_gpio_init
+ *
+ * Description:
+ *   Called to configure the PX4FMU user GPIOs
+ *
+ ****************************************************************************************************/
+
+extern void px4fmu_gpio_init(void);
+
+
+// XXX additional SPI chipselect functions required?
+
+#endif /* __ASSEMBLY__ */
diff --git a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..d1ff8c112
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   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 "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c
new file mode 100644
index 000000000..70245a3ec
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_spi.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+	stm32_configgpio(GPIO_SPI_CS_SDCARD);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
+		break;
+
+	case PX4_SPIDEV_ACCEL:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
+	return SPI_STATUS_PRESENT;
+}
+
diff --git a/apps/drivers/boards/px4fmu/px4fmu_usb.c b/apps/drivers/boards/px4fmu/px4fmu_usb.c
new file mode 100644
index 000000000..b0b669fbe
--- /dev/null
+++ b/apps/drivers/boards/px4fmu/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include "up_arch.h"
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h
index 9af678465..01692c315 100644
--- a/apps/drivers/device/device.h
+++ b/apps/drivers/device/device.h
@@ -54,7 +54,7 @@
 /**
  * Namespace encapsulating all device framework classes, functions and data.
  */
-namespace device __EXPORT 
+namespace device __EXPORT
 {
 
 /**
@@ -276,14 +276,14 @@ public:
 	 */
 	virtual int	poll(struct file *filp, struct pollfd *fds, bool setup);
 
-       /**
-        * Test whether the device is currently open.
-        *
-        * This can be used to avoid tearing down a device that is still active.
-        *
-        * @return              True if the device is currently open.
-        */
-       bool            is_open() { return _open_count > 0; }
+	/**
+	 * Test whether the device is currently open.
+	 *
+	 * This can be used to avoid tearing down a device that is still active.
+	 *
+	 * @return              True if the device is currently open.
+	 */
+	bool            is_open() { return _open_count > 0; }
 
 protected:
 	/**
diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp
index c4b2aa944..4b832b548 100644
--- a/apps/drivers/device/i2c.cpp
+++ b/apps/drivers/device/i2c.cpp
@@ -121,7 +121,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
 	unsigned tries = 0;
 
 	do {
-	//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+		//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
 
 		msgs = 0;
 
@@ -144,7 +144,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
 		if (msgs == 0)
 			return -EINVAL;
 
-		/* 
+		/*
 		 * I2C architecture means there is an unavoidable race here
 		 * if there are any devices on the bus with a different frequency
 		 * preference.  Really, this is pointless.
@@ -154,7 +154,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
 
 		if (ret == OK)
 			break;
-		
+
 		// reset the I2C bus to unwedge on error
 		up_i2creset(_dev);
 
diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h
index 7c5a14d6b..eb1b6cb05 100644
--- a/apps/drivers/device/i2c.h
+++ b/apps/drivers/device/i2c.h
@@ -42,7 +42,7 @@
 
 #include <nuttx/i2c.h>
 
-namespace device __EXPORT 
+namespace device __EXPORT
 {
 
 /**
diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp
index a1761b769..528333e04 100644
--- a/apps/drivers/device/spi.cpp
+++ b/apps/drivers/device/spi.cpp
@@ -134,6 +134,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	/* do common setup */
 	if (!up_interrupt_context())
 		SPI_LOCK(_dev, true);
+
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
 	SPI_SETBITS(_dev, 8);
@@ -144,6 +145,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 
 	/* and clean up */
 	SPI_SELECT(_dev, _device, false);
+
 	if (!up_interrupt_context())
 		SPI_LOCK(_dev, false);
 
diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h
index b2a111562..e8c8e2c5e 100644
--- a/apps/drivers/device/spi.h
+++ b/apps/drivers/device/spi.h
@@ -84,7 +84,7 @@ protected:
 	 * If called from interrupt context, this interface does not lock
 	 * the bus and may interfere with non-interrupt-context callers.
 	 *
-	 * Clients in a mixed interrupt/non-interrupt configuration must 
+	 * Clients in a mixed interrupt/non-interrupt configuration must
 	 * ensure appropriate interlocking.
 	 *
 	 * At least one of send or recv must be non-null.
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index 6d0c8c545..3834795e0 100644
--- a/apps/drivers/drv_accel.h
+++ b/apps/drivers/drv_accel.h
@@ -83,7 +83,7 @@ ORB_DECLARE(sensor_accel);
 /*
  * ioctl() definitions
  *
- * Accelerometer drivers also implement the generic sensor driver 
+ * Accelerometer drivers also implement the generic sensor driver
  * interfaces from drv_sensor.h
  */
 
diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h
index 5c646f243..0dbf05c5b 100644
--- a/apps/drivers/drv_gyro.h
+++ b/apps/drivers/drv_gyro.h
@@ -58,7 +58,7 @@ struct gyro_report {
 	float temperature;	/**< temperature in degrees celcius */
 	float range_rad_s;
 	float scaling;
-	
+
 	int16_t x_raw;
 	int16_t y_raw;
 	int16_t z_raw;
diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h
new file mode 100644
index 000000000..3b493a81a
--- /dev/null
+++ b/apps/drivers/drv_hrt.h
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_hrt.h
+ *
+ * High-resolution timer with callouts and timekeeping.
+ */
+
+#pragma once
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <time.h>
+#include <queue.h>
+
+__BEGIN_DECLS
+
+/*
+ * Absolute time, in microsecond units.
+ *
+ * Absolute time is measured from some arbitrary epoch shortly after
+ * system startup.  It should never wrap or go backwards.
+ */
+typedef uint64_t	hrt_abstime;
+
+/*
+ * Callout function type.
+ *
+ * Note that callouts run in the timer interrupt context, so
+ * they are serialised with respect to each other, and must not
+ * block.
+ */
+typedef void	(* hrt_callout)(void *arg);
+
+/*
+ * Callout record.
+ */
+typedef struct hrt_call {
+	struct sq_entry_s	link;
+
+	hrt_abstime		deadline;
+	hrt_abstime		period;
+	hrt_callout		callout;
+	void			*arg;
+} *hrt_call_t;
+
+/*
+ * Get absolute time.
+ */
+__EXPORT extern hrt_abstime hrt_absolute_time(void);
+
+/*
+ * Convert a timespec to absolute time.
+ */
+__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
+
+/*
+ * Convert absolute time to a timespec.
+ */
+__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+__EXPORT extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+__EXPORT extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+__EXPORT extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
+
+/*
+ * If this returns true, the entry has been invoked and removed from the callout list,
+ * or it has never been entered.
+ *
+ * Always returns false for repeating callouts.
+ */
+__EXPORT extern bool	hrt_called(struct hrt_call *entry);
+
+/*
+ * Remove the entry from the callout list.
+ */
+__EXPORT extern void	hrt_cancel(struct hrt_call *entry);
+
+/*
+ * Initialise the HRT.
+ */
+__EXPORT extern void	hrt_init(void);
+
+__END_DECLS
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index 7ba9dd695..114bcb646 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -59,7 +59,7 @@ struct mag_report {
 	float z;
 	float range_ga;
 	float scaling;
-	
+
 	int16_t x_raw;
 	int16_t y_raw;
 	int16_t z_raw;
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index 340175a4b..97033f2cd 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -41,14 +41,15 @@
  * channel.
  */
 
-#ifndef _DRV_PWM_OUTPUT_H
-#define _DRV_PWM_OUTPUT_H
+#pragma once
 
 #include <stdint.h>
 #include <sys/ioctl.h>
 
 #include "drv_orb_dev.h"
 
+__BEGIN_DECLS
+
 /**
  * Path for the default PWM output device.
  *
@@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_GET(_servo)	_IOC(_PWM_SERVO_BASE, 0x40 + _servo)
 
 
-#endif /* _DRV_PWM_OUTPUT_H */
+/*
+ * Low-level PWM output interface.
+ *
+ * This is the low-level API to the platform-specific PWM driver.
+ */
+
+/**
+ * Intialise the PWM servo outputs using the specified configuration.
+ *
+ * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
+ *			This allows some of the channels to remain configured
+ *			as GPIOs or as another function.
+ * @return		OK on success.
+ */
+__EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);
+
+/**
+ * De-initialise the PWM servo outputs.
+ */
+__EXPORT extern void	up_pwm_servo_deinit(void);
+
+/**
+ * Arm or disarm servo outputs.
+ *
+ * When disarmed, servos output no pulse.
+ *
+ * @bug This function should, but does not, guarantee that any pulse
+ *      currently in progress is cleanly completed.
+ *
+ * @param armed		If true, outputs are armed; if false they
+ *			are disarmed.
+ */
+__EXPORT extern void	up_pwm_servo_arm(bool armed);
+
+/**
+ * Set the servo update rate
+ *
+ * @param rate		The update rate in Hz to set.
+ * @return		OK on success, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate(unsigned rate);
+
+/**
+ * Set the current output value for a channel.
+ *
+ * @param channel	The channel to set.
+ * @param value		The output pulse width in microseconds.
+ */
+__EXPORT extern int	up_pwm_servo_set(unsigned channel, servo_position_t value);
+
+/**
+ * Get the current output value for a channel.
+ *
+ * @param channel	The channel to read.
+ * @return		The output pulse width in microseconds, or zero if
+ *			outputs are not armed or not configured.
+ */
+__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
+
+__END_DECLS
diff --git a/apps/drivers/drv_sensor.h b/apps/drivers/drv_sensor.h
index 325bd42bf..3a89cab9d 100644
--- a/apps/drivers/drv_sensor.h
+++ b/apps/drivers/drv_sensor.h
@@ -52,7 +52,7 @@
 #define _SENSORIOC(_n)		(_IOC(_SENSORIOCBASE, _n))
 
 /**
- * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE 
+ * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
  * constants
  */
 #define SENSORIOCSPOLLRATE	_SENSORIOC(0)
@@ -68,8 +68,8 @@
 #define SENSOR_POLLRATE_MAX		1000002	/**< poll at device maximum rate */
 #define SENSOR_POLLRATE_DEFAULT		1000003	/**< poll at driver normal rate */
 
-/** 
- * Set the internal queue depth to (arg) entries, must be at least 1 
+/**
+ * Set the internal queue depth to (arg) entries, must be at least 1
  *
  * This sets the upper bound on the number of readings that can be
  * read from the driver.
diff --git a/apps/drivers/drv_tone_alarm.h b/apps/drivers/drv_tone_alarm.h
index 27b146de9..0c6afc64c 100644
--- a/apps/drivers/drv_tone_alarm.h
+++ b/apps/drivers/drv_tone_alarm.h
@@ -34,8 +34,8 @@
 /*
  * Driver for the PX4 audio alarm port, /dev/tone_alarm.
  *
- * The tone_alarm driver supports a set of predefined "alarm" 
- * patterns and one user-supplied pattern.  Patterns are ordered by 
+ * The tone_alarm driver supports a set of predefined "alarm"
+ * patterns and one user-supplied pattern.  Patterns are ordered by
  * priority, with a higher-priority pattern interrupting any
  * lower-priority pattern that might be playing.
  *
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 8e78825c3..7943012cc 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -115,6 +115,10 @@
 #endif
 static const int ERROR = -1;
 
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
 class HMC5883 : public device::I2C
 {
 public:
@@ -328,13 +332,16 @@ HMC5883::init()
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct mag_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
+
 	_oldest_report = _next_report = 0;
 
 	/* get a publish handle on the mag topic */
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
+
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
@@ -354,30 +361,37 @@ int HMC5883::set_range(unsigned range)
 		range_bits = 0x00;
 		_range_scale = 1.0f / 1370.0f;
 		_range_ga = 0.88f;
+
 	} else if (range <= 1) {
 		range_bits = 0x01;
 		_range_scale = 1.0f / 1090.0f;
 		_range_ga = 1.3f;
+
 	} else if (range <= 2) {
 		range_bits = 0x02;
 		_range_scale = 1.0f / 820.0f;
 		_range_ga = 1.9f;
+
 	} else if (range <= 3) {
 		range_bits = 0x03;
 		_range_scale = 1.0f / 660.0f;
 		_range_ga = 2.5f;
+
 	} else if (range <= 4) {
 		range_bits = 0x04;
 		_range_scale = 1.0f / 440.0f;
 		_range_ga = 4.0f;
+
 	} else if (range <= 4.7f) {
 		range_bits = 0x05;
 		_range_scale = 1.0f / 390.0f;
 		_range_ga = 4.7f;
+
 	} else if (range <= 5.6f) {
 		range_bits = 0x06;
 		_range_scale = 1.0f / 330.0f;
 		_range_ga = 5.6f;
+
 	} else {
 		range_bits = 0x07;
 		_range_scale = 1.0f / 230.0f;
@@ -409,10 +423,12 @@ HMC5883::probe()
 	uint8_t data[3] = {0, 0, 0};
 
 	_retries = 10;
+
 	if (read_reg(ADDR_ID_A, data[0]) ||
 	    read_reg(ADDR_ID_B, data[1]) ||
 	    read_reg(ADDR_ID_C, data[2]))
 		debug("read_reg fail");
+
 	_retries = 1;
 
 	if ((data[0] != ID_A_WHO_AM_I) ||
@@ -548,6 +564,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -662,7 +679,7 @@ HMC5883::cycle()
 		if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
 
 			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK, 
+			work_queue(HPWORK,
 				   &_work,
 				   (worker_t)&HMC5883::cycle_trampoline,
 				   this,
@@ -680,7 +697,7 @@ HMC5883::cycle()
 	_collect_phase = true;
 
 	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK, 
+	work_queue(HPWORK,
 		   &_work,
 		   (worker_t)&HMC5883::cycle_trampoline,
 		   this,
@@ -846,7 +863,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	warnx("starting mag scale calibration");
 
 	/* do a simple demand read */
-	sz = read(filp, (char*)&report, sizeof(report));
+	sz = read(filp, (char *)&report, sizeof(report));
+
 	if (sz != sizeof(report)) {
 		warn("immediate read failed");
 		ret = 1;
@@ -916,6 +934,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		if (sz != sizeof(report)) {
 			warn("periodic read failed");
 			goto out;
+
 		} else {
 			avg_excited[0] += report.x;
 			avg_excited[1] += report.y;
@@ -942,7 +961,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	scaling[2] = fabsf(1.08f / avg_excited[2]);
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-	
+
 	/* set back to normal mode */
 	/* Set to 1.1 Gauss */
 	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
@@ -967,12 +986,15 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	ret = OK;
 
-	out:
-		if (ret == OK) {
-			warnx("mag scale calibration successfully finished.");
-		} else {
-			warnx("mag scale calibration failed.");
-		}
+out:
+
+	if (ret == OK) {
+		warnx("mag scale calibration successfully finished.");
+
+	} else {
+		warnx("mag scale calibration failed.");
+	}
+
 	return ret;
 }
 
@@ -982,16 +1004,22 @@ int HMC5883::set_excitement(unsigned enable)
 	/* arm the excitement strap */
 	uint8_t conf_reg;
 	ret = read_reg(ADDR_CONF_A, conf_reg);
+
 	if (OK != ret)
 		perf_count(_comms_errors);
+
 	if (((int)enable) < 0) {
 		conf_reg |= 0x01;
+
 	} else if (enable > 0) {
 		conf_reg |= 0x02;
+
 	} else {
 		conf_reg &= ~0x03;
 	}
+
 	ret = write_reg(ADDR_CONF_A, conf_reg);
+
 	if (OK != ret)
 		perf_count(_comms_errors);
 
@@ -1083,17 +1111,22 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
+
 	exit(0);
 
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -1110,11 +1143,13 @@ test()
 	int ret;
 
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
+
 	if (sz != sizeof(report))
 		err(1, "immediate read failed");
 
@@ -1163,7 +1198,7 @@ test()
  * Basic idea:
  *
  *   output = (ext field +- 1.1 Ga self-test) * scale factor
- * 
+ *
  * and consequently:
  *
  *   1.1 Ga = (excited - normal) * scale factor
@@ -1203,6 +1238,7 @@ int calibrate()
 	int ret;
 
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
 
@@ -1214,6 +1250,7 @@ int calibrate()
 
 	if (ret == OK) {
 		errx(0, "PASS");
+
 	} else {
 		errx(1, "FAIL");
 	}
@@ -1226,10 +1263,13 @@ void
 reset()
 {
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
@@ -1286,6 +1326,7 @@ hmc5883_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "calibrate")) {
 		if (hmc5883::calibrate() == 0) {
 			errx(0, "calibration successful");
+
 		} else {
 			errx(1, "calibration failed");
 		}
diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp
index e9b60b01c..4915b81e3 100644
--- a/apps/drivers/l3gd20/l3gd20.cpp
+++ b/apps/drivers/l3gd20/l3gd20.cpp
@@ -57,7 +57,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
 #include <drivers/device/spi.h>
@@ -317,6 +317,7 @@ L3GD20::init()
 	_num_reports = 2;
 	_oldest_report = _next_report = 0;
 	_reports = new struct gyro_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
 
@@ -330,7 +331,7 @@ L3GD20::init()
 	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
 	write_reg(ADDR_CTRL_REG4, REG4_BDU);
 	write_reg(ADDR_CTRL_REG5, 0);
-		
+
 	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);	  /* disable wake-on-interrupt */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
 
@@ -451,6 +452,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_call_interval == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -478,7 +480,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports -1;
+		return _num_reports - 1;
 
 	case SENSORIOCRESET:
 		/* XXX implement */
@@ -497,12 +499,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
+		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
+		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCSRANGE:
@@ -562,12 +564,15 @@ L3GD20::set_range(unsigned max_dps)
 	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;
 	}
@@ -590,15 +595,19 @@ L3GD20::set_samplerate(unsigned frequency)
 	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;
 	}
@@ -746,17 +755,21 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
 	exit(0);
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -774,15 +787,17 @@ test()
 
 	/* get the driver */
 	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd_gyro < 0)
 		err(1, "%s open failed", GYRO_DEVICE_PATH);
 
 	/* reset to manual polling */
 	if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
 		err(1, "reset to manual polling");
-	
+
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
 	if (sz != sizeof(g_report))
 		err(1, "immediate gyro read failed");
 
@@ -793,7 +808,7 @@ test()
 	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
 	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
 	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
-		(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
+	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 
 	/* XXX add poll-rate tests here too */
 
@@ -808,10 +823,13 @@ void
 reset()
 {
 	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 7b8a84f7e..90786886a 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -61,7 +61,7 @@
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
@@ -569,6 +569,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_call_interval == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH:
@@ -592,12 +593,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
+		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSRANGE:
@@ -639,12 +640,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
+		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
+		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCSRANGE:
@@ -976,17 +977,21 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
 	exit(0);
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -1006,21 +1011,24 @@ test()
 
 	/* get the driver */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
-		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", 
-			ACCEL_DEVICE_PATH);
+		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
+		    ACCEL_DEVICE_PATH);
 
 	/* get the driver */
 	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd_gyro < 0)
 		err(1, "%s open failed", GYRO_DEVICE_PATH);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
 		err(1, "reset to manual polling");
-	
+
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
+
 	if (sz != sizeof(a_report))
 		err(1, "immediate acc read failed");
 
@@ -1033,10 +1041,11 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-		(double)(a_report.range_m_s2 / 9.81f));
+	      (double)(a_report.range_m_s2 / 9.81f));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
 	if (sz != sizeof(g_report))
 		err(1, "immediate gyro read failed");
 
@@ -1047,7 +1056,7 @@ test()
 	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
 	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
 	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
-		(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
+	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 
 	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
 	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
@@ -1066,10 +1075,13 @@ 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");
 
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
index 4df9de94c..699cd36d2 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/apps/drivers/ms5611/ms5611.cpp
@@ -57,7 +57,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -70,6 +70,10 @@
 #endif
 static const int ERROR = -1;
 
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
 /**
  * Calibration PROM as reported by the device.
  */
@@ -299,6 +303,7 @@ MS5611::init()
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct baro_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
 
@@ -307,6 +312,7 @@ MS5611::init()
 	/* get a publish handle on the baro topic */
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+
 	if (_baro_topic < 0)
 		debug("failed to create sensor_baro object");
 
@@ -319,9 +325,10 @@ int
 MS5611::probe()
 {
 	_retries = 10;
-	if((OK == probe_address(MS5611_ADDRESS_1)) ||
-	   (OK == probe_address(MS5611_ADDRESS_2))) {
-	   	_retries = 1;
+
+	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
+	    (OK == probe_address(MS5611_ADDRESS_2))) {
+		_retries = 1;
 		return OK;
 	}
 
@@ -480,6 +487,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -514,9 +522,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return -EINVAL;
 
 	case BAROIOCSMSLPRESSURE:
+
 		/* range-check for sanity */
 		if ((arg < 80000) || (arg > 120000))
 			return -EINVAL;
+
 		_msl_pressure = arg;
 		return OK;
 
@@ -684,7 +694,7 @@ MS5611::collect()
 			int64_t SENS2 = 5 * f >> 2;
 
 			if (_TEMP < -1500) {
-				int64_t f2 = POW2(_TEMP + 1500); 
+				int64_t f2 = POW2(_TEMP + 1500);
 				OFF2 += 7 * f2;
 				SENS2 += 11 * f2 >> 1;
 			}
@@ -693,6 +703,7 @@ MS5611::collect()
 			_OFF  -= OFF2;
 			_SENS -= SENS2;
 		}
+
 	} else {
 
 		/* pressure calculation, result in Pa */
@@ -810,8 +821,8 @@ MS5611::read_prom()
 		uint16_t	w;
 	} cvt;
 
-	/* 
-	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are 
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
 	 * called immediately after reset.
 	 */
 	usleep(3000);
@@ -937,17 +948,22 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
+
 	exit(0);
 
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -964,11 +980,13 @@ test()
 	int ret;
 
 	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
+
 	if (sz != sizeof(report))
 		err(1, "immediate read failed");
 
@@ -1021,10 +1039,13 @@ void
 reset()
 {
 	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
@@ -1057,6 +1078,7 @@ calibrate(unsigned altitude)
 	float	p1;
 
 	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
 
@@ -1066,6 +1088,7 @@ calibrate(unsigned altitude)
 
 	/* average a few measurements */
 	pressure = 0.0f;
+
 	for (unsigned i = 0; i < 20; i++) {
 		struct pollfd fds;
 		int ret;
@@ -1087,6 +1110,7 @@ calibrate(unsigned altitude)
 
 		pressure += report.pressure;
 	}
+
 	pressure /= 20;		/* average */
 	pressure /= 10;		/* scale from millibar to kPa */
 
@@ -1104,8 +1128,10 @@ calibrate(unsigned altitude)
 
 	/* save as integer Pa */
 	p1 *= 1000.0f;
+
 	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
 		err(1, "BAROIOCSMSLPRESSURE");
+
 	exit(0);
 }
 
@@ -1146,7 +1172,7 @@ ms5611_main(int argc, char *argv[])
 			errx(1, "missing altitude");
 
 		long altitude = strtol(argv[2], nullptr, 10);
-		
+
 		ms5611::calibrate(altitude);
 	}
 
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 66db1c360..995c9393f 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -110,7 +110,7 @@ private:
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
 	volatile bool		_switch_armed;	///< PX4IO switch armed state
-						// XXX how should this work?
+	// XXX how should this work?
 
 	bool			_send_needed;	///< If true, we need to send a packet to IO
 
@@ -149,13 +149,13 @@ private:
 	 * group/index during mixing.
 	 */
 	static int		control_callback(uintptr_t handle,
-						 uint8_t control_group,
-						 uint8_t control_index,
-						 float &input);
+			uint8_t control_group,
+			uint8_t control_index,
+			float &input);
 };
 
 
-namespace 
+namespace
 {
 
 PX4IO	*g_dev;
@@ -190,6 +190,7 @@ PX4IO::~PX4IO()
 
 		/* spin waiting for the thread to stop */
 		unsigned i = 10;
+
 		do {
 			/* wait 50ms - it should wake every 100ms or so worst-case */
 			usleep(50000);
@@ -223,11 +224,13 @@ PX4IO::init()
 
 	/* 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;
@@ -235,6 +238,7 @@ PX4IO::init()
 
 	/* start the IO interface task */
 	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
@@ -256,6 +260,7 @@ PX4IO::task_main()
 
 	/* open the serial port */
 	_serial_fd = ::open("/dev/ttyS2", O_RDWR);
+
 	if (_serial_fd < 0) {
 		debug("failed to open serial port for IO: %d", errno);
 		_task = -1;
@@ -343,6 +348,7 @@ PX4IO::task_main()
 					_send_needed = true;
 				}
 			}
+
 			if (fds[2].revents & POLLIN) {
 
 				orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
@@ -364,9 +370,9 @@ PX4IO::task_main()
 
 int
 PX4IO::control_callback(uintptr_t handle,
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
+			uint8_t control_group,
+			uint8_t control_index,
+			float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -458,13 +464,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+
 		/* fake an update to the selected servo channel */
 		if ((arg >= 900) && (arg <= 2100)) {
 			_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
 			_send_needed = true;
+
 		} else {
 			ret = -EINVAL;
 		}
+
 		break;
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
@@ -481,6 +490,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			delete _mixers;
 			_mixers = nullptr;
 		}
+
 		break;
 
 	case MIXERIOCADDSIMPLE: {
@@ -519,6 +529,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 			/* allocate a new mixer group and load it from the file */
 			newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
 			if (newmixers->load_from_file(path) != 0) {
 				delete newmixers;
 				ret = -EINVAL;
@@ -528,6 +539,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			if (_mixers != nullptr) {
 				delete _mixers;
 			}
+
 			_mixers = newmixers;
 
 		}
@@ -537,6 +549,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		/* not a recognised value */
 		ret = -ENOTTY;
 	}
+
 	unlock();
 
 	return ret;
@@ -576,6 +589,7 @@ px4io_main(int argc, char *argv[])
 		if (argc > 2) {
 			fn[0] = argv[2];
 			fn[1] = nullptr;
+
 		} else {
 			fn[0] = "/fs/microsd/px4io.bin";
 			fn[1] =	"/etc/px4io.bin";
@@ -589,18 +603,24 @@ px4io_main(int argc, char *argv[])
 		switch (ret) {
 		case OK:
 			break;
+
 		case -ENOENT:
 			errx(1, "PX4IO firmware file not found");
+
 		case -EEXIST:
 		case -EIO:
 			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
 		case -EINVAL:
 			errx(1, "verify failed - retry the update");
+
 		case -ETIMEDOUT:
 			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
 		default:
 			errx(1, "unexpected error %d", ret);
 		}
+
 		return ret;
 	}
 
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 0fbbac839..5669aeb01 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -67,6 +67,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 	int	ret;
 
 	_io_fd = open("/dev/ttyS2", O_RDWR);
+
 	if (_io_fd < 0) {
 		log("could not open interface");
 		return -errno;
@@ -74,6 +75,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 	/* look for the bootloader */
 	ret = sync();
+
 	if (ret != OK) {
 		/* this is immediately fatal */
 		log("bootloader not responding");
@@ -87,17 +89,20 @@ PX4IO_Uploader::upload(const char *filenames[])
 			log("failed to open %s", filenames[i]);
 			continue;
 		}
+
 		log("using firmware from %s", filenames[i]);
 		break;
 	}
+
 	if (_fw_fd == -1)
 		return -ENOENT;
 
 	/* do the usual program thing - allow for failure */
 	for (unsigned retries = 0; retries < 1; retries++) {
 		if (retries > 0) {
-			log("retrying update..."); 
+			log("retrying update...");
 			ret = sync();
+
 			if (ret != OK) {
 				/* this is immediately fatal */
 				log("bootloader not responding");
@@ -106,25 +111,33 @@ PX4IO_Uploader::upload(const char *filenames[])
 		}
 
 		ret = erase();
+
 		if (ret != OK) {
 			log("erase failed");
 			continue;
 		}
+
 		ret = program();
+
 		if (ret != OK) {
 			log("program failed");
 			continue;
 		}
+
 		ret = verify();
+
 		if (ret != OK) {
 			log("verify failed");
 			continue;
 		}
+
 		ret = reboot();
+
 		if (ret != OK) {
 			log("reboot failed");
 			return ret;
 		}
+
 		log("update complete");
 
 		ret = OK;
@@ -145,6 +158,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 
 	/* wait 100 ms for a character */
 	int ret = ::poll(&fds[0], 1, timeout);
+
 	if (ret < 1) {
 		//log("poll timeout %d", ret);
 		return -ETIMEDOUT;
@@ -160,9 +174,11 @@ PX4IO_Uploader::recv(uint8_t *p, unsigned count)
 {
 	while (count--) {
 		int ret = recv(*p++);
+
 		if (ret != OK)
 			return ret;
 	}
+
 	return OK;
 }
 
@@ -175,7 +191,7 @@ PX4IO_Uploader::drain()
 	do {
 		ret = recv(c, 10);
 		//log("discard 0x%02x", c);
-	} while(ret == OK);
+	} while (ret == OK);
 }
 
 int
@@ -184,6 +200,7 @@ PX4IO_Uploader::send(uint8_t c)
 	//log("send 0x%02x", c);
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
+
 	return OK;
 }
 
@@ -192,9 +209,11 @@ PX4IO_Uploader::send(uint8_t *p, unsigned count)
 {
 	while (count--) {
 		int ret = send(*p++);
+
 		if (ret != OK)
 			return ret;
 	}
+
 	return OK;
 }
 
@@ -205,15 +224,20 @@ PX4IO_Uploader::get_sync(unsigned timeout)
 	int ret;
 
 	ret = recv(c[0], timeout);
+
 	if (ret != OK)
 		return ret;
+
 	ret = recv(c[1], timeout);
+
 	if (ret != OK)
 		return ret;
+
 	if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
 		log("bad sync 0x%02x,0x%02x", c[0], c[1]);
 		return -EIO;
 	}
+
 	return OK;
 }
 
@@ -221,9 +245,11 @@ int
 PX4IO_Uploader::sync()
 {
 	drain();
+
 	/* complete any pending program operation */
 	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
 		send(0);
+
 	send(PROTO_GET_SYNC);
 	send(PROTO_EOC);
 	return get_sync();
@@ -239,8 +265,10 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
 	send(PROTO_EOC);
 
 	ret = recv((uint8_t *)&val, sizeof(val));
+
 	if (ret != OK)
 		return ret;
+
 	return get_sync();
 }
 
@@ -267,10 +295,13 @@ PX4IO_Uploader::program()
 		/* get more bytes to program */
 		//log("  %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
 		count = read(_fw_fd, file_buf, sizeof(file_buf));
+
 		if (count == 0)
 			return OK;
+
 		if (count < 0)
 			return -errno;
+
 		ASSERT((count % 4) == 0);
 
 		send(PROTO_PROG_MULTI);
@@ -279,6 +310,7 @@ PX4IO_Uploader::program()
 		send(PROTO_EOC);
 
 		ret = get_sync(1000);
+
 		if (ret != OK)
 			return ret;
 	}
@@ -297,6 +329,7 @@ PX4IO_Uploader::verify()
 	send(PROTO_CHIP_VERIFY);
 	send(PROTO_EOC);
 	ret = get_sync();
+
 	if (ret != OK)
 		return ret;
 
@@ -304,19 +337,24 @@ PX4IO_Uploader::verify()
 		/* get more bytes to verify */
 		int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
 		count = read(_fw_fd, file_buf, sizeof(file_buf));
+
 		if (count == 0)
 			break;
+
 		if (count < 0)
 			return -errno;
+
 		ASSERT((count % 4) == 0);
 
 		send(PROTO_READ_MULTI);
 		send(count);
 		send(PROTO_EOC);
+
 		for (ssize_t i = 0; i < count; i++) {
 			uint8_t c;
 
 			ret = recv(c);
+
 			if (ret != OK) {
 				log("%d: got %d waiting for bytes", base + i, ret);
 				return ret;
@@ -327,12 +365,15 @@ PX4IO_Uploader::verify()
 				return -EINVAL;
 			}
 		}
+
 		ret = get_sync();
+
 		if (ret != OK) {
 			log("timeout waiting for post-verify sync");
 			return ret;
 		}
 	}
+
 	return OK;
 }
 
@@ -358,6 +399,7 @@ PX4IO_Uploader::compare(bool &identical)
 	send(PROTO_CHIP_VERIFY);
 	send(PROTO_EOC);
 	ret = get_sync();
+
 	if (ret != OK)
 		return ret;
 
@@ -365,6 +407,7 @@ PX4IO_Uploader::compare(bool &identical)
 	send(sizeof(fw_vectors));
 	send(PROTO_EOC);
 	ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
+
 	if (ret != OK)
 		return ret;
 
diff --git a/apps/drivers/stm32/Makefile b/apps/drivers/stm32/Makefile
new file mode 100644
index 000000000..4ad57f413
--- /dev/null
+++ b/apps/drivers/stm32/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# STM32 driver support code
+#
+# Modules in this directory are compiled for all STM32 targets.
+#
+
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c
new file mode 100644
index 000000000..1ac90b16d
--- /dev/null
+++ b/apps/drivers/stm32/drv_hrt.c
@@ -0,0 +1,858 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_hrt.c
+ *
+ * High-resolution timer callouts and timekeeping.
+ *
+ * This can use any general or advanced STM32 timer.
+ *
+ * Note that really, this could use systick too, but that's
+ * monopolised by NuttX and stealing it would just be awkward.
+ *
+ * We don't use the NuttX STM32 driver per se; rather, we
+ * claim the timer and then drive it directly.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+#ifdef CONFIG_HRT_TIMER
+
+/* HRT configuration */
+#if   HRT_TIMER == 1
+# define HRT_TIMER_BASE		STM32_TIM1_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
+# if CONFIG_STM32_TIM1
+#  error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
+# endif
+#elif HRT_TIMER == 2
+# define HRT_TIMER_BASE		STM32_TIM2_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM2
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
+# if CONFIG_STM32_TIM2
+#  error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2
+# endif
+#elif HRT_TIMER == 3
+# define HRT_TIMER_BASE		STM32_TIM3_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM3EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM3
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
+# if CONFIG_STM32_TIM3
+#  error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3
+# endif
+#elif HRT_TIMER == 4
+# define HRT_TIMER_BASE		STM32_TIM4_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM4EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM4
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
+# if CONFIG_STM32_TIM4
+#  error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4
+# endif
+#elif HRT_TIMER == 5
+# define HRT_TIMER_BASE		STM32_TIM5_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM5
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
+# if CONFIG_STM32_TIM5
+#  error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5
+# endif
+#elif HRT_TIMER == 8
+# define HRT_TIMER_BASE		STM32_TIM8_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM8CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
+# if CONFIG_STM32_TIM8
+#  error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6
+# endif
+#elif HRT_TIMER == 9
+# define HRT_TIMER_BASE		STM32_TIM9_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1BRK
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
+# if CONFIG_STM32_TIM9
+#  error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9
+# endif
+#elif HRT_TIMER == 10
+# define HRT_TIMER_BASE		STM32_TIM10_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1UP
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM10_CLKIN
+# if CONFIG_STM32_TIM10
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
+# endif
+#elif HRT_TIMER == 11
+# define HRT_TIMER_BASE		STM32_TIM11_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM11_CLKIN
+# if CONFIG_STM32_TIM11
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
+# endif
+#else
+# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
+#endif
+
+/*
+ * HRT clock must be a multiple of 1MHz greater than 1MHz
+ */
+#if (HRT_TIMER_CLOCK % 1000000) != 0
+# error HRT_TIMER_CLOCK must be a multiple of 1MHz
+#endif
+#if HRT_TIMER_CLOCK <= 1000000
+# error HRT_TIMER_CLOCK must be greater than 1MHz
+#endif
+
+/*
+ * Minimum/maximum deadlines.
+ *
+ * These are suitable for use with a 16-bit timer/counter clocked
+ * at 1MHz.  The high-resolution timer need only guarantee that it
+ * not wrap more than once in the 50ms period for absolute time to
+ * be consistently maintained.
+ *
+ * The minimum deadline must be such that the time taken between
+ * reading a time and writing a deadline to the timer cannot
+ * result in missing the deadline.
+ */
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
+/*
+ * Period of the free-running counter, in microseconds.
+ */
+#define HRT_COUNTER_PERIOD	65536
+
+/*
+ * Scaling factor(s) for the free-running counter; convert an input
+ * in counts to a time in microseconds.
+ */
+#define HRT_COUNTER_SCALE(_c)	(_c)
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if HRT_TIMER_CHANNEL == 1
+# define rCCR_HRT	rCCR1			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC1IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC1IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 2
+# define rCCR_HRT	rCCR2			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC2IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC2IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 3
+# define rCCR_HRT	rCCR3			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC3IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC3IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 4
+# define rCCR_HRT	rCCR4			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC4IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC4IF		/* interrupt status for HRT */
+#else
+# error HRT_TIMER_CHANNEL must be a value between 1 and 4
+#endif
+
+/*
+ * Queue of callout entries.
+ */
+static struct sq_queue_s	callout_queue;
+
+/* latency baseline (last compare value applied) */
+static uint16_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint16_t			latency_actual;
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT	8
+static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+/* timer-specific functions */
+static void		hrt_tim_init(void);
+static int		hrt_tim_isr(int irq, void *context);
+static void		hrt_latency_update(void);
+
+/* callout list manipulation */
+static void		hrt_call_internal(struct hrt_call *entry,
+		hrt_abstime deadline,
+		hrt_abstime interval,
+		hrt_callout callout,
+		void *arg);
+static void		hrt_call_enter(struct hrt_call *entry);
+static void		hrt_call_reschedule(void);
+static void		hrt_call_invoke(void);
+
+/*
+ * Specific registers and bits used by PPM sub-functions
+ */
+#ifdef CONFIG_HRT_PPM
+# if HRT_PPM_CHANNEL == 1
+#  define rCCR_PPM	rCCR1			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC1IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC1IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC1OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	1			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
+# elif HRT_PPM_CHANNEL == 2
+#  define rCCR_PPM	rCCR2			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC2IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC2IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC2OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	2			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
+# elif HRT_PPM_CHANNEL == 3
+#  define rCCR_PPM	rCCR3			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC3IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC3IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC3OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	1			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
+# elif HRT_PPM_CHANNEL == 4
+#  define rCCR_PPM	rCCR4			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC4IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC4IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC4OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	2			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
+# else
+#  error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
+# endif
+
+/*
+ * PPM decoder tuning parameters
+ */
+# define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+# define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* decoded PPM buffer */
+#define PPM_MAX_CHANNELS	12
+__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT unsigned ppm_decoded_channels;
+__EXPORT uint64_t ppm_last_valid_decode = 0;
+
+/* PPM edge history */
+__EXPORT uint16_t ppm_edge_history[32];
+unsigned ppm_edge_next;
+
+/* PPM pulse history */
+__EXPORT uint16_t ppm_pulse_history[32];
+unsigned ppm_pulse_next;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+static void	hrt_ppm_decode(uint32_t status);
+
+#else
+/* disable the PPM configuration */
+# define rCCR_PPM	0
+# define DIER_PPM	0
+# define SR_INT_PPM	0
+# define SR_OVF_PPM	0
+# define CCMR1_PPM	0
+# define CCMR2_PPM	0
+# define CCER_PPM	0
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Initialise the timer we are going to use.
+ *
+ * We expect that we'll own one of the reduced-function STM32 general
+ * timers, and that we can use channel 1 in compare mode.
+ */
+static void
+hrt_tim_init(void)
+{
+	/* clock/power on our timer */
+	modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
+
+	/* claim our interrupt vector */
+	irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
+
+	/* disable and configure the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = DIER_HRT | DIER_PPM;
+	rCCER = 0;		/* unlock CCMR* registers */
+	rCCMR1 = CCMR1_PPM;
+	rCCMR2 = CCMR2_PPM;
+	rCCER = CCER_PPM;
+	rDCR = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC = (HRT_TIMER_CLOCK / 1000000) - 1;	/* this really only works for whole-MHz clocks */
+
+	/* run the full span of the counter */
+	rARR = 0xffff;
+
+	/* set an initial capture a little ways off */
+	rCCR_HRT = 1000;
+
+	/* generate an update event; reloads the counter, all registers */
+	rEGR = GTIM_EGR_UG;
+
+	/* enable the timer */
+	rCR1 = GTIM_CR1_CEN;
+
+	/* enable interrupts */
+	up_enable_irq(HRT_TIMER_VECTOR);
+}
+
+#ifdef CONFIG_HRT_PPM
+/*
+ * Handle the PPM decoder state machine.
+ */
+static void
+hrt_ppm_decode(uint32_t status)
+{
+	uint16_t count = rCCR_PPM;
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (status & SR_OVF_PPM)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+	ppm.last_edge = count;
+
+	ppm_edge_history[ppm_edge_next++] = width;
+
+	if (ppm_edge_next >= 32)
+		ppm_edge_next = 0;
+
+	/*
+	 * if this looks like a start pulse, then push the last set of values
+	 * and reset the state machine
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/* export the last set of samples if we got something sensible */
+		if (ppm.next_channel > 4) {
+			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
+				ppm_buffer[i] = ppm_temp_buffer[i];
+
+			ppm_decoded_channels = i;
+			ppm_last_valid_decode = hrt_absolute_time();
+
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+		return;
+
+	case ACTIVE:
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		ppm_pulse_history[ppm_pulse_next++] = interval;
+
+		if (ppm_pulse_next >= 32)
+			ppm_pulse_next = 0;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+
+}
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Handle the compare interupt by calling the callout dispatcher
+ * and then re-scheduling the next deadline.
+ */
+static int
+hrt_tim_isr(int irq, void *context)
+{
+	uint32_t status;
+
+	/* grab the timer for latency tracking purposes */
+	latency_actual = rCNT;
+
+	/* copy interrupt status */
+	status = rSR;
+
+	/* ack the interrupts we just read */
+	rSR = ~status;
+
+#ifdef CONFIG_HRT_PPM
+
+	/* was this a PPM edge? */
+	if (status & (SR_INT_PPM | SR_OVF_PPM))
+		hrt_ppm_decode(status);
+
+#endif
+
+	/* was this a timer tick? */
+	if (status & SR_INT_HRT) {
+
+		/* do latency calculations */
+		hrt_latency_update();
+
+		/* run any callouts that have met their deadline */
+		hrt_call_invoke();
+
+		/* and schedule the next interrupt */
+		hrt_call_reschedule();
+	}
+
+	return OK;
+}
+
+/*
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime	abstime;
+	uint32_t	count;
+	uint32_t	flags;
+
+	/*
+	 * Counter state.  Marked volatile as they may change
+	 * inside this routine but outside the irqsave/restore
+	 * pair.  Discourage the compiler from moving loads/stores
+	 * to these outside of the protected range.
+	 */
+	static volatile hrt_abstime base_time;
+	static volatile uint32_t last_count;
+
+	/* prevent re-entry */
+	flags = irqsave();
+
+	/* get the current counter value */
+	count = rCNT;
+
+	/*
+	 * Determine whether the counter has wrapped since the
+	 * last time we're called.
+	 *
+	 * This simple test is sufficient due to the guarantee that
+	 * we are always called at least once per counter period.
+	 */
+	if (count < last_count)
+		base_time += HRT_COUNTER_PERIOD;
+
+	/* save the count for next time */
+	last_count = count;
+
+	/* compute the current time */
+	abstime = HRT_COUNTER_SCALE(base_time + count);
+
+	irqrestore(flags);
+
+	return abstime;
+}
+
+/*
+ * Convert a timespec to absolute time
+ */
+hrt_abstime
+ts_to_abstime(struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/*
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/*
+ * Initalise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	sq_init(&callout_queue);
+	hrt_tim_init();
+
+#ifdef CONFIG_HRT_PPM
+	/* configure the PPM input pin */
+	stm32_configgpio(GPIO_PPM_IN);
+#endif
+}
+
+/*
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+/*
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	irqstate_t flags = irqsave();
+
+	/* if the entry is currently queued, remove it */
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+
+	irqrestore(flags);
+}
+
+/*
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	irqstate_t flags = irqsave();
+
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+
+	irqrestore(flags);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+static void
+hrt_call_invoke(void)
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	while (true) {
+		/* get the current time */
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == NULL)
+			break;
+
+		if (call->deadline > now)
+			break;
+
+		sq_rem(&call->link, &callout_queue);
+		//lldbg("call pop\n");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline;
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* invoke the callout (if there is one) */
+		if (call->callout) {
+			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			call->callout(call->arg);
+		}
+
+		/* if the callout has a non-zero period, it has to be re-entered */
+		if (call->period != 0) {
+			call->deadline = deadline + call->period;
+			hrt_call_enter(call);
+		}
+	}
+}
+
+/*
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+
+	//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
+
+	/* set the new compare value and remember it for latency tracking */
+	rCCR_HRT = latency_baseline = deadline & 0xffff;
+}
+
+static void
+hrt_latency_update(void)
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	/* bounded buckets */
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	/* catch-all at the end */
+	latency_counters[index]++;
+}
+
+
+#endif /* CONFIG_HRT_TIMER */
diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c
new file mode 100644
index 000000000..e3801a417
--- /dev/null
+++ b/apps/drivers/stm32/drv_pwm_servo.c
@@ -0,0 +1,308 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file drv_pwm_servo.c
+ *
+ * Servo driver supporting PWM servos connected to STM32 timer blocks.
+ *
+ * Works with any of the 'generic' or 'advanced' STM32 timers that
+ * have output pins, does not require an interrupt.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "drv_pwm_servo.h"
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+
+/* default rate (in Hz) of PWM updates */
+static uint32_t	pwm_update_rate = 50;
+
+#define REG(_tmr, _reg)	(*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
+
+#define rCR1(_tmr)    	REG(_tmr, STM32_GTIM_CR1_OFFSET)
+#define rCR2(_tmr)    	REG(_tmr, STM32_GTIM_CR2_OFFSET)
+#define rSMCR(_tmr)   	REG(_tmr, STM32_GTIM_SMCR_OFFSET)
+#define rDIER(_tmr)   	REG(_tmr, STM32_GTIM_DIER_OFFSET)
+#define rSR(_tmr)     	REG(_tmr, STM32_GTIM_SR_OFFSET)
+#define rEGR(_tmr)    	REG(_tmr, STM32_GTIM_EGR_OFFSET)
+#define rCCMR1(_tmr)  	REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2(_tmr)  	REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
+#define rCCER(_tmr)   	REG(_tmr, STM32_GTIM_CCER_OFFSET)
+#define rCNT(_tmr)    	REG(_tmr, STM32_GTIM_CNT_OFFSET)
+#define rPSC(_tmr)    	REG(_tmr, STM32_GTIM_PSC_OFFSET)
+#define rARR(_tmr)    	REG(_tmr, STM32_GTIM_ARR_OFFSET)
+#define rCCR1(_tmr)   	REG(_tmr, STM32_GTIM_CCR1_OFFSET)
+#define rCCR2(_tmr)   	REG(_tmr, STM32_GTIM_CCR2_OFFSET)
+#define rCCR3(_tmr)   	REG(_tmr, STM32_GTIM_CCR3_OFFSET)
+#define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
+#define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
+#define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+
+static void
+pwm_timer_init(unsigned timer)
+{
+	/* enable the timer clock before we try to talk to it */
+	modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit);
+
+	/* disable and configure the timer */
+	rCR1(timer) = 0;
+	rCR2(timer) = 0;
+	rSMCR(timer) = 0;
+	rDIER(timer) = 0;
+	rCCER(timer) = 0;
+	rCCMR1(timer) = 0;
+	rCCMR2(timer) = 0;
+	rCCER(timer) = 0;
+	rDCR(timer) = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
+
+	/* and update at the desired rate */
+	rARR(timer) = (1000000 / pwm_update_rate) - 1;
+
+	/* generate an update event; reloads the counter and all registers */
+	rEGR(timer) = GTIM_EGR_UG;
+
+	/* note that the timer is left disabled - arming is performed separately */
+}
+
+static void
+pwm_timer_set_rate(unsigned timer, unsigned rate)
+{
+	/* configure the timer to update at the desired rate */
+	rARR(timer) = 1000000 / rate;
+
+	/* generate an update event; reloads the counter and all registers */
+	rEGR(timer) = GTIM_EGR_UG;
+}
+
+static void
+pwm_channel_init(unsigned channel)
+{
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* configure the GPIO first */
+	stm32_configgpio(pwm_channels[channel].gpio);
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		rCCMR1(timer) |= (6 << 4);
+		rCCR1(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= (1 << 0);
+		break;
+
+	case 2:
+		rCCMR1(timer) |= (6 << 12);
+		rCCR2(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= (1 << 4);
+		break;
+
+	case 3:
+		rCCMR2(timer) |= (6 << 4);
+		rCCR3(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= (1 << 8);
+		break;
+
+	case 4:
+		rCCMR2(timer) |= (6 << 12);
+		rCCR4(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= (1 << 12);
+		break;
+	}
+}
+
+int
+up_pwm_servo_set(unsigned channel, servo_position_t value)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS) {
+		lldbg("pwm_channel_set: bogus channel %u\n", channel);
+		return -1;
+	}
+
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].gpio == 0))
+		return -1;
+
+	/* configure the channel */
+	if (value > 0)
+		value--;
+
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		rCCR1(timer) = value;
+		break;
+
+	case 2:
+		rCCR2(timer) = value;
+		break;
+
+	case 3:
+		rCCR3(timer) = value;
+		break;
+
+	case 4:
+		rCCR4(timer) = value;
+		break;
+
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+servo_position_t
+up_pwm_servo_get(unsigned channel)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS) {
+		lldbg("pwm_channel_get: bogus channel %u\n", channel);
+		return 0;
+	}
+
+	unsigned timer = pwm_channels[channel].timer_index;
+	servo_position_t value = 0;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].gpio == 0))
+		return 0;
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		value = rCCR1(timer);
+		break;
+
+	case 2:
+		value = rCCR2(timer);
+		break;
+
+	case 3:
+		value = rCCR3(timer);
+		break;
+
+	case 4:
+		value = rCCR4(timer);
+		break;
+	}
+
+	return value;
+}
+
+int
+up_pwm_servo_init(uint32_t channel_mask)
+{
+	/* do basic timer initialisation first */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			pwm_timer_init(i);
+	}
+
+	/* now init channels */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		/* don't do init for disabled channels; this leaves the pin configs alone */
+		if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0))
+			pwm_channel_init(i);
+	}
+
+	return OK;
+}
+
+void
+up_pwm_servo_deinit(void)
+{
+	/* disable the timers */
+	up_pwm_servo_arm(false);
+}
+
+int
+up_pwm_servo_set_rate(unsigned rate)
+{
+	if ((rate < 50) || (rate > 400))
+		return -ERANGE;
+
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			pwm_timer_set_rate(i, rate);
+	}
+
+	return OK;
+}
+
+void
+up_pwm_servo_arm(bool armed)
+{
+	/*
+	 * XXX this is inelgant and in particular will either jam outputs at whatever level
+	 * they happen to be at at the time the timers stop or generate runts.
+	 * The right thing is almost certainly to kill auto-reload on the timers so that
+	 * they just stop at the end of their count for disable, and to reset/restart them
+	 * for enable.
+	 */
+
+	/* iterate timers and arm/disarm appropriately */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			rCR1(i) = armed ? GTIM_CR1_CEN : 0;
+	}
+}
diff --git a/apps/drivers/stm32/drv_pwm_servo.h b/apps/drivers/stm32/drv_pwm_servo.h
new file mode 100644
index 000000000..667283424
--- /dev/null
+++ b/apps/drivers/stm32/drv_pwm_servo.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_pwm_servo.h
+ *
+ * stm32-specific PWM output data.
+ */
+
+#pragma once
+
+#include <drivers/drv_pwm_output.h>
+
+/* configuration limits */
+#define PWM_SERVO_MAX_TIMERS	2
+#define PWM_SERVO_MAX_CHANNELS	8
+
+/* array of timers dedicated to PWM servo use */
+struct pwm_servo_timer {
+	uint32_t	base;
+	uint32_t	clock_register;
+	uint32_t	clock_bit;
+	uint32_t	clock_freq;
+};
+
+/* supplied by board-specific code */
+__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
+
+/* array of channels in logical order */
+struct pwm_servo_channel {
+	uint32_t	gpio;
+	uint8_t		timer_index;
+	uint8_t		timer_channel;
+	servo_position_t default_value;
+};
+
+__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];
diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
index bfde83cde..03cf3ee5d 100644
--- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -34,8 +34,8 @@
 /**
  * Driver for the PX4 audio alarm port, /dev/tone_alarm.
  *
- * The tone_alarm driver supports a set of predefined "alarm" 
- * patterns and one user-supplied pattern.  Patterns are ordered by 
+ * The tone_alarm driver supports a set of predefined "alarm"
+ * patterns and one user-supplied pattern.  Patterns are ordered by
  * priority, with a higher-priority pattern interrupting any
  * lower-priority pattern that might be playing.
  *
@@ -71,7 +71,7 @@
 #include <unistd.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
 #include <up_internal.h>
@@ -244,7 +244,8 @@ const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = {
 	{{TONE_NOTE_C7, 100}},
 	{{TONE_NOTE_D7, 100}},
 	{{TONE_NOTE_E7, 100}},
-	{	//This is tetris ;)
+	{
+		//This is tetris ;)
 		{TONE_NOTE_C6, 40},
 		{TONE_NOTE_G5, 20},
 		{TONE_NOTE_G5S, 20},
@@ -361,6 +362,7 @@ ToneAlarm::init()
 	int ret;
 
 	ret = CDev::init();
+
 	if (ret != OK)
 		return ret;
 
@@ -368,34 +370,34 @@ ToneAlarm::init()
 	stm32_configgpio(GPIO_TONE_ALARM);
 
 	/* clock/power on our timer */
-        modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
+	modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
 
 	/* initialise the timer */
-        rCR1 = 0;
-        rCR2 = 0;
-        rSMCR = 0;
-        rDIER = 0;
-        rCCER &= TONE_CCER;		/* unlock CCMR* registers */
-        rCCMR1 = TONE_CCMR1;
-        rCCMR2 = TONE_CCMR2;
-        rCCER = TONE_CCER;
-        rDCR = 0;
-
-        /* toggle the CC output each time the count passes 1 */
-        TONE_rCCR = 1;
-
-        /* 
-         * Configure the timebase to free-run at half max frequency.
-         * XXX this should be more flexible in order to get a better
-         * frequency range, but for the F4 with the APB1 timers based
-         * at 42MHz, this gets us down to ~320Hz or so.
-         */
-        rPSC = 1;
-
-        /* make sure the timer is running */
-        rCR1 = GTIM_CR1_CEN;
-
-        debug("ready");
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = 0;
+	rCCER &= TONE_CCER;		/* unlock CCMR* registers */
+	rCCMR1 = TONE_CCMR1;
+	rCCMR2 = TONE_CCMR2;
+	rCCER = TONE_CCER;
+	rDCR = 0;
+
+	/* toggle the CC output each time the count passes 1 */
+	TONE_rCCR = 1;
+
+	/*
+	 * Configure the timebase to free-run at half max frequency.
+	 * XXX this should be more flexible in order to get a better
+	 * frequency range, but for the F4 with the APB1 timers based
+	 * at 42MHz, this gets us down to ~320Hz or so.
+	 */
+	rPSC = 1;
+
+	/* make sure the timer is running */
+	rCR1 = GTIM_CR1_CEN;
+
+	debug("ready");
 	return OK;
 }
 
@@ -413,6 +415,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 	case TONE_SET_ALARM:
 		debug("TONE_SET_ALARM %u", arg);
+
 		if (new_pattern == 0) {
 			/* cancel any current alarm */
 			_current_pattern = _pattern_none;
@@ -431,10 +434,13 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 
 			/* and start playing it */
 			next();
+
 		} else {
 			/* current pattern is higher priority than the new pattern, ignore */
 		}
+
 		break;
+
 	default:
 		result = -ENOTTY;
 		break;
@@ -457,8 +463,10 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
 	/* sanity-check the size of the write */
 	if (len > (_max_pattern_len * sizeof(tone_note)))
 		return -EFBIG;
+
 	if ((len % sizeof(tone_note)) || (len == 0))
 		return -EIO;
+
 	if (!check((tone_note *)buffer))
 		return -EIO;
 
@@ -479,6 +487,7 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
 		debug("starting user pattern");
 		next();
 	}
+
 	irqrestore(flags);
 
 	return len;
@@ -511,18 +520,22 @@ ToneAlarm::next(void)
 	/* find the note to play */
 	if (_current_pattern == _pattern_user) {
 		np = &_user_pattern[_next_note];
+
 	} else {
 		np = &_patterns[_current_pattern - 1][_next_note];
 	}
 
 	/* work out which note is next */
 	_next_note++;
+
 	if (_next_note >= _note_max) {
 		/* hit the end of the pattern, stop */
 		_current_pattern = _pattern_none;
+
 	} else if (np[1].duration == DURATION_END) {
 		/* hit the end of the pattern, stop */
 		_current_pattern = _pattern_none;
+
 	} else if (np[1].duration == DURATION_REPEAT) {
 		/* next note is a repeat, rewind in preparation */
 		_next_note = 0;
@@ -534,11 +547,11 @@ ToneAlarm::next(void)
 		/* set reload based on the pitch */
 		rARR = _notes[np->pitch];
 
-	        /* force an update, reloads the counter and all registers */
-	        rEGR = GTIM_EGR_UG;
+		/* force an update, reloads the counter and all registers */
+		rEGR = GTIM_EGR_UG;
 
-	        /* enable the output */
-	        rCCER |= TONE_CCER;
+		/* enable the output */
+		rCCER |= TONE_CCER;
 	}
 
 	/* arrange a callback when the note/rest is done */
@@ -554,6 +567,7 @@ ToneAlarm::check(tone_note *pattern)
 		if ((i == 0) &&
 		    ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT)))
 			return false;
+
 		if (pattern[i].duration == DURATION_END)
 			break;
 
@@ -561,6 +575,7 @@ ToneAlarm::check(tone_note *pattern)
 		if (pattern[i].pitch >= _note_max)
 			return false;
 	}
+
 	return true;
 }
 
@@ -592,13 +607,16 @@ play_pattern(unsigned pattern)
 	int	fd, ret;
 
 	fd = open("/dev/tone_alarm", 0);
+
 	if (fd < 0)
 		err(1, "/dev/tone_alarm");
 
 	warnx("playing pattern %u", pattern);
 	ret = ioctl(fd, TONE_SET_ALARM, pattern);
+
 	if (ret != 0)
 		err(1, "TONE_SET_ALARM");
+
 	exit(0);
 }
 
@@ -615,6 +633,7 @@ tone_alarm_main(int argc, char *argv[])
 
 		if (g_dev == nullptr)
 			errx(1, "couldn't allocate the ToneAlarm driver");
+
 		if (g_dev->init() != OK) {
 			delete g_dev;
 			errx(1, "ToneAlarm init failed");
@@ -623,8 +642,10 @@ tone_alarm_main(int argc, char *argv[])
 
 	if ((argc > 1) && !strcmp(argv[1], "start"))
 		play_pattern(1);
+
 	if ((argc > 1) && !strcmp(argv[1], "stop"))
 		play_pattern(0);
+
 	if ((pattern = strtol(argv[1], nullptr, 10)) != 0)
 		play_pattern(pattern);
 
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index 8629c2f11..e04033481 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -48,7 +48,7 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index 2e90f50b1..374280d28 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -43,7 +43,7 @@
 #include <pthread.h>
 #include <poll.h>
 #include <fcntl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>
diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c
index d1c9d364b..577a3a01c 100644
--- a/apps/gps/nmea_helper.c
+++ b/apps/gps/nmea_helper.c
@@ -44,7 +44,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
 #define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 2632aebab..f029a9e37 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -40,7 +40,7 @@
 #include "gps.h"
 #include <sys/prctl.h>
 #include <poll.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <string.h>
 #include <stdbool.h>
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 235f5f8f3..7d8232b3a 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -50,7 +50,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 550746794..cfff0f469 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -49,7 +49,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c
index d2be9a88d..50282a9e3 100644
--- a/apps/mavlink/missionlib.c
+++ b/apps/mavlink/missionlib.c
@@ -49,7 +49,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 0b073cc65..90b0073cf 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -46,7 +46,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index dfb778fd0..0cdb53554 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -54,7 +54,7 @@
 #include <math.h>
 #include <poll.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
 #include <uORB/topics/vehicle_status.h>
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 8c0e10fc3..eb94cca8a 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -52,7 +52,7 @@
 #include <math.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/param/param.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 42aa0ac63..a5bff97e6 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -53,7 +53,7 @@
 #include <systemlib/pid/pid.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c
index 7cf687306..474ced731 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control.c
+++ b/apps/multirotor_pos_control/multirotor_pos_control.c
@@ -48,7 +48,7 @@
 #include <termios.h>
 #include <time.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index 45267f315..60737a654 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -46,7 +46,7 @@
 #include <sys/time.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <string.h>
 #include <poll.h>
 #include <uORB/uORB.h>
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 6d990c46b..eccdeb78e 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -61,8 +61,6 @@
 #include <systemlib/mixer/mixer.h>
 #include <drivers/drv_mixer.h>
 
-#include <arch/board/up_pwm_servo.h>
-
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 
diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c
index 8c6951b63..f364ea080 100644
--- a/apps/px4/tests/test_hrt.c
+++ b/apps/px4/tests/test_hrt.c
@@ -50,7 +50,7 @@
 #include <unistd.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 
 #include <nuttx/spi.h>
diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c
index c128c73a3..25bf02c31 100644
--- a/apps/px4/tests/test_time.c
+++ b/apps/px4/tests/test_time.c
@@ -53,7 +53,7 @@
 
 #include <math.h>
 #include <float.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 
 /****************************************************************************
diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c
index de1249b8c..fc5b03036 100644
--- a/apps/px4/tests/test_uart_console.c
+++ b/apps/px4/tests/test_uart_console.c
@@ -56,7 +56,7 @@
 
 #include <math.h>
 #include <float.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 
 /****************************************************************************
diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c
index 83d205440..a88e617d9 100644
--- a/apps/px4/tests/test_uart_send.c
+++ b/apps/px4/tests/test_uart_send.c
@@ -56,7 +56,7 @@
 
 #include <math.h>
 #include <float.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 
 /****************************************************************************
diff --git a/apps/px4/tests/tests_file.c b/apps/px4/tests/tests_file.c
index 2cff622f7..697410cee 100644
--- a/apps/px4/tests/tests_file.c
+++ b/apps/px4/tests/tests_file.c
@@ -44,7 +44,7 @@
 #include <systemlib/perf_counter.h>
 #include <string.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "tests.h"
 
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 19802bf4f..f9106d830 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -48,7 +48,7 @@
 
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 9dc1fdcba..a91aad3ca 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -47,7 +47,7 @@
 
 #include <arch/board/drv_ppm_input.h>
 #include <arch/board/drv_pwm_servo.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "px4io.h"
 
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 30a62fa65..a67db9a7e 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -50,7 +50,7 @@
 #include <arch/board/up_boardinitialize.h>
 #include <arch/board/drv_gpio.h>
 #include <arch/board/drv_ppm_input.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "px4io.h"
 
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 895c33806..6cb85798f 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -49,7 +49,7 @@
 #include <arch/board/up_boardinitialize.h>
 #include <arch/board/drv_gpio.h>
 #include <arch/board/drv_ppm_input.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "px4io.h"
 
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index 36225386c..312768095 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -51,7 +51,7 @@
 #include <string.h>
 #include <systemlib/err.h>
 #include <unistd.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 3e204662a..54d2f6a0b 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -52,15 +52,13 @@
 #include <errno.h>
 #include <math.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
 
-#include <arch/board/up_adc.h>
-
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c
index 752c01986..745f76852 100644
--- a/apps/systemcmds/bl_update/bl_update.c
+++ b/apps/systemcmds/bl_update/bl_update.c
@@ -67,10 +67,12 @@ bl_update_main(int argc, char *argv[])
 		setopt();
 
 	int fd = open(argv[1], O_RDONLY);
+
 	if (fd < 0)
 		err(1, "open %s", argv[1]);
 
 	struct stat s;
+
 	if (stat(argv[1], &s) < 0)
 		err(1, "stat %s", argv[1]);
 
@@ -79,14 +81,17 @@ bl_update_main(int argc, char *argv[])
 		errx(1, "%s: file too large", argv[1]);
 
 	uint8_t *buf = malloc(s.st_size);
+
 	if (buf == NULL)
 		errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size);
 
 	if (read(fd, buf, s.st_size) != s.st_size)
 		err(1, "firmware read error");
+
 	close(fd);
 
 	uint32_t *hdr = (uint32_t *)buf;
+
 	if ((hdr[0] < 0x20000000) ||			/* stack not below RAM */
 	    (hdr[0] > (0x20000000 + (128 * 1024))) ||	/* stack not above RAM */
 	    (hdr[1] < 0x08000000) ||			/* entrypoint not below flash */
@@ -123,6 +128,7 @@ bl_update_main(int argc, char *argv[])
 	/* wait for the operation to complete */
 	while (*sr & 0x1000) {
 	}
+
 	if (*sr & 0xf2) {
 		warnx("WARNING: erase error 0x%02x", *sr);
 		goto flash_end;
@@ -148,6 +154,7 @@ bl_update_main(int argc, char *argv[])
 		/* wait for the operation to complete */
 		while (*sr & 0x1000) {
 		}
+
 		if (*sr & 0xf2) {
 			warnx("WARNING: program error 0x%02x", *sr);
 			goto flash_end;
@@ -203,6 +210,7 @@ setopt(void)
 
 	if ((*optcr & opt_mask) == opt_bits)
 		errx(0, "option bits set");
+
 	errx(1, "option bits setting failed; readback 0x%04x", *optcr);
 
 }
\ No newline at end of file
diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c
index 3cded52fa..79149caa0 100644
--- a/apps/systemcmds/eeprom/24xxxx_mtd.c
+++ b/apps/systemcmds/eeprom/24xxxx_mtd.c
@@ -135,20 +135,19 @@
  * cast between pointers to struct mtd_dev_s and struct at24c_dev_s.
  */
 
-struct at24c_dev_s
-{
-  struct mtd_dev_s      mtd;      /* MTD interface */
-  FAR struct i2c_dev_s *dev;      /* Saved I2C interface instance */
-  uint8_t               addr;     /* I2C address */
-  uint16_t              pagesize; /* 32, 63 */
-  uint16_t              npages;   /* 128, 256, 512, 1024 */
-
-  perf_counter_t        perf_reads;
-  perf_counter_t        perf_writes;
-  perf_counter_t        perf_resets;
-  perf_counter_t        perf_read_retries;
-  perf_counter_t        perf_read_errors;
-  perf_counter_t        perf_write_errors;
+struct at24c_dev_s {
+	struct mtd_dev_s      mtd;      /* MTD interface */
+	FAR struct i2c_dev_s *dev;      /* Saved I2C interface instance */
+	uint8_t               addr;     /* I2C address */
+	uint16_t              pagesize; /* 32, 63 */
+	uint16_t              npages;   /* 128, 256, 512, 1024 */
+
+	perf_counter_t        perf_reads;
+	perf_counter_t        perf_writes;
+	perf_counter_t        perf_resets;
+	perf_counter_t        perf_read_retries;
+	perf_counter_t        perf_read_errors;
+	perf_counter_t        perf_write_errors;
 };
 
 /************************************************************************************
@@ -159,9 +158,9 @@ struct at24c_dev_s
 
 static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks);
 static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
-                           size_t nblocks, FAR uint8_t *buf);
+			   size_t nblocks, FAR uint8_t *buf);
 static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
-                            size_t nblocks, FAR const uint8_t *buf);
+			    size_t nblocks, FAR const uint8_t *buf);
 static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
 
 /************************************************************************************
@@ -180,35 +179,32 @@ static struct at24c_dev_s g_at24c;
 
 static int at24c_eraseall(FAR struct at24c_dev_s *priv)
 {
-  int startblock = 0;
-  uint8_t buf[AT24XX_PAGESIZE + 2];
-
-  struct i2c_msg_s msgv[1] = 
-    {
-      {
-        .addr = priv->addr,
-        .flags = 0,
-        .buffer = &buf[0],
-        .length = sizeof(buf),
-      }
-    };
-
-  memset(&buf[2],0xff,priv->pagesize);
-
-  for (startblock = 0; startblock < priv->npages; startblock++)
-    {
-      uint16_t offset = startblock * priv->pagesize;
-      buf[1] = offset & 0xff;
-      buf[0] = (offset >> 8) & 0xff;
-
-      while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0)
-        { 
-          fvdbg("erase stall\n");
-          usleep(10000);
-        }
-    }
-
-  return OK;
+	int startblock = 0;
+	uint8_t buf[AT24XX_PAGESIZE + 2];
+
+	struct i2c_msg_s msgv[1] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &buf[0],
+			.length = sizeof(buf),
+		}
+	};
+
+	memset(&buf[2], 0xff, priv->pagesize);
+
+	for (startblock = 0; startblock < priv->npages; startblock++) {
+		uint16_t offset = startblock * priv->pagesize;
+		buf[1] = offset & 0xff;
+		buf[0] = (offset >> 8) & 0xff;
+
+		while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) {
+			fvdbg("erase stall\n");
+			usleep(10000);
+		}
+	}
+
+	return OK;
 }
 
 /************************************************************************************
@@ -217,9 +213,9 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv)
 
 static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks)
 {
-  /* EEprom need not erase */
+	/* EEprom need not erase */
 
-  return (int)nblocks;
+	return (int)nblocks;
 }
 
 /************************************************************************************
@@ -227,90 +223,86 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc
  ************************************************************************************/
 
 static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
-                           size_t nblocks, FAR uint8_t *buffer)
+			   size_t nblocks, FAR uint8_t *buffer)
 {
-  FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
-  size_t blocksleft;
-  uint8_t addr[2];
-  int ret;
-
-  struct i2c_msg_s msgv[2] = 
-    {
-      {
-        .addr = priv->addr,
-        .flags = 0,
-        .buffer = &addr[0],
-        .length = sizeof(addr),
-      },
-      { 
-        .addr = priv->addr,
-        .flags = I2C_M_READ,
-        .buffer = 0,
-        .length = priv->pagesize,
-      }
-    };
+	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
+	size_t blocksleft;
+	uint8_t addr[2];
+	int ret;
+
+	struct i2c_msg_s msgv[2] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &addr[0],
+			.length = sizeof(addr),
+		},
+		{
+			.addr = priv->addr,
+			.flags = I2C_M_READ,
+			.buffer = 0,
+			.length = priv->pagesize,
+		}
+	};
 
 #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-  startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-  nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
 #endif
-  blocksleft  = nblocks;
-
-  fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
-
-  if (startblock >= priv->npages)
-    {
-      return 0;
-    }
-
-  if (startblock + nblocks > priv->npages)
-    {
-      nblocks = priv->npages - startblock;
-    }
-
-  while (blocksleft-- > 0)
-    {
-      uint16_t offset = startblock * priv->pagesize;
-      unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
-
-      addr[1] = offset & 0xff;
-      addr[0] = (offset >> 8) & 0xff;
-      msgv[1].buffer = buffer;
-
-      for (;;)
-        {
-
-          perf_begin(priv->perf_reads);
-          ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-          perf_end(priv->perf_reads);
-          if (ret >= 0)
-            break;
-
-          fvdbg("read stall");
-          usleep(1000);
-
-          /* We should normally only be here on the first read after
-           * a write.
-           *
-           * XXX maybe do special first-read handling with optional 
-           * bus reset as well?
-           */
-          perf_count(priv->perf_read_retries);
-          if (--tries == 0)
-            { 
-              perf_count(priv->perf_read_errors);
-              return ERROR;
-            }
-        }
-
-      startblock++;
-      buffer += priv->pagesize;
-    }
+	blocksleft  = nblocks;
+
+	fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+
+	if (startblock >= priv->npages) {
+		return 0;
+	}
+
+	if (startblock + nblocks > priv->npages) {
+		nblocks = priv->npages - startblock;
+	}
+
+	while (blocksleft-- > 0) {
+		uint16_t offset = startblock * priv->pagesize;
+		unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
+
+		addr[1] = offset & 0xff;
+		addr[0] = (offset >> 8) & 0xff;
+		msgv[1].buffer = buffer;
+
+		for (;;) {
+
+			perf_begin(priv->perf_reads);
+			ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
+			perf_end(priv->perf_reads);
+
+			if (ret >= 0)
+				break;
+
+			fvdbg("read stall");
+			usleep(1000);
+
+			/* We should normally only be here on the first read after
+			 * a write.
+			 *
+			 * XXX maybe do special first-read handling with optional
+			 * bus reset as well?
+			 */
+			perf_count(priv->perf_read_retries);
+
+			if (--tries == 0) {
+				perf_count(priv->perf_read_errors);
+				return ERROR;
+			}
+		}
+
+		startblock++;
+		buffer += priv->pagesize;
+	}
 
 #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-  return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
 #else
-  return nblocks;
+	return nblocks;
 #endif
 }
 
@@ -322,81 +314,75 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
  ************************************************************************************/
 
 static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks,
-                            FAR const uint8_t *buffer)
+			    FAR const uint8_t *buffer)
 {
-  FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
-  size_t blocksleft;
-  uint8_t buf[AT24XX_PAGESIZE+2];
-  int ret;
-
-  struct i2c_msg_s msgv[1] = 
-    {
-      {
-        .addr = priv->addr,
-        .flags = 0,
-        .buffer = &buf[0],
-        .length = sizeof(buf),
-      }
-    };
+	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
+	size_t blocksleft;
+	uint8_t buf[AT24XX_PAGESIZE + 2];
+	int ret;
+
+	struct i2c_msg_s msgv[1] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &buf[0],
+			.length = sizeof(buf),
+		}
+	};
 
 #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-  startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
-  nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	nblocks    *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
 #endif
-  blocksleft  = nblocks;
+	blocksleft  = nblocks;
 
-  if (startblock >= priv->npages)
-    {
-      return 0;
-    }
+	if (startblock >= priv->npages) {
+		return 0;
+	}
 
-  if (startblock + nblocks > priv->npages)
-    {
-      nblocks = priv->npages - startblock;
-    }
+	if (startblock + nblocks > priv->npages) {
+		nblocks = priv->npages - startblock;
+	}
 
-  fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+	fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
 
-  while (blocksleft-- > 0)
-    {
-      uint16_t offset = startblock * priv->pagesize;
-      unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
+	while (blocksleft-- > 0) {
+		uint16_t offset = startblock * priv->pagesize;
+		unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
 
-      buf[1] = offset & 0xff;
-      buf[0] = (offset >> 8) & 0xff;
-      memcpy(&buf[2], buffer, priv->pagesize);
+		buf[1] = offset & 0xff;
+		buf[0] = (offset >> 8) & 0xff;
+		memcpy(&buf[2], buffer, priv->pagesize);
 
-      for (;;) 
-        {
+		for (;;) {
 
-          perf_begin(priv->perf_writes);
-          ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
-          perf_end(priv->perf_writes);
+			perf_begin(priv->perf_writes);
+			ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
+			perf_end(priv->perf_writes);
 
-          if (ret >= 0)
-            break;
+			if (ret >= 0)
+				break;
 
-          fvdbg("write stall");
-          usleep(1000);
+			fvdbg("write stall");
+			usleep(1000);
 
-          /* We expect to see a number of retries per write cycle as we 
-           * poll for write completion.
-           */
-          if (--tries == 0)
-            { 
-              perf_count(priv->perf_write_errors);
-              return ERROR;
-            }
-        }
+			/* We expect to see a number of retries per write cycle as we
+			 * poll for write completion.
+			 */
+			if (--tries == 0) {
+				perf_count(priv->perf_write_errors);
+				return ERROR;
+			}
+		}
 
-      startblock++;
-      buffer += priv->pagesize;
-    }
+		startblock++;
+		buffer += priv->pagesize;
+	}
 
 #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-  return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
+	return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
 #else
-  return nblocks;
+	return nblocks;
 #endif
 }
 
@@ -406,67 +392,65 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
 
 static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
 {
-  FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
-  int ret = -EINVAL; /* Assume good command with bad parameters */
-
-  fvdbg("cmd: %d \n", cmd);
-
-  switch (cmd)
-    {
-      case MTDIOC_GEOMETRY:
-        {
-         FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg);
-          if (geo)
-            {
-              /* Populate the geometry structure with information need to know
-               * the capacity and how to access the device.
-               *
-               * NOTE: that the device is treated as though it where just an array
-               * of fixed size blocks.  That is most likely not true, but the client
-               * will expect the device logic to do whatever is necessary to make it
-               * appear so.
-               *
-               * blocksize:
-               *   May be user defined. The block size for the at24XX devices may be
-               *   larger than the page size in order to better support file systems.
-               *   The read and write functions translate BLOCKS to pages for the
-               *   small flash devices
-               * erasesize:
-               *   It has to be at least as big as the blocksize, bigger serves no
-               *   purpose.
-               * neraseblocks
-               *   Note that the device size is in kilobits and must be scaled by
-               *   1024 / 8
-               */
+	FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
+	int ret = -EINVAL; /* Assume good command with bad parameters */
+
+	fvdbg("cmd: %d \n", cmd);
+
+	switch (cmd) {
+	case MTDIOC_GEOMETRY: {
+			FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg);
+
+			if (geo) {
+				/* Populate the geometry structure with information need to know
+				 * the capacity and how to access the device.
+				 *
+				 * NOTE: that the device is treated as though it where just an array
+				 * of fixed size blocks.  That is most likely not true, but the client
+				 * will expect the device logic to do whatever is necessary to make it
+				 * appear so.
+				 *
+				 * blocksize:
+				 *   May be user defined. The block size for the at24XX devices may be
+				 *   larger than the page size in order to better support file systems.
+				 *   The read and write functions translate BLOCKS to pages for the
+				 *   small flash devices
+				 * erasesize:
+				 *   It has to be at least as big as the blocksize, bigger serves no
+				 *   purpose.
+				 * neraseblocks
+				 *   Note that the device size is in kilobits and must be scaled by
+				 *   1024 / 8
+				 */
 
 #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
-              geo->blocksize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
-              geo->erasesize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
-              geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
+				geo->blocksize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
+				geo->erasesize    = CONFIG_AT24XX_MTD_BLOCKSIZE;
+				geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
 #else
-              geo->blocksize    = priv->pagesize;
-              geo->erasesize    = priv->pagesize;
-              geo->neraseblocks = priv->npages;
+				geo->blocksize    = priv->pagesize;
+				geo->erasesize    = priv->pagesize;
+				geo->neraseblocks = priv->npages;
 #endif
-              ret               = OK;
+				ret               = OK;
 
-              fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n",
-                    geo->blocksize, geo->erasesize, geo->neraseblocks);
-            }
-        }
-        break;
+				fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n",
+				      geo->blocksize, geo->erasesize, geo->neraseblocks);
+			}
+		}
+		break;
 
-      case MTDIOC_BULKERASE:
-        ret=at24c_eraseall(priv);
-        break;
+	case MTDIOC_BULKERASE:
+		ret = at24c_eraseall(priv);
+		break;
 
-      case MTDIOC_XIPBASE:
-      default:
-        ret = -ENOTTY; /* Bad command */
-        break;
-    }
+	case MTDIOC_XIPBASE:
+	default:
+		ret = -ENOTTY; /* Bad command */
+		break;
+	}
 
-  return ret;
+	return ret;
 }
 
 /************************************************************************************
@@ -483,46 +467,45 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
  *
  ************************************************************************************/
 
-FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev)
-{
-  FAR struct at24c_dev_s *priv;
-
-  fvdbg("dev: %p\n", dev);
-
-  /* Allocate a state structure (we allocate the structure instead of using
-   * a fixed, static allocation so that we can handle multiple FLASH devices.
-   * The current implementation would handle only one FLASH part per I2C
-   * device (only because of the SPIDEV_FLASH definition) and so would have
-   * to be extended to handle multiple FLASH parts on the same I2C bus.
-   */
-
-  priv = &g_at24c;
-  if (priv)
-    {
-      /* Initialize the allocated structure */
- 
-      priv->addr       = CONFIG_AT24XX_ADDR;
-      priv->pagesize   = AT24XX_PAGESIZE;
-      priv->npages     = AT24XX_NPAGES;
-
-      priv->mtd.erase  = at24c_erase;
-      priv->mtd.bread  = at24c_bread;
-      priv->mtd.bwrite = at24c_bwrite;
-      priv->mtd.ioctl  = at24c_ioctl;
-      priv->dev        = dev;
-
-      priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
-      priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
-      priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
-      priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
-      priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
-      priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
-    }
-
-  /* Return the implementation-specific state structure as the MTD device */
-
-  fvdbg("Return %p\n", priv);
-  return (FAR struct mtd_dev_s *)priv;
+FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
+	FAR struct at24c_dev_s *priv;
+
+	fvdbg("dev: %p\n", dev);
+
+	/* Allocate a state structure (we allocate the structure instead of using
+	 * a fixed, static allocation so that we can handle multiple FLASH devices.
+	 * The current implementation would handle only one FLASH part per I2C
+	 * device (only because of the SPIDEV_FLASH definition) and so would have
+	 * to be extended to handle multiple FLASH parts on the same I2C bus.
+	 */
+
+	priv = &g_at24c;
+
+	if (priv) {
+		/* Initialize the allocated structure */
+
+		priv->addr       = CONFIG_AT24XX_ADDR;
+		priv->pagesize   = AT24XX_PAGESIZE;
+		priv->npages     = AT24XX_NPAGES;
+
+		priv->mtd.erase  = at24c_erase;
+		priv->mtd.bread  = at24c_bread;
+		priv->mtd.bwrite = at24c_bwrite;
+		priv->mtd.ioctl  = at24c_ioctl;
+		priv->dev        = dev;
+
+		priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
+		priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
+		priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
+		priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
+		priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
+		priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
+	}
+
+	/* Return the implementation-specific state structure as the MTD device */
+
+	fvdbg("Return %p\n", priv);
+	return (FAR struct mtd_dev_s *)priv;
 }
 
 /*
@@ -530,5 +513,5 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev)
  */
 int at24c_nuke(void)
 {
-  return at24c_eraseall(&g_at24c);
+	return at24c_eraseall(&g_at24c);
 }
diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c
index fa88fa09e..19a14aa02 100644
--- a/apps/systemcmds/eeprom/eeprom.c
+++ b/apps/systemcmds/eeprom/eeprom.c
@@ -143,13 +143,13 @@ eeprom_start(void)
 
 	if (ret < 0)
 		errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
-	
+
 	/* mount the EEPROM */
 	ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
 
 	if (ret < 0)
 		errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
-	
+
 	started = true;
 	warnx("mounted EEPROM at /eeprom");
 	exit(0);
@@ -165,6 +165,7 @@ eeprom_erase(void)
 
 	if (at24c_nuke())
 		errx(1, "erase failed");
+
 	errx(0, "erase done, reboot now");
 }
 
@@ -190,7 +191,7 @@ eeprom_save(const char *name)
 	if (!started)
 		errx(1, "must be started first");
 
-	if (!name) 
+	if (!name)
 		err(1, "missing argument for device name, try '/eeprom/parameters'");
 
 	warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
@@ -221,9 +222,9 @@ eeprom_load(const char *name)
 	if (!started)
 		errx(1, "must be started first");
 
-	if (!name) 
+	if (!name)
 		err(1, "missing argument for device name, try '/eeprom/parameters'");
-	
+
 	warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
 
 	int fd = open(name, O_RDONLY);
diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c
index 02c1bb133..15d448118 100644
--- a/apps/systemcmds/led/led.c
+++ b/apps/systemcmds/led/led.c
@@ -50,7 +50,7 @@
 #include <termios.h>
 #include <time.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/drv_led.h>
 
 #include <systemlib/err.h>
@@ -121,6 +121,7 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
+
 	fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n");
 	exit(1);
 }
@@ -129,7 +130,7 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_create().
  */
@@ -165,9 +166,11 @@ int led_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			printf("\tled is running\n");
+
 		} else {
 			printf("\tled not started\n");
 		}
+
 		exit(0);
 	}
 
@@ -187,7 +190,7 @@ int led_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit) {
 		/* swell blue led */
-		
+
 
 		/* 200 Hz base loop */
 		usleep(1000000 / rate);
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
index 9d52557e7..3f52bdbf1 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/apps/systemcmds/mixer/mixer.c
@@ -97,6 +97,7 @@ load(const char *devname, const char *fname)
 
 	/* tell it to load the file */
 	ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
+
 	if (ret != 0) {
 		fprintf(stderr, "failed loading %s\n", fname);
 	}
diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c
index d70d15da4..68dbd822e 100644
--- a/apps/systemcmds/param/param.c
+++ b/apps/systemcmds/param/param.c
@@ -70,10 +70,13 @@ param_main(int argc, char *argv[])
 	if (argc >= 2) {
 		if (!strcmp(argv[1], "save"))
 			do_save();
+
 		if (!strcmp(argv[1], "load"))
 			do_load();
+
 		if (!strcmp(argv[1], "import"))
 			do_import();
+
 		if (!strcmp(argv[1], "show"))
 			do_show();
 	}
@@ -154,8 +157,8 @@ do_show_print(void *arg, param_t param)
 	float f;
 
 	printf("%c %s: ",
-		param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
-		param_name(param));
+	       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
+	       param_name(param));
 
 	/*
 	 * This case can be expanded to handle printing common structure types.
@@ -167,19 +170,25 @@ do_show_print(void *arg, param_t param)
 			printf("%d\n", i);
 			return;
 		}
+
 		break;
+
 	case PARAM_TYPE_FLOAT:
 		if (!param_get(param, &f)) {
 			printf("%4.4f\n", (double)f);
 			return;
 		}
+
 		break;
+
 	case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
 		printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
 		return;
+
 	default:
 		printf("<unknown type %d>\n", 0 + param_type(param));
 		return;
 	}
+
 	printf("<error fetching parameter %d>\n", param);
 }
diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c
index f4e260211..59d2bc8f1 100644
--- a/apps/systemcmds/top/top.c
+++ b/apps/systemcmds/top/top.c
@@ -46,8 +46,8 @@
 #include <string.h>
 #include <poll.h>
 
-#include <arch/board/up_cpuload.h>
-#include <arch/board/up_hrt.h>
+#include <systemlib/cpuload.h>
+#include <drivers/drv_hrt.h>
 
 /**
  * Start the top application.
@@ -190,13 +190,15 @@ int top_main(int argc, char *argv[])
 						runtime_spaces = "";
 					}
 
-					unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - 
-								(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+					unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+							      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
 					unsigned stack_free = 0;
 					uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+
 					while (stack_free < stack_size) {
 						if (*stack_sweeper++ != 0xff)
 							break;
+
 						stack_free++;
 					}
 
diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile
index b49a3c54a..4bc7033e7 100644
--- a/apps/systemlib/Makefile
+++ b/apps/systemlib/Makefile
@@ -41,8 +41,13 @@ CSRCS		 = err.c \
 		   param/param.c \
 		   bson/tinybson.c \
 		   conversions.c \
+		   cpuload.c \
 		   getopt_long.c
 
+#		   ppm_decode.c \
+
+
+
 #
 # XXX this really should be a CONFIG_* test
 #
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 10b736fd6..75578d2ec 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -56,7 +56,7 @@
 static int
 read_int8(bson_decoder_t decoder, int8_t *b)
 {
-	return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;	
+	return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;
 }
 
 static int
@@ -119,12 +119,14 @@ bson_decoder_next(bson_decoder_t decoder)
 	while (decoder->pending > 0) {
 		if (read_int8(decoder, &tbyte))
 			CODER_KILL(decoder, "read error discarding pending bytes");
+
 		decoder->pending--;
 	}
 
 	/* get the type byte */
 	if (read_int8(decoder, &tbyte))
 		CODER_KILL(decoder, "read error on type byte");
+
 	decoder->node.type = tbyte;
 	decoder->pending = 0;
 
@@ -135,13 +137,17 @@ bson_decoder_next(bson_decoder_t decoder)
 
 		/* get the node name */
 		nlen = 0;
+
 		for (;;) {
 			if (nlen >= BSON_MAXNAME)
 				CODER_KILL(decoder, "node name overflow");
+
 			if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen]))
 				CODER_KILL(decoder, "read error on node name");
+
 			if (decoder->node.name[nlen] == '\0')
 				break;
+
 			nlen++;
 		}
 
@@ -151,20 +157,28 @@ bson_decoder_next(bson_decoder_t decoder)
 		case BSON_INT:
 			if (read_int32(decoder, &decoder->node.i))
 				CODER_KILL(decoder, "read error on BSON_INT");
+
 			break;
+
 		case BSON_DOUBLE:
 			if (read_double(decoder, &decoder->node.d))
 				CODER_KILL(decoder, "read error on BSON_DOUBLE");
+
 			break;
+
 		case BSON_STRING:
 			if (read_int32(decoder, &decoder->pending))
 				CODER_KILL(decoder, "read error on BSON_STRING length");
+
 			break;
+
 		case BSON_BINDATA:
 			if (read_int32(decoder, &decoder->pending))
 				CODER_KILL(decoder, "read error on BSON_BINDATA size");
+
 			if (read_int8(decoder, &tbyte))
 				CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
+
 			decoder->node.subtype = tbyte;
 			break;
 
@@ -186,11 +200,12 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
 	CODER_CHECK(decoder);
 
 	/* if data already copied, return zero bytes */
-	if (decoder->pending == 0) 
+	if (decoder->pending == 0)
 		return 0;
 
 	/* copy bytes per the node size */
 	result = read(decoder->fd, buf, decoder->pending);
+
 	if (result != decoder->pending)
 		CODER_KILL(decoder, "read error on copy_data");
 
@@ -209,7 +224,7 @@ static int
 write_int8(bson_encoder_t encoder, int8_t b)
 {
 	debug("write_int8 %d", b);
-	return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;	
+	return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
 }
 
 static int
@@ -233,6 +248,7 @@ write_name(bson_encoder_t encoder, const char *name)
 
 	if (len > BSON_MAXNAME)
 		return -1;
+
 	debug("write name '%s' len %d", name, len);
 	return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1;
 }
@@ -300,6 +316,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char
 	    write_int32(encoder, len) ||
 	    write(encoder->fd, name, len + 1) != (int)(len + 1))
 		CODER_KILL(encoder, "write error on BSON_STRING");
+
 	return 0;
 }
 
@@ -314,5 +331,6 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary
 	    write_int8(encoder, subtype) ||
 	    write(encoder->fd, data, size) != (int)(size))
 		CODER_KILL(encoder, "write error on BSON_BINDATA");
+
 	return 0;
 }
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index 1b9de5cd3..b6229dc50 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/apps/systemlib/bson/tinybson.h
@@ -31,14 +31,14 @@
  *
  ****************************************************************************/
 
- /**
- * @file tinybson.h
- *
- * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
- *
- * Some types and defines taken from the standalone BSON parser/generator 
- * in the Mongo C connector.
- */
+/**
+* @file tinybson.h
+*
+* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
+*
+* Some types and defines taken from the standalone BSON parser/generator
+* in the Mongo C connector.
+*/
 
 #ifndef _TINYBSON_H
 #define _TINYBSON_H
@@ -77,8 +77,7 @@ typedef enum bson_binary_subtype {
 /**
  * Node structure passed to the callback.
  */
-typedef struct bson_node_s
-{
+typedef struct bson_node_s {
 	char			name[BSON_MAXNAME];
 	bson_type_t		type;
 	bson_binary_subtype_t	subtype;
@@ -96,8 +95,7 @@ typedef struct bson_decoder_s *bson_decoder_t;
  */
 typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node);
 
-struct bson_decoder_s
-{
+struct bson_decoder_s {
 	int			fd;
 	bson_decoder_callback	callback;
 	void			*private;
@@ -143,8 +141,7 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder);
 /**
  * Encoder state structure.
  */
-typedef struct bson_encoder_s
-{
+typedef struct bson_encoder_s {
 	int		fd;
 
 } *bson_encoder_t;
@@ -169,7 +166,7 @@ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, i
  */
 __EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
 
-/** 
+/**
  * Append a string to the encoded stream.
  */
 __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c
index 99ee41508..9105d83cb 100644
--- a/apps/systemlib/conversions.c
+++ b/apps/systemlib/conversions.c
@@ -45,13 +45,13 @@
 int16_t
 int16_t_from_bytes(uint8_t bytes[])
 {
-    union {
-        uint8_t    b[2];
-        int16_t    w;
-    } u;
+	union {
+		uint8_t    b[2];
+		int16_t    w;
+	} u;
 
-    u.b[1] = bytes[0];
-    u.b[0] = bytes[1];
+	u.b[1] = bytes[0];
+	u.b[0] = bytes[1];
 
-    return u.w;
+	return u.w;
 }
diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c
new file mode 100644
index 000000000..20b711fa6
--- /dev/null
+++ b/apps/systemlib/cpuload.c
@@ -0,0 +1,176 @@
+/****************************************************************************
+ * configs/px4fmu/src/up_leds.c
+ * arch/arm/src/board/up_leds.c
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <sys/time.h>
+#include <sched.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "cpuload.h"
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT void sched_note_start(FAR _TCB *tcb);
+__EXPORT void sched_note_stop(FAR _TCB *tcb);
+__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
+
+/****************************************************************************
+ * Name:
+ ****************************************************************************/
+
+__EXPORT struct system_load_s system_load;
+
+extern FAR _TCB *sched_gettcb(pid_t pid);
+
+void cpuload_initialize_once()
+{
+//	if (!system_load.initialized)
+//	{
+	system_load.start_time = hrt_absolute_time();
+	int i;
+
+	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+		system_load.tasks[i].valid = false;
+	}
+
+	system_load.total_count = 0;
+
+	uint64_t now = hrt_absolute_time();
+
+	/* initialize idle thread statically */
+	system_load.tasks[0].start_time = now;
+	system_load.tasks[0].total_runtime = 0;
+	system_load.tasks[0].curr_start_time = 0;
+	system_load.tasks[0].tcb = sched_gettcb(0);
+	system_load.tasks[0].valid = true;
+	system_load.total_count++;
+
+	/* initialize init thread statically */
+	system_load.tasks[1].start_time = now;
+	system_load.tasks[1].total_runtime = 0;
+	system_load.tasks[1].curr_start_time = 0;
+	system_load.tasks[1].tcb = sched_gettcb(1);
+	system_load.tasks[1].valid = true;
+	/* count init thread */
+	system_load.total_count++;
+	//	}
+}
+
+void sched_note_start(FAR _TCB *tcb)
+{
+	/* search first free slot */
+	int i;
+
+	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+		if (!system_load.tasks[i].valid) {
+			/* slot is available */
+			system_load.tasks[i].start_time = hrt_absolute_time();
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+			system_load.tasks[i].tcb = tcb;
+			system_load.tasks[i].valid = true;
+			system_load.total_count++;
+			break;
+		}
+	}
+}
+
+void sched_note_stop(FAR _TCB *tcb)
+{
+	int i;
+
+	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+		if (system_load.tasks[i].tcb->pid == tcb->pid) {
+			/* mark slot as fee */
+			system_load.tasks[i].valid = false;
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+			system_load.tasks[i].tcb = NULL;
+			system_load.total_count--;
+			break;
+		}
+	}
+}
+
+void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
+{
+	uint64_t new_time = hrt_absolute_time();
+
+	/* Kind of inefficient: find both tasks and update times */
+	uint8_t both_found = 0;
+
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		/* Task ending its current scheduling run */
+		if (system_load.tasks[i].tcb->pid == pFromTcb->pid) {
+			//if (system_load.tasks[i].curr_start_time != 0)
+			{
+				system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
+			}
+			both_found++;
+
+		} else if (system_load.tasks[i].tcb->pid == pToTcb->pid) {
+			system_load.tasks[i].curr_start_time = new_time;
+			both_found++;
+		}
+
+		/* Do only iterate as long as needed */
+		if (both_found == 2) {
+			break;
+		}
+	}
+}
+
+#endif /* CONFIG_SCHED_INSTRUMENTATION */
diff --git a/apps/systemlib/cpuload.h b/apps/systemlib/cpuload.h
new file mode 100644
index 000000000..a97047ea8
--- /dev/null
+++ b/apps/systemlib/cpuload.h
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+
+__BEGIN_DECLS
+
+#include <nuttx/sched.h>
+
+struct system_load_taskinfo_s {
+	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
+	uint64_t start_time;         ///< FIRST start time of task
+	FAR struct _TCB *tcb;               ///<
+	bool valid;                  ///< Task is currently active / valid
+};
+
+struct system_load_s {
+	uint64_t start_time;         ///< Global start time of measurements
+	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
+	uint8_t initialized;
+	int total_count;
+	int running_count;
+	int sleeping_count;
+};
+
+__EXPORT extern struct system_load_s system_load;
+
+__EXPORT void cpuload_initialize_once(void);
+
+#endif
diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c
index f22c5ed0d..3011743a1 100644
--- a/apps/systemlib/err.c
+++ b/apps/systemlib/err.c
@@ -79,8 +79,10 @@ warnerr_core(int errcode, const char *fmt, va_list args)
 	/* convenience as many parts of NuttX use negative errno */
 	if (errcode < 0)
 		errcode = -errcode;
+
 	if (errcode < NOCODE)
 		fprintf(stderr, ": %s", strerror(errcode));
+
 	fprintf(stderr, "\n");
 #elif CONFIG_ARCH_LOWPUTC
 	lib_lowprintf("%s: ", getprogname());
@@ -89,8 +91,10 @@ warnerr_core(int errcode, const char *fmt, va_list args)
 	/* convenience as many parts of NuttX use negative errno */
 	if (errcode < 0)
 		errcode = -errcode;
+
 	if (errcode < NOCODE)
 		lib_lowprintf(": %s", strerror(errcode));
+
 	lib_lowprintf("\n");
 #endif
 }
@@ -144,7 +148,7 @@ verrx(int exitcode, const char *fmt, va_list args)
 }
 
 void
-warn(const char *fmt, ...) 
+warn(const char *fmt, ...)
 {
 	va_list args;
 
@@ -153,13 +157,13 @@ warn(const char *fmt, ...)
 }
 
 void
-vwarn(const char *fmt, va_list args) 
+vwarn(const char *fmt, va_list args)
 {
 	warnerr_core(errno, fmt, args);
 }
 
 void
-warnc(int errcode, const char *fmt, ...) 
+warnc(int errcode, const char *fmt, ...)
 {
 	va_list args;
 
@@ -168,13 +172,13 @@ warnc(int errcode, const char *fmt, ...)
 }
 
 void
-vwarnc(int errcode, const char *fmt, va_list args) 
+vwarnc(int errcode, const char *fmt, va_list args)
 {
 	warnerr_core(errcode, fmt, args);
 }
 
 void
-warnx(const char *fmt, ...) 
+warnx(const char *fmt, ...)
 {
 	va_list args;
 
@@ -183,7 +187,7 @@ warnx(const char *fmt, ...)
 }
 
 void
-vwarnx(const char *fmt, va_list args) 
+vwarnx(const char *fmt, va_list args)
 {
 	warnerr_core(NOCODE, fmt, args);
 }
diff --git a/apps/systemlib/err.h b/apps/systemlib/err.h
index f798b97e7..ca13d6265 100644
--- a/apps/systemlib/err.h
+++ b/apps/systemlib/err.h
@@ -36,30 +36,30 @@
  *
  * Simple error/warning functions, heavily inspired by the BSD functions of
  * the same names.
- * 
+ *
  * The err() and warn() family of functions display a formatted error
  * message on the standard error output.  In all cases, the last
  * component of the program name, a colon character, and a space are
  * output.  If the fmt argument is not NULL, the printf(3)-like formatted
  * error message is output.  The output is terminated by a newline
  * character.
- * 
+ *
  * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and
  * vwarnc() functions append an error message obtained from strerror(3)
  * based on a supplied error code value or the global variable errno,
  * preceded by another colon and space unless the fmt argument is NULL.
- * 
+ *
  * In the case of the errc(), verrc(), warnc(), and vwarnc() functions,
  * the code argument is used to look up the error message.
- * 
+ *
  * The err(), verr(), warn(), and vwarn() functions use the global
  * variable errno to look up the error message.
- * 
+ *
  * The errx() and warnx() functions do not append an error message.
- * 
+ *
  * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do
  * not return, but exit with the value of the argument eval.
- * 
+ *
  */
 
 #ifndef _SYSTEMLIB_ERR_H
@@ -71,18 +71,18 @@ __BEGIN_DECLS
 
 __EXPORT const char *getprogname(void);
 
-__EXPORT void	err(int eval, const char *fmt, ...)		__attribute__((noreturn,format(printf,2, 3)));
-__EXPORT void	verr(int eval, const char *fmt, va_list)	__attribute__((noreturn,format(printf,2, 0)));
-__EXPORT void	errc(int eval, int code, const char *fmt, ...)	__attribute__((noreturn,format(printf,3, 4)));
-__EXPORT void	verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn,format(printf,3, 0)));
-__EXPORT void	errx(int eval, const char *fmt, ...) 		__attribute__((noreturn,format(printf,2, 3)));
-__EXPORT void	verrx(int eval, const char *fmt, va_list)	__attribute__((noreturn,format(printf,2, 0)));
-__EXPORT void	warn(const char *fmt, ...)			__attribute__((format(printf,1, 2)));
-__EXPORT void	vwarn(const char *fmt, va_list)			__attribute__((format(printf,1, 0)));
-__EXPORT void	warnc(int code, const char *fmt, ...)		__attribute__((format(printf,2, 3)));
-__EXPORT void	vwarnc(int code, const char *fmt, va_list)	__attribute__((format(printf,2, 0)));
-__EXPORT void	warnx(const char *fmt, ...)			__attribute__((format(printf,1, 2)));
-__EXPORT void	vwarnx(const char *fmt, va_list)		__attribute__((format(printf,1, 0)));
+__EXPORT void	err(int eval, const char *fmt, ...)		__attribute__((noreturn, format(printf, 2, 3)));
+__EXPORT void	verr(int eval, const char *fmt, va_list)	__attribute__((noreturn, format(printf, 2, 0)));
+__EXPORT void	errc(int eval, int code, const char *fmt, ...)	__attribute__((noreturn, format(printf, 3, 4)));
+__EXPORT void	verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0)));
+__EXPORT void	errx(int eval, const char *fmt, ...) 		__attribute__((noreturn, format(printf, 2, 3)));
+__EXPORT void	verrx(int eval, const char *fmt, va_list)	__attribute__((noreturn, format(printf, 2, 0)));
+__EXPORT void	warn(const char *fmt, ...)			__attribute__((format(printf, 1, 2)));
+__EXPORT void	vwarn(const char *fmt, va_list)			__attribute__((format(printf, 1, 0)));
+__EXPORT void	warnc(int code, const char *fmt, ...)		__attribute__((format(printf, 2, 3)));
+__EXPORT void	vwarnc(int code, const char *fmt, va_list)	__attribute__((format(printf, 2, 0)));
+__EXPORT void	warnx(const char *fmt, ...)			__attribute__((format(printf, 1, 2)));
+__EXPORT void	vwarnx(const char *fmt, va_list)		__attribute__((format(printf, 1, 0)));
 
 __END_DECLS
 
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index bc467bfa3..789368fbd 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
 {
 // This function returns the distance to the nearest point on the track line.  Distance is positive if current
-// position is right of the track and negative if left of the track as seen from a point on the track line 
+// position is right of the track and negative if left of the track as seen from a point on the track line
 // headed towards the end point.
 
 	crosstrack_error_s return_var;
@@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now,
 	float bearing_end;
 	float bearing_track;
 	float bearing_diff;
-	
+
 	return_var.error = true;		// Set error flag, cleared when valid result calculated.
 	return_var.past_end = false;
 	return_var.distance = 0.0f;
 	return_var.bearing = 0.0f;
-	
+
 	// Return error if arguments are bad
-	if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
-	
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
+
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
 	bearing_diff = bearing_track - bearing_end;
 	bearing_diff = _wrapPI(bearing_diff);
-	
+
 	// Return past_end = true if past end point of line
-	if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
+	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
 		return_var.past_end = true;
 		return_var.error = false;
 		return return_var;
 	}
-	
+
 	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	return_var.distance = (dist_to_end)*sin(bearing_diff);
-	if(sin(bearing_diff) >=0 ) {
+	return_var.distance = (dist_to_end) * sin(bearing_diff);
+
+	if (sin(bearing_diff) >= 0) {
 		return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
+
 	} else {
 		return_var.bearing = _wrapPI(bearing_track + M_PI_2_F);
 	}
+
 	return_var.error = false;
-	
+
 	return return_var;
 
 }
 
 
-__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, 
-									float radius, float arc_start_bearing, float arc_sweep)
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep)
 {
 	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
 	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
 	float	bearing_sector_end;
 	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
 	bool	in_sector;
-	
+
 	return_var.error = true;		// Set error flag, cleared when valid result calculated.
 	return_var.past_end = false;
 	return_var.distance = 0.0f;
 	return_var.bearing = 0.0f;
-	
+
 	// Return error if arguments are bad
-	if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
-	
-	
-	if(arc_sweep >= 0) {
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
+
+
+	if (arc_sweep >= 0) {
 		bearing_sector_start = arc_start_bearing;
 		bearing_sector_end = arc_start_bearing + arc_sweep;
-		if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
+		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
 	} else {
 		bearing_sector_end = arc_start_bearing;
 		bearing_sector_start = arc_start_bearing - arc_sweep;
-		if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
+
+		if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
 	}
+
 	in_sector = false;
-	
+
 	// Case where sector does not span zero
-	if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
-	
+	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+
 	// Case where sector does span zero
-	if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true;
-	
+	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+
 	// If in the sector then calculate distance and bearing to closest point
-	if(in_sector) {
+	if (in_sector) {
 		return_var.past_end = false;
 		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-		
-		if(dist_to_center <= radius) {
+
+		if (dist_to_center <= radius) {
 			return_var.distance = radius - dist_to_center;
 			return_var.bearing = bearing_now + M_PI_F;
+
 		} else {
 			return_var.distance = dist_to_center - radius;
 			return_var.bearing = bearing_now;
 		}
-		
-	// If out of the sector then calculate dist and bearing to start or end point
+
+		// If out of the sector then calculate dist and bearing to start or end point
+
 	} else {
-	
-	// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude) 
-	// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
-	// calculate the position of the start and end points.  We should not be doing this often
-	// as this function generally will not be called repeatedly when we are out of the sector.
-	
-	// TO DO - this is messed up and won't compile
+
+		// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude)
+		// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
+		// calculate the position of the start and end points.  We should not be doing this often
+		// as this function generally will not be called repeatedly when we are out of the sector.
+
+		// TO DO - this is messed up and won't compile
 		float start_disp_x = radius * sin(arc_start_bearing);
 		float start_disp_y = radius * cos(arc_start_bearing);
-		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep));
-		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep));
-		float lon_start = lon_now + start_disp_x/111111.0d;
-		float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d;
-		float lon_end = lon_now + end_disp_x/111111.0d;
-		float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d;
+		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
+		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
+		float lon_start = lon_now + start_disp_x / 111111.0d;
+		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
+		float lon_end = lon_now + end_disp_x / 111111.0d;
+		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
 		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
 		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-		if(dist_to_start < dist_to_end) {
+
+		if (dist_to_start < dist_to_end) {
 			return_var.distance = dist_to_start;
 			return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+
 		} else {
 			return_var.past_end = true;
 			return_var.distance = dist_to_end;
@@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
 		}
 
 	}
-	
+
 	return_var.bearing = _wrapPI(return_var.bearing);
 	return_var.error = false;
-	return return_var;	
+	return return_var;
 }
 
 float _wrapPI(float bearing)
@@ -227,21 +238,25 @@ float _wrapPI(float bearing)
 	while (bearing > M_PI_F) {
 		bearing = bearing - M_TWOPI_F;
 	}
+
 	while (bearing <=  -M_PI_F) {
 		bearing = bearing + M_TWOPI_F;
 	}
+
 	return bearing;
 }
-		
+
 float _wrap2PI(float bearing)
 {
 
 	while (bearing >= M_TWOPI_F) {
 		bearing = bearing - M_TWOPI_F;
 	}
+
 	while (bearing <  0.0f) {
 		bearing = bearing + M_TWOPI_F;
 	}
+
 	return bearing;
 }
 
@@ -251,23 +266,26 @@ float _wrap180(float bearing)
 	while (bearing > 180.0f) {
 		bearing = bearing - 360.0f;
 	}
+
 	while (bearing <=  -180.0f) {
 		bearing = bearing + 360.0f;
 	}
+
 	return bearing;
 }
-		
+
 float _wrap360(float bearing)
 {
 
 	while (bearing >= 360.0f) {
 		bearing = bearing - 360.0f;
 	}
+
 	while (bearing <  0.0f) {
 		bearing = bearing + 360.0f;
 	}
+
 	return bearing;
 }
-		
 
-	
\ No newline at end of file
+
diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 205afc79e..c98b4c85d 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -44,8 +44,8 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
- 
- 
+
+
 #include <stdbool.h>
 
 typedef struct {
@@ -59,14 +59,14 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-// 
+//
 
 __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
-__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, 
-									 float radius, float arc_start_bearing, float arc_sweep);
-									 
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep);
+
 float _wrap180(float bearing);
-float _wrap360(float bearing);	 
+float _wrap360(float bearing);
 float _wrapPI(float bearing);
 float _wrap2PI(float bearing);
\ No newline at end of file
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index b56226c03..6c1bbe469 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -63,7 +63,7 @@ float
 Mixer::get_control(uint8_t group, uint8_t index)
 {
 	float	value;
-	
+
 	_control_cb(_cb_handle, group, index, value);
 
 	return value;
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 44a33bf8b..004151235 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -224,29 +224,35 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co
 
 	if (!strcmp(geomname, "4+")) {
 		geometry = MultirotorMixer::QUAD_PLUS;
+
 	} else if (!strcmp(geomname, "4x")) {
 		geometry = MultirotorMixer::QUAD_X;
+
 	} else if (!strcmp(geomname, "6+")) {
 		geometry = MultirotorMixer::HEX_PLUS;
+
 	} else if (!strcmp(geomname, "6x")) {
 		geometry = MultirotorMixer::HEX_X;
+
 	} else if (!strcmp(geomname, "8+")) {
 		geometry = MultirotorMixer::OCTA_PLUS;
+
 	} else if (!strcmp(geomname, "8x")) {
 		geometry = MultirotorMixer::OCTA_X;
+
 	} else {
 		debug("unrecognised geometry '%s'", geomname);
 		return nullptr;
 	}
 
 	return new MultirotorMixer(
-		control_cb,
-		cb_handle,
-		geometry,
-		s[0] / 10000.0f,
-		s[1] / 10000.0f,
-		s[2] / 10000.0f,
-		s[3] / 10000.0f);
+		       control_cb,
+		       cb_handle,
+		       geometry,
+		       s[0] / 10000.0f,
+		       s[1] / 10000.0f,
+		       s[2] / 10000.0f,
+		       s[3] / 10000.0f);
 }
 
 int
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index e2576a848..0e714ed50 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -57,7 +57,7 @@
 #define		CW		(-1.0f)
 #define		CCW		(1.0f)
 
-namespace 
+namespace
 {
 
 /*
@@ -167,17 +167,21 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 			    pitch * _rotors[i].pitch_scale +
 			    yaw   * _rotors[i].yaw_scale +
 			    thrust;
+
 		if (tmp > max)
 			max = tmp;
+
 		outputs[i] = tmp;
 	}
 
 	/* scale values into the -1.0 - 1.0 range */
 	if (max > 1.0f) {
 		fixup_scale = 2.0f / max;
+
 	} else {
 		fixup_scale = 2.0f;
 	}
+
 	for (unsigned i = 0; i < _rotor_count; i++)
 		outputs[i] = -1.0f + (outputs[i] * fixup_scale);
 
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index 9e886ea65..eeb867f11 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -50,7 +50,7 @@
 
 #include <sys/stat.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "systemlib/param/param.h"
 #include "systemlib/uthash/utarray.h"
@@ -189,6 +189,7 @@ param_notify_changes(void)
 	 */
 	if (param_topic == -1) {
 		param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
+
 	} else {
 		orb_publish(ORB_ID(parameter_update), param_topic, &pup);
 	}
@@ -455,6 +456,7 @@ param_reset(param_t param)
 			utarray_erase(param_values, pos, 1);
 		}
 	}
+
 	param_unlock();
 
 	if (s != NULL)
@@ -560,8 +562,7 @@ out:
 	return result;
 }
 
-struct param_import_state
-{
+struct param_import_state {
 	bool mark_saved;
 };
 
@@ -689,9 +690,10 @@ param_import_internal(int fd, bool mark_saved)
 	do {
 		result = bson_decoder_next(&decoder);
 
-	} while(result > 0);
+	} while (result > 0);
 
 out:
+
 	if (result < 0)
 		debug("BSON error decoding parameters");
 
diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c
index e25e548f0..e6c49d432 100644
--- a/apps/systemlib/perf_counter.c
+++ b/apps/systemlib/perf_counter.c
@@ -40,7 +40,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <sys/queue.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "perf_counter.h"
 
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index f9b50d030..7e277cdc7 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -43,7 +43,7 @@
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-	      float limit, uint8_t mode)
+		       float limit, uint8_t mode)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -65,30 +65,35 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 
 	if (isfinite(kp)) {
 		pid->kp = kp;
+
 	} else {
 		ret = 1;
 	}
 
 	if (isfinite(ki)) {
 		pid->ki = ki;
+
 	} else {
 		ret = 1;
 	}
 
 	if (isfinite(kd)) {
 		pid->kd = kd;
+
 	} else {
 		ret = 1;
 	}
 
 	if (isfinite(intmax)) {
 		pid->intmax = intmax;
+
 	}  else {
 		ret = 1;
 	}
-	
+
 	if (isfinite(limit)) {
 		pid->limit = limit;
+
 	}  else {
 		ret = 1;
 	}
@@ -121,17 +126,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	 goto start
 	 */
 
-	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
-	{
+	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
 		return pid->last_output;
 	}
 
 	float i, d;
 	pid->sp = sp;
-	
+
 	// Calculated current error value
 	float error = pid->sp - val;
-	
+
 	if (isfinite(error)) {				// Why is this necessary?  DEW
 		pid->error_previous = error;
 	}
@@ -140,30 +144,36 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
 		d = (error - pid->error_previous) / dt;
+
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
+
 	} else {
 		d = 0.0f;
 	}
 
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
-	if( fabs((error * pid->kp) + (i * pid->ki) +  (d * pid->kd)) > pid->limit ||
-		fabs(i) > pid->intmax )
-	{
+
+	if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
+	    fabs(i) > pid->intmax) {
 		i = pid->integral;		// If saturated then do not update integral value
 		pid->saturated = 1;
+
 	} else {
 		if (!isfinite(i)) {
 			i = 0;
 		}
+
 		pid->integral = i;
 		pid->saturated = 0;
 	}
 
 	// Calculate the output.  Limit output magnitude to pid->limit
 	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+
 	if (output > pid->limit) output = pid->limit;
+
 	if (output < -pid->limit) output = -pid->limit;
 
 	if (isfinite(output)) {
diff --git a/apps/systemlib/ppm_decode.c b/apps/systemlib/ppm_decode.c
new file mode 100644
index 000000000..a5d2f738d
--- /dev/null
+++ b/apps/systemlib/ppm_decode.c
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   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 ppm_decode.c
+ *
+ * PPM input decoder.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "ppm_decode.h"
+
+/*
+ * PPM decoder tuning parameters.
+ *
+ * The PPM decoder works as follows.
+ *
+ * Initially, the decoder waits in the UNSYNCH state for two edges
+ * separated by PPM_MIN_START.  Once the second edge is detected,
+ * the decoder moves to the ARM state.
+ *
+ * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the
+ * timing mark for the first channel.  If this is detected, it moves to
+ * the INACTIVE state.
+ *
+ * The INACTIVE phase waits for and discards the next edge, as it is not
+ * significant.  Once the edge is detected, it moves to the ACTIVE stae.
+ *
+ * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
+ * received calculates the time from the previous mark and records
+ * this time as the value for the next channel.
+ *
+ * If at any time waiting for an edge, the delay from the previous edge
+ * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
+ * values are advertised to clients.
+ */
+#define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+#define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+#define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+#define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* Input timeout - after this interval we assume signal is lost */
+#define PPM_INPUT_TIMEOUT	100 * 1000	/* 100ms */
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	3		/* should be less than the input timeout */
+
+/* decoded PPM buffer */
+#define PPM_MIN_CHANNELS	4
+#define PPM_MAX_CHANNELS	12
+
+/*
+ * Public decoder state
+ */
+uint16_t	ppm_buffer[PPM_MAX_CHANNELS];
+unsigned	ppm_decoded_channels;
+hrt_abstime	ppm_last_valid_decode;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+static struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	unsigned	count_max;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+
+void
+ppm_input_init(unsigned count_max)
+{
+	ppm_decoded_channels = 0;
+	ppm_last_valid_decode = 0;
+
+	memset(&ppm, 0, sizeof(ppm));
+	ppm.count_max = count_max;
+}
+
+void
+ppm_input_decode(bool reset, unsigned count)
+{
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (reset)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+
+	if (count < ppm.last_edge)
+		width += ppm.count_max;	/* handle wrapped count */
+
+	ppm.last_edge = count;
+
+	/*
+	 * If this looks like a start pulse, then push the last set of values
+	 * and reset the state machine.
+	 *
+	 * Note that this is not a "high performance" design; it implies a whole
+	 * frame of latency between the pulses being received and their being
+	 * considered valid.
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
+
+				ppm_last_valid_decode = hrt_absolute_time();
+			}
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+
+		/* note that we don't bother looking at the timing of this edge */
+
+		return;
+
+	case ACTIVE:
+
+		/* we expect a well-formed pulse */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+}
+
diff --git a/apps/systemlib/ppm_decode.h b/apps/systemlib/ppm_decode.h
new file mode 100644
index 000000000..c2b24321a
--- /dev/null
+++ b/apps/systemlib/ppm_decode.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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 ppm_decode.h
+ *
+ * PPM input decoder.
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+/**
+ * Maximum number of channels that we will decode.
+ */
+#define PPM_MAX_CHANNELS	12
+
+__BEGIN_DECLS
+
+/*
+ * PPM decoder state
+ */
+__EXPORT extern uint16_t	ppm_buffer[];		/**< decoded PPM channel values */
+__EXPORT extern unsigned	ppm_decoded_channels;	/**< count of decoded channels */
+__EXPORT extern hrt_abstime	ppm_last_valid_decode;	/**< timestamp of the last valid decode */
+
+/**
+ * Initialise the PPM input decoder.
+ *
+ * @param count_max		The maximum value of the counter passed to
+ *				ppm_input_decode, used to determine how to
+ *				handle counter wrap.
+ */
+__EXPORT void		ppm_input_init(unsigned count_max);
+
+/**
+ * Inform the decoder of an edge on the PPM input.
+ *
+ * This function can be registered with the HRT as the PPM edge handler.
+ *
+ * @param reset			If set, the edge detector has missed one or
+ *				more edges and the decoder needs to be reset.
+ * @param count			A microsecond timestamp corresponding to the
+ *				edge, in the range 0-count_max.  This value
+ *				is expected to wrap.
+ */
+__EXPORT void		ppm_input_decode(bool reset, unsigned count);
+
+__END_DECLS
\ No newline at end of file
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index 94a5283f0..50ac62464 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -131,7 +131,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
 	pid = task_create(name, priority, stack_size, entry, argv);
 
 	if (pid > 0) {
-		
+
 		/* configure the scheduler */
 		struct sched_param param;
 
@@ -140,6 +140,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
 
 		/* XXX do any other private task accounting here before the task starts */
 	}
+
 	sched_unlock();
 
 	return pid;
@@ -157,15 +158,18 @@ int fmu_get_board_info(struct fmu_board_info_s *info)
 	statres = stat("/dev/bma180", &sb);
 
 	if (statres == OK) {
-			/* BMA180 indicates a v1.5-v1.6 board */
-			strcpy(info->board_name, "FMU v1.6");
-			info->board_version = 16;
+		/* BMA180 indicates a v1.5-v1.6 board */
+		strcpy(info->board_name, "FMU v1.6");
+		info->board_version = 16;
+
 	} else {
 		statres = stat("/dev/accel", &sb);
+
 		if (statres == OK) {
 			/* MPU-6000 indicates a v1.7+ board */
 			strcpy(info->board_name, "FMU v1.7");
 			info->board_version = 17;
+
 		} else {
 			/* If no BMA and no MPU is present, it is a v1.3 board */
 			strcpy(info->board_name, "FMU v1.3");
diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp
index 72a60f871..532e54b8e 100644
--- a/apps/uORB/uORB.cpp
+++ b/apps/uORB/uORB.cpp
@@ -56,7 +56,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/drv_orb_dev.h>