Merge branch 'master' into vector_control

87 files changed, 10048 insertions, 2218 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5a8157deb..5e45cc936 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -68,6 +68,7 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -77,10 +78,9 @@
 
 Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
+	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
+	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_collect_phase(false),
@@ -88,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	_airspeed_pub(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows"))
+	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -105,7 +104,12 @@ Airspeed::~Airspeed()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
 }
 
 int
@@ -118,20 +122,14 @@ Airspeed::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct differential_pressure_s[_num_reports];
-
-	for (unsigned i = 0; i < _num_reports; i++)
-		_reports[i].max_differential_pressure_pa = 0;
-
+	_reports = new RingBuffer(2, sizeof(differential_pressure_s));
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the airspeed topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
+	differential_pressure_s zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
 
 	if (_airspeed_pub < 0)
 		warnx("failed to create airspeed sensor object. Did you start uOrb?");
@@ -146,7 +144,14 @@ out:
 int
 Airspeed::probe()
 {
-	return measure();
+	/* on initial power up the device needs more than one retry
+	   for detection. Once it is running then retries aren't
+	   needed 
+	*/
+	_retries = 4;
+	int ret = measure();
+	_retries = 2;
+	return ret;
 }
 
 int
@@ -217,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
 			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
+			if ((arg < 1) || (arg > 100))
 				return -EINVAL;
 
-			/* allocate new buffer */
-			struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
-
-			if (nullptr == buf)
+			irqstate_t flags = irqsave();
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
 				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
+			}
+			irqrestore(flags);
 
 			return OK;
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -269,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(struct differential_pressure_s);
+	unsigned count = buflen / sizeof(differential_pressure_s);
+	differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -285,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(abuf)) {
+				ret += sizeof(*abuf);
+				abuf++;
 			}
 		}
 
@@ -297,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -317,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(abuf)) {
+			ret = sizeof(*abuf);
+		}
 
 	} while (0);
 
@@ -330,7 +326,7 @@ Airspeed::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
@@ -373,6 +369,12 @@ Airspeed::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	warnx("poll interval:  %u ticks", _measure_ticks);
-	warnx("report queue:   %u (%u/%u @ %p)",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
+}
+
+void
+Airspeed::new_report(const differential_pressure_s &report)
+{
+	if (!_reports->force(&report))
+		perf_count(_buffer_overflows);
 }
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 89dfb22d7..048784813 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -68,6 +68,7 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -102,6 +103,10 @@ public:
 	 */
 	virtual void	print_info();
 
+private:
+	RingBuffer		*_reports;
+	perf_counter_t		_buffer_overflows;
+
 protected:
 	virtual int	probe();
 
@@ -114,10 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	differential_pressure_s	*_reports;
+	uint16_t		_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
@@ -129,7 +131,6 @@ protected:
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
-	perf_counter_t		_buffer_overflows;
 
 
 	/**
@@ -162,8 +163,12 @@ protected:
 	*/
 	static void	cycle_trampoline(void *arg);
 
+	/**
+	* add a new report to the reports queue
+	*
+	* @param report		differential_pressure_s report
+	*/
+	void	new_report(const differential_pressure_s &report);
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
 
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 735bdb41a..b88f61ce8 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -53,8 +53,10 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
 #include <systemlib/systemlib.h>
 
@@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int led_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 	struct actuator_armed_s armed;
+	//XXX is this necessairy?
 	armed.armed = false;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	printf("[ardrone_interface] Motors initialized - ready.\n");
@@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		} else {
 			/* MAIN OPERATION MODE */
 
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index be8968a4e..01b89c8fa 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -41,6 +41,7 @@
 #include <stdio.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <math.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
@@ -369,11 +370,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float yaw_control = actuators->control[2];
 	float motor_thrust = actuators->control[3];
 
-	//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
-
 	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
 	const float max_thrust = 1.0f;			/**< 100% max thrust */
-	const float scaling = 500.0f;			/**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
+	const float scaling = 510.0f;			/**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */
 	const float min_gas = min_thrust * scaling;	/**< value range sent to motors, minimum */
 	const float max_gas = max_thrust * scaling;	/**< value range sent to motors, maximum */
 
@@ -382,71 +381,56 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float motor_calc[4] = {0};
 
 	float output_band = 0.0f;
-	float band_factor = 0.75f;
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
-	float yaw_factor = 1.0f;
 
 	static bool initialized = false;
 	/* publish effective outputs */
 	static struct actuator_controls_effective_s actuator_controls_effective;
 	static orb_advert_t actuator_controls_effective_pub;
 
-	if (motor_thrust <= min_thrust) {
-		motor_thrust = min_thrust;
-		output_band = 0.0f;
-	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
-		output_band = band_factor * (motor_thrust - min_thrust);
-	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * startpoint_full_control;
-	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * (max_thrust - motor_thrust);
+	/* linearly scale the control inputs from 0 to startpoint_full_control */
+	if (motor_thrust < startpoint_full_control) {
+		output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1
+	} else {
+		output_band = 1.0f;
 	}
 
+	roll_control *= output_band;
+	pitch_control *= output_band;
+	yaw_control *= output_band;
+
+
 	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
 
 	// FRONT (MOTOR 1)
 	motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
 	// RIGHT (MOTOR 2)
 	motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
 	// BACK (MOTOR 3)
 	motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
 	// LEFT (MOTOR 4)
 	motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 
-	// if we are not in the output band
-	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
-	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
-	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
-	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
+	/* if one motor is saturated, reduce throttle */
+	float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
+
+
+	if (saturation > 0.0f) {
+
+		/* reduce the motor thrust according to the saturation */
+		motor_thrust = motor_thrust - saturation;
 
-		yaw_factor = 0.5f;
-		yaw_control *= yaw_factor;
 		// FRONT (MOTOR 1)
 		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
 		// RIGHT (MOTOR 2)
 		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
 		// BACK (MOTOR 3)
 		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
 		// LEFT (MOTOR 4)
 		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 	}
 
-	for (int i = 0; i < 4; i++) {
-		//check for limits
-		if (motor_calc[i] < motor_thrust - output_band) {
-			motor_calc[i] = motor_thrust - output_band;
-		}
 
-		if (motor_calc[i] > motor_thrust + output_band) {
-			motor_calc[i] = motor_thrust + output_band;
-		}
-	}
 
 	/* publish effective outputs */
 	actuator_controls_effective.control_effective[0] = roll_control;
@@ -467,25 +451,29 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 
 	/* set the motor values */
 
-	/* scale up from 0..1 to 10..512) */
+	/* scale up from 0..1 to 10..500) */
 	motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
 
-	/* Keep motors spinning while armed and prevent overflows */
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
-	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
-	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
-	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
-	motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
-	motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
-	motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
+	/* scale up from 0..1 to 10..500) */
+	motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas));
+
+	/* Failsafe logic for min values - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;
+	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas;
+	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas;
+	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas;
+
+	/* Failsafe logic for max values - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas;
+	motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas;
+	motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas;
+	motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas;
 
 	/* send motors via UART */
 	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 3fabfd9a5..2361f4dd1 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -92,9 +92,6 @@
 
 #include <nuttx/config.h>
 
-#include <arch/board/board.h>
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <string.h>
@@ -104,18 +101,23 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <ctype.h>
-
-#include <drivers/drv_blinkm.h>
+#include <poll.h>
 
 #include <nuttx/wqueue.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
-
 #include <systemlib/systemlib.h>
-#include <poll.h>
+
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_blinkm.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
@@ -375,7 +377,9 @@ BlinkM::led()
 {
 
 	static int vehicle_status_sub_fd;
+	static int vehicle_control_mode_sub_fd;
 	static int vehicle_gps_position_sub_fd;
+	static int actuator_armed_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -386,6 +390,8 @@ BlinkM::led()
 	static int led_interval = 1000;
 
 	static int no_data_vehicle_status = 0;
+	static int no_data_vehicle_control_mode = 0;
+	static int no_data_actuator_armed = 0;
 	static int no_data_vehicle_gps_position = 0;
 
 	static bool topic_initialized = false;
@@ -398,6 +404,12 @@ BlinkM::led()
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 1000);
 
+		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
+		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+
+		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+		orb_set_interval(actuator_armed_sub_fd, 1000);
+
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
 		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
 
@@ -452,12 +464,16 @@ BlinkM::led()
 	if (led_thread_runcount == 15) {
 		/* obtained data for the first file descriptor */
 		struct vehicle_status_s vehicle_status_raw;
+		struct vehicle_control_mode_s vehicle_control_mode;
+		struct actuator_armed_s actuator_armed;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
 
 		bool new_data_vehicle_status;
+		bool new_data_vehicle_control_mode;
+		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +487,28 @@ BlinkM::led()
 				no_data_vehicle_status = 3;
 		}
 
+		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
+
+		if (new_data_vehicle_control_mode) {
+			orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
+			no_data_vehicle_control_mode = 0;
+		} else {
+			no_data_vehicle_control_mode++;
+			if(no_data_vehicle_control_mode >= 3)
+				no_data_vehicle_control_mode = 3;
+		}
+
+		orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
+
+		if (new_data_actuator_armed) {
+			orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
+			no_data_actuator_armed = 0;
+		} else {
+			no_data_actuator_armed++;
+			if(no_data_actuator_armed >= 3)
+				no_data_actuator_armed = 3;
+		}
+
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
 
 		if (new_data_vehicle_gps_position) {
@@ -486,24 +524,24 @@ BlinkM::led()
 
 		/* get number of used satellites in navigation */
 		num_of_used_sats = 0;
-		//for(int satloop=0; satloop<20; satloop++) {
-		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+
+		for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
 			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
 				num_of_used_sats++;
 			}
 		}
 
-		if(new_data_vehicle_status || no_data_vehicle_status < 3){
-			if(num_of_cells == 0) {
+		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+			if (num_of_cells == 0) {
 				/* looking for lipo cells that are connected */
 				printf("<blinkm> checking cells\n");
 				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+					if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
 				}
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;
@@ -515,7 +553,7 @@ BlinkM::led()
 					led_color_8 = LED_YELLOW;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -530,7 +568,7 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(vehicle_status_raw.flag_system_armed == false) {
+					if(actuator_armed.armed == false) {
 						/* system not armed */
 						led_color_1 = LED_RED;
 						led_color_2 = LED_RED;
@@ -554,27 +592,24 @@ BlinkM::led()
 						led_color_8 = LED_OFF;
 						led_blink = LED_BLINK;
 
-						/* handle 4th led - flightmode indicator */
-						switch((int)vehicle_status_raw.flight_mode) {
-							case VEHICLE_FLIGHT_MODE_MANUAL:
-								led_color_4 = LED_AMBER;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_STAB:
-								led_color_4 = LED_YELLOW;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_HOLD:
-								led_color_4 = LED_BLUE;
-								break;
+						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 
-							case VEHICLE_FLIGHT_MODE_AUTO:
+							//XXX please check
+							if (vehicle_control_mode.flag_control_position_enabled)
 								led_color_4 = LED_GREEN;
-								break;
+							else if (vehicle_control_mode.flag_control_velocity_enabled)
+								led_color_4 = LED_BLUE;
+							else if (vehicle_control_mode.flag_control_attitude_enabled)
+								led_color_4 = LED_YELLOW;
+							else if (vehicle_control_mode.flag_control_manual_enabled)
+								led_color_4 = LED_AMBER;
+							else 
+								led_color_4 = LED_OFF;
+								
 						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used sat´s */
+							/* handling used sat�s */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -831,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2])
 	return transfer(&msg, sizeof(msg), version, 2);
 }
 
+void blinkm_usage();
+
 void blinkm_usage() {
-	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
-	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+	warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (3)");
+	warnx("\t-a --blinkmaddr blinkmaddr (9)");
 }
 
 int
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 4409a8a9c..1590cc182 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -59,10 +59,11 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
+#include <drivers/device/ringbuffer.h>
 
 
 /* oddly, ERROR is not defined for c++ */
@@ -146,10 +147,7 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct accel_report	*_reports;
+	RingBuffer		*_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
@@ -233,16 +231,9 @@ private:
 	int			set_lowpass(unsigned frequency);
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-
 BMA180::BMA180(int bus, spi_dev_e device) :
 	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
@@ -270,7 +261,7 @@ BMA180::~BMA180()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -286,16 +277,15 @@ BMA180::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct accel_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(accel_report));
 
 	if (_reports == nullptr)
 		goto out;
 
 	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
+	struct accel_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 
 	/* perform soft reset (p48) */
 	write_reg(ADDR_RESET, SOFT_RESET);
@@ -352,6 +342,7 @@ ssize_t
 BMA180::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct accel_report);
+	struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -367,10 +358,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(arp)) {
+				ret += sizeof(*arp);
+				arp++;
 			}
 		}
 
@@ -379,12 +369,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+	if (_reports->get(arp))
+		ret = sizeof(*arp);
 
 	return ret;
 }
@@ -449,31 +439,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct accel_report *buf = new struct accel_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 2) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement */
@@ -654,7 +635,7 @@ BMA180::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
@@ -688,7 +669,7 @@ BMA180::measure()
 // 	} raw_report;
 // #pragma pack(pop)
 
-	accel_report		*report = &_reports[_next_report];
+	struct accel_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -708,45 +689,41 @@ BMA180::measure()
 	 * them before.  There is no good way to synchronise with the internal
 	 * measurement flow without using the external interrupt.
 	 */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
+        report.error_count = 0;
 	/*
 	 * y of board is x of sensor and x of board is -y of sensor
 	 * perform only the axis assignment here.
 	 * Two non-value bits are discarded directly
 	 */
-	report->y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
-	report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
-	report->x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
-	report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
-	report->z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
-	report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
+	report.y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
+	report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
+	report.x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
+	report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
+	report.z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
+	report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
 
 	/* discard two non-value bits in the 16 bit measurement */
-	report->x_raw = (report->x_raw / 4);
-	report->y_raw = (report->y_raw / 4);
-	report->z_raw = (report->z_raw / 4);
+	report.x_raw = (report.x_raw / 4);
+	report.y_raw = (report.y_raw / 4);
+	report.z_raw = (report.z_raw / 4);
 
 	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
-	report->y_raw = -report->y_raw;
-
-	report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	report->scaling = _accel_range_scale;
-	report->range_m_s2 = _accel_range_m_s2;
+	report.y_raw = -report.y_raw;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	report.scaling = _accel_range_scale;
+	report.range_m_s2 = _accel_range_m_s2;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -756,8 +733,7 @@ void
 BMA180::print_info()
 {
 	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 383a046ff..27621211a 100644
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file px4fmu_internal.h
+ * @file board_config.h
  *
  * PX4FMU internal definitions
  */
@@ -51,13 +51,16 @@ __BEGIN_DECLS
 
 /* these headers are not C++ safe */
 #include <stm32.h>
-
+#include <arch/board/board.h>
  
 /****************************************************************************************************
  * Definitions
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
+
 //#ifdef CONFIG_STM32_SPI2
 //#  error "SPI2 is not supported on this board"
 //#endif
@@ -74,15 +77,47 @@ __BEGIN_DECLS
 
 /* 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)
 
+/*
+ * Use these in place of the spi_dev_e enumeration to
+ * select a specific SPI device on SPI1
+ */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL	2
+#define PX4_SPIDEV_MPU		3
+
+/*
+ * Optional devices on IO's external port
+ */
+#define PX4_SPIDEV_ACCEL_MAG 2
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_ESC		1
+#define PX4_I2C_BUS_ONBOARD	2
+#define PX4_I2C_BUS_EXPANSION	3
+#define PX4_I2C_BUS_LED		3
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+#define PX4_I2C_OBDEV_MS5611	0x76
+#define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
+#define PX4_I2C_OBDEV_LED	0x55
+
+#define PX4_I2C_OBDEV_PX4IO_BL	0x18
+#define PX4_I2C_OBDEV_PX4IO	0x1a
+
 /* User GPIOs
  *
  * GPIO0-1 are the buffered high-power GPIOs.
@@ -107,31 +142,45 @@ __BEGIN_DECLS
 #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)
 
+/*
+ * Tone alarm output
+ */
+#define TONE_ALARM_TIMER	3	/* timer 3 */
+#define TONE_ALARM_CHANNEL	3	/* channel 3 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+
+/*
+ * PWM
+ *
+ * Four PWM outputs can be configured on pins otherwise shared with
+ * USART2; two can take the flow control pins if they are not being used.
+ *
+ * Pins:
+ *
+ * CTS - PA0 - TIM2CH1
+ * RTS - PA1 - TIM2CH2
+ * TX  - PA2 - TIM2CH3
+ * RX  - PA3 - TIM2CH4
+ *
+ */
+#define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_1
+#define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
+#define GPIO_TIM2_CH3OUT	GPIO_TIM2_CH3OUT_1
+#define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_1
+
 /* 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
+/* High-resolution timer
  */
-
-#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
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+#define HRT_PPM_CHANNEL		3	/* use capture/compare channel 3 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
 
 /****************************************************************************************************
  * Public Types
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917..66b776917 100644
--- a/src/drivers/boards/px4fmu/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
index 187689ff9..1e1f10040 100644
--- a/src/drivers/boards/px4fmu/px4fmu_can.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
@@ -54,7 +54,7 @@
 
 #include "stm32.h"
 #include "stm32_can.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 #ifdef CONFIG_CAN
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 36af2298c..4b12b75f9 100644
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -59,7 +59,7 @@
 #include <nuttx/analog/adc.h>
 
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
@@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
 	 */
-	spi2 = up_spiinitialize(2);
 
-	if (!spi2) {
-		message("[boot] Enabling IN12/13 instead of SPI2\n");
-		/* no SPI2, use pins for ADC */
-		stm32_configgpio(GPIO_ADC1_IN12);
-		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-	} else {
+	#ifdef CONFIG_STM32_SPI2
+		spi2 = up_spiinitialize(2);
 		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
 		SPI_SETFREQUENCY(spi2, 10000000);
 		SPI_SETBITS(spi2, 8);
@@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
 		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
 
 		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
-	}
+	#else
+		spi2 = NULL;
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	#endif
 
 	/* Get the SPI port for the microSD slot */
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index 31b25984e..ea91f34ad 100644
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -42,7 +42,7 @@
 #include <stdbool.h>
 
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 #include <arch/board/board.h>
 
@@ -57,6 +57,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 __EXPORT void led_init()
@@ -94,3 +95,21 @@ __EXPORT void led_off(int led)
 		stm32_gpiowrite(GPIO_LED2, true);
 	}
 }
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 0)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED2))
+			stm32_gpiowrite(GPIO_LED2, false);
+		else
+			stm32_gpiowrite(GPIO_LED2, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
index d85131dd8..848e21d79 100644
--- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
@@ -41,15 +41,15 @@
 
 #include <stdint.h>
 
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
 #include <stm32.h>
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
 __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
 	{
 		.base = STM32_TIM2_BASE,
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
index e05ddecf3..17e6862f7 100644
--- a/src/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -53,7 +53,7 @@
 #include "up_arch.h"
 #include "chip.h"
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 /************************************************************************************
  * Public Functions
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
index 0be981c1e..0fc8569aa 100644
--- a/src/drivers/boards/px4fmu/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
@@ -53,7 +53,7 @@
 
 #include "up_arch.h"
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 /************************************************************************************
  * Definitions
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
new file mode 100644
index 000000000..ec8dde499
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   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 board_config.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO	GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO	GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE	STM32_USART6_BASE	/* hardwired on the board */
+#define PX4IO_SERIAL_VECTOR	STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP	DMAMAP_USART6_TX_2
+#define PX4IO_SERIAL_RX_DMAMAP	DMAMAP_USART6_RX_2
+#define PX4IO_SERIAL_CLOCK	STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE	1500000			/* 1.5Mbps -> max rate for IO */
+
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_BARO		3
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION	1
+#define PX4_I2C_BUS_LED		2
+
+/* Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED	0x55
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_SERVO_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7)
+#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER	2	/* timer 2 */
+#define TONE_ALARM_CHANNEL	1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9  : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+
+/* 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)
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
new file mode 100644
index 000000000..99d37eeca
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu2_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c \
+		   px4fmu2_led.c
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
new file mode 100644
index 000000000..ae2a645f7
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ *   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/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	stm32_configgpio(GPIO_ADC1_IN10);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+
+	/* configure power supply control/sense pins */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	stm32_configgpio(GPIO_VDD_SERVO_VALID);
+	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\n");
+
+	/* Get the SPI port for the FRAM */
+
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi2, 375000000);
+	SPI_SETBITS(spi2, 8);
+	SPI_SETMODE(spi2, SPIDEV_MODE3);
+	SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+	message("[boot] Successfully initialized SPI port 2\n");
+
+	#ifdef CONFIG_MMCSD
+	/* First, get an instance of the SDIO interface */
+
+	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+	if (!sdio) {
+		message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
+			CONFIG_NSH_MMCSDSLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+	if (ret != OK) {
+		message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+	sdio_mediachange(sdio, true);
+
+	message("[boot] Initialized SDIO\n");
+	#endif
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
new file mode 100644
index 000000000..a856ccb02
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1 GPIO for output */
+
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
new file mode 100644
index 000000000..f66c7cd79
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
\ No newline at end of file
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..600dfef41
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
new file mode 100644
index 000000000..09838d02f
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
new file mode 100644
index 000000000..f329e06ff
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/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.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h
index 6638e715e..48aadbd76 100644
--- a/src/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
  
 /**
- * @file px4io_internal.h
+ * @file board_config.h
  * 
  * PX4IO hardware definitions.
  */
@@ -47,7 +47,9 @@
 #include <nuttx/compiler.h>
 #include <stdint.h>
 
+/* these headers are not C++ safe */
 #include <stm32.h>
+#include <arch/board/board.h>
 
 /************************************************************************************
  * Definitions
@@ -83,3 +85,11 @@
 
 #define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
 #define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15..2601a1c15 100644
--- a/src/drivers/boards/px4io/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c
index 15c59e423..8292da9e1 100644
--- a/src/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io-v1/px4io_init.c
@@ -55,7 +55,7 @@
 #include <nuttx/arch.h>
 
 #include "stm32.h"
-#include "px4io_internal.h"
+#include "board_config.h"
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
index 6df470da6..6df470da6 100644
--- a/src/drivers/boards/px4io/px4io_pwm_servo.c
+++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
new file mode 100644
index 000000000..4d41d0d07
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   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 px4iov2_internal.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE	1500000
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS  **********************************************************************/
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+/* Safety switch button *******************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+
+/* the same rssi signal goes to both an adc and a timer input */
+#define GPIO_ADC_RSSI   (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_TIM_RSSI   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
+
+/* PWM pins  **************************************************************/
+
+#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+/* SBUS pins  *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED       0  /* LED? */
+#define LED_HEAPALLOCATE  1  /* LED? */
+#define LED_IRQSENABLED   2  /* LED? + LED? */
+#define LED_STACKCREATED  3  /* LED? */
+#define LED_INIRQ         4  /* LED? + LED? */
+#define LED_SIGNAL        5  /* LED? + LED? */
+#define LED_ASSERTION     6  /* LED? + LED? + LED? */
+#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
new file mode 100644
index 000000000..85f94e8be
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IOv2
+#
+
+SRCS		= px4iov2_init.c \
+		  px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
new file mode 100644
index 000000000..ccd01edf5
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4iov2_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 <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+
+	/* configure GPIOs */
+
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	/* spektrum power enable is active high - enable it by default */
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+
+	/* RSSI inputs */
+	stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+	stm32_configgpio(GPIO_ADC_RSSI);
+
+	/* servo rail voltage */
+	stm32_configgpio(GPIO_ADC_VSERVO);
+
+	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
+	stm32_configgpio(GPIO_SBUS_OUTPUT);
+	
+	/* sbus output enable is active low - disable it by default */
+	stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
+	stm32_configgpio(GPIO_SBUS_OENABLE);
+
+	stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_PWM1, false);
+	stm32_configgpio(GPIO_PWM1);
+
+	stm32_gpiowrite(GPIO_PWM2, false);
+	stm32_configgpio(GPIO_PWM2);
+
+	stm32_gpiowrite(GPIO_PWM3, false);
+	stm32_configgpio(GPIO_PWM3);
+
+	stm32_gpiowrite(GPIO_PWM4, false);
+	stm32_configgpio(GPIO_PWM4);
+
+	stm32_gpiowrite(GPIO_PWM5, false);
+	stm32_configgpio(GPIO_PWM5);
+
+	stm32_gpiowrite(GPIO_PWM6, false);
+	stm32_configgpio(GPIO_PWM6);
+
+	stm32_gpiowrite(GPIO_PWM7, false);
+	stm32_configgpio(GPIO_PWM7);
+
+	stm32_gpiowrite(GPIO_PWM8, false);
+	stm32_configgpio(GPIO_PWM8);
+}
diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
new file mode 100644
index 000000000..4f1b9487c
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file px4iov2_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
index dc0c84052..a9e22eaa6 100644
--- a/src/drivers/device/ringbuffer.h
+++ b/src/drivers/device/ringbuffer.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,15 +34,14 @@
 /**
  * @file ringbuffer.h
  *
- * A simple ringbuffer template.
+ * A flexible ringbuffer class.
  */
 
 #pragma once
 
-template<typename T>
 class RingBuffer {
 public:
-	RingBuffer(unsigned size);
+	RingBuffer(unsigned ring_size, size_t entry_size);
 	virtual ~RingBuffer();
 
 	/**
@@ -52,15 +50,37 @@ public:
 	 * @param val		Item to put
 	 * @return		true if the item was put, false if the buffer is full
 	 */
-	bool			put(T &val);
+	bool			put(const void *val, size_t val_size = 0);
+
+	bool			put(int8_t val);
+	bool			put(uint8_t val);
+	bool			put(int16_t val);
+	bool			put(uint16_t val);
+	bool			put(int32_t val);
+	bool			put(uint32_t val);
+	bool			put(int64_t val);
+	bool			put(uint64_t val);
+	bool			put(float val);
+	bool			put(double val);
 
 	/**
-	 * Put an item into the buffer.
+	 * Force an item into the buffer, discarding an older item if there is not space.
 	 *
 	 * @param val		Item to put
-	 * @return		true if the item was put, false if the buffer is full
+	 * @return		true if an item was discarded to make space
 	 */
-	bool			put(const T &val);
+	bool			force(const void *val, size_t val_size = 0);
+
+	bool			force(int8_t val);
+	bool			force(uint8_t val);
+	bool			force(int16_t val);
+	bool			force(uint16_t val);
+	bool			force(int32_t val);
+	bool			force(uint32_t val);
+	bool			force(int64_t val);
+	bool			force(uint64_t val);
+	bool			force(float val);
+	bool			force(double val);
 
 	/**
 	 * Get an item from the buffer.
@@ -68,15 +88,18 @@ public:
 	 * @param val		Item that was gotten
 	 * @return		true if an item was got, false if the buffer was empty.
 	 */
-	bool			get(T &val);
+	bool			get(void *val, size_t val_size = 0);
 
-	/**
-	 * Get an item from the buffer (scalars only).
-	 *
-	 * @return		The value that was fetched, or zero if the buffer was
-	 *			empty.
-	 */
-	T			get(void);
+	bool			get(int8_t &val);
+	bool			get(uint8_t &val);
+	bool			get(int16_t &val);
+	bool			get(uint16_t &val);
+	bool			get(int32_t &val);
+	bool			get(uint32_t &val);
+	bool			get(int64_t &val);
+	bool			get(uint64_t &val);
+	bool			get(float &val);
+	bool			get(double &val);
 
 	/*
 	 * Get the number of slots free in the buffer.
@@ -97,54 +120,103 @@ public:
 	/*
 	 * Returns true if the buffer is empty.
 	 */
-	bool			empty() { return _tail == _head; }
+	bool			empty();
 
 	/*
 	 * Returns true if the buffer is full.
 	 */
-	bool			full() { return _next(_head) == _tail; }
+	bool			full();
 
 	/*
 	 * Returns the capacity of the buffer, or zero if the buffer could
 	 * not be allocated.
 	 */
-	unsigned		size() { return (_buf != nullptr) ? _size : 0; }
+	unsigned		size();
 
 	/*
 	 * Empties the buffer.
 	 */
-	void			flush() { _head = _tail = _size; }
+	void			flush();
+
+	/*
+	 * resize the buffer. This is unsafe to be called while
+	 * a producer or consuming is running. Caller is responsible
+	 * for any locking needed
+	 *
+	 * @param new_size	new size for buffer
+	 * @return		true if the resize succeeds, false if
+	 * 			not (allocation error)
+	 */
+	bool			resize(unsigned new_size);
+
+	/*
+	 * printf() some info on the buffer
+	 */
+	void			print_info(const char *name);
 
 private:
-	T			*const _buf;	
-	const unsigned		_size;
-	volatile unsigned	_head;	/**< insertion point */
-	volatile unsigned	_tail;	/**< removal point */
+	unsigned		_num_items;
+	const size_t		_item_size;
+	char			*_buf;	
+	volatile unsigned	_head;	/**< insertion point in _item_size units */
+	volatile unsigned	_tail;	/**< removal point in _item_size units */
 
 	unsigned		_next(unsigned index);
 };
 
-template <typename T>
-RingBuffer<T>::RingBuffer(unsigned with_size) :
-	_buf(new T[with_size + 1]),
-	_size(with_size),
- 	_head(with_size),
- 	_tail(with_size)
+RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
+	_num_items(num_items),
+	_item_size(item_size),
+	_buf(new char[(_num_items+1) * item_size]),
+ 	_head(_num_items),
+ 	_tail(_num_items)
 {}
 
-template <typename T>
-RingBuffer<T>::~RingBuffer()
+RingBuffer::~RingBuffer()
 {
 	if (_buf != nullptr)
 		delete[] _buf;
 }
 
-template <typename T>
-bool RingBuffer<T>::put(T &val) 
+unsigned
+RingBuffer::_next(unsigned index)
+{
+	return (0 == index) ? _num_items : (index - 1); 
+}
+
+bool
+RingBuffer::empty()
+{
+	return _tail == _head; 
+}
+
+bool
+RingBuffer::full()
+{
+	return _next(_head) == _tail; 
+}
+
+unsigned
+RingBuffer::size()
+{
+	return (_buf != nullptr) ? _num_items : 0; 
+}
+
+void
+RingBuffer::flush()
+{
+	while (!empty())
+		get(NULL);
+}
+
+bool
+RingBuffer::put(const void *val, size_t val_size) 
 {
 	unsigned next = _next(_head);
 	if (next != _tail) {
-		_buf[_head] = val;
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+		memcpy(&_buf[_head * _item_size], val, val_size);
 		_head = next;
 		return true;
 	} else {
@@ -152,52 +224,286 @@ bool RingBuffer<T>::put(T &val)
 	}
 }
 
-template <typename T>
-bool RingBuffer<T>::put(const T &val) 
+bool
+RingBuffer::put(int8_t val)
 {
-	unsigned next = _next(_head);
-	if (next != _tail) {
-		_buf[_head] = val;
-		_head = next;
-		return true;
-	} else {
-		return false;
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(float val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(double val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(const void *val, size_t val_size)
+{
+	bool overwrote = false;
+
+	for (;;) {
+		if (put(val, val_size))
+			break;
+		get(NULL);
+		overwrote = true;
 	}
+	return overwrote;
+}
+
+bool
+RingBuffer::force(int8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint32_t val)
+{
+	return force(&val, sizeof(val));
 }
 
-template <typename T>
-bool RingBuffer<T>::get(T &val) 
+bool
+RingBuffer::force(int64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(float val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(double val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(void *val, size_t val_size) 
 {
 	if (_tail != _head) {
-		val = _buf[_tail];
-		_tail = _next(_tail);
+		unsigned candidate;
+		unsigned next;
+
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+
+		do {
+			/* decide which element we think we're going to read */
+			candidate = _tail;
+
+			/* and what the corresponding next index will be */
+			next = _next(candidate);
+
+			/* go ahead and read from this index */
+			if (val != NULL)
+				memcpy(val, &_buf[candidate * _item_size], val_size);
+
+			/* if the tail pointer didn't change, we got our item */
+		} while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
+
 		return true;
 	} else {
 		return false;
 	}
 }
 
-template <typename T>
-T RingBuffer<T>::get(void) 
+bool
+RingBuffer::get(int8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint8_t &val)
 {
-	T val;
-	return get(val) ? val : 0;
+	return get(&val, sizeof(val));
 }
 
-template <typename T>
-unsigned RingBuffer<T>::space(void) 
+bool
+RingBuffer::get(int16_t &val)
 {
-	return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1);
+	return get(&val, sizeof(val));
 }
 
-template <typename T>
-unsigned RingBuffer<T>::count(void) 
+bool
+RingBuffer::get(uint16_t &val)
 {
-	return _size - space();
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(float &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(double &val)
+{
+	return get(&val, sizeof(val));
+}
+
+unsigned
+RingBuffer::space(void) 
+{
+	unsigned tail, head;
+
+	/*
+	 * Make a copy of the head/tail pointers in a fashion that
+	 * may err on the side of under-estimating the free space
+	 * in the buffer in the case that the buffer is being updated
+	 * asynchronously with our check.
+	 * If the head pointer changes (reducing space) while copying,
+	 * re-try the copy.
+	 */
+	do {
+		head = _head;
+		tail = _tail;
+	} while (head != _head);
+
+	return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
+}
+
+unsigned
+RingBuffer::count(void) 
+{
+	/*
+	 * Note that due to the conservative nature of space(), this may
+	 * over-estimate the number of items in the buffer.
+	 */
+	return _num_items - space();
+}
+
+bool
+RingBuffer::resize(unsigned new_size) 
+{
+	char *old_buffer;
+	char *new_buffer = new char [(new_size+1) * _item_size];
+	if (new_buffer == nullptr) {
+		return false;
+	}
+	old_buffer = _buf;
+	_buf = new_buffer;
+	_num_items = new_size;
+	_head = new_size;
+	_tail = new_size;
+	delete[] old_buffer;
+	return true;
 }
 
-template <typename T>
-unsigned RingBuffer<T>::_next(unsigned index) 
+void
+RingBuffer::print_info(const char *name) 
 {
-	return (0 == index) ? _size : (index - 1);
+	printf("%s	%u/%u (%u/%u @ %p)\n",
+	       name, 
+	       _num_items, 
+	       _num_items * _item_size, 
+	       _head, 
+	       _tail, 
+	       _buf);
 }
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 8fffd60cb..fa6b78d64 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -67,6 +67,7 @@ SPI::SPI(const char *name,
 	CDev(name, devname, irq),
 	// public
 	// protected
+	locking_mode(LOCK_PREEMPTION),
 	// private
 	_bus(bus),
 	_device(device),
@@ -132,13 +133,25 @@ SPI::probe()
 int
 SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
+	irqstate_t	state;
 
 	if ((send == nullptr) && (recv == nullptr))
 		return -EINVAL;
 
-	/* do common setup */
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, true);
+	/* lock the bus as required */
+	if (!up_interrupt_context()) {
+		switch (locking_mode) {
+		default:
+		case LOCK_PREEMPTION:
+			state = irqsave();
+			break;
+		case LOCK_THREADS:
+			SPI_LOCK(_dev, true);
+			break;
+		case LOCK_NONE:
+			break;
+		}
+	}
 
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
@@ -151,8 +164,19 @@ 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);
+	if (!up_interrupt_context()) {
+		switch (locking_mode) {
+		default:
+		case LOCK_PREEMPTION:
+			irqrestore(state);
+			break;
+		case LOCK_THREADS:
+			SPI_LOCK(_dev, false);
+			break;
+		case LOCK_NONE:
+			break;
+		}
+	}
 
 	return OK;
 }
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index e0122372a..9103dca2e 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -101,6 +101,17 @@ protected:
 	 */
 	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
 
+	/**
+	 * Locking modes supported by the driver.
+	 */
+	enum LockMode {
+		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
+		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
+		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
+	};
+
+	LockMode	locking_mode;	/**< selected locking mode */
+
 private:
 	int			_bus;
 	enum spi_dev_e		_device;
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 794de584b..7d93ed938 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Accelerometer driver interface.
+ * @file drv_accel.h
+ *
+ * Accelerometer driver interface.
  */
 
 #ifndef _DRV_ACCEL_H
@@ -52,6 +54,7 @@
  */
 struct accel_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< acceleration in the NED X board axis in m/s^2 */
 	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
 	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
@@ -65,7 +68,7 @@ struct accel_report {
 	int16_t temperature_raw;
 };
 
-/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
+/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
 struct accel_scale {
 	float	x_offset;
 	float	x_scale;
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index 7bb9ee2af..78f31495d 100644
--- a/src/drivers/drv_airspeed.h
+++ b/src/drivers/drv_airspeed.h
@@ -32,7 +32,10 @@
  ****************************************************************************/
 
 /**
- * @file Airspeed driver interface.
+ * @file drv_airspeed.h
+ *
+ * Airspeed driver interface.
+ * 
  * @author Simon Wilks
  */
 
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index 176f798c0..e2f0137ae 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Barometric pressure sensor driver interface.
+ * @file drv_baro.h
+ *
+ * Barometric pressure sensor driver interface.
  */
 
 #ifndef _DRV_BARO_H
@@ -55,6 +57,7 @@ struct baro_report {
 	float altitude;
 	float temperature;
 	uint64_t timestamp;
+	uint64_t error_count;
 };
 
 /*
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 510983d4b..37af26d52 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -59,17 +59,47 @@
 # define GPIO_CAN_RX		(1<<7)		/**< CAN2 RX */
 
 /**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
  */
 # define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
 # define PX4IO_DEVICE_PATH	"/dev/px4io"
 
 #endif
 
-#ifndef PX4IO_DEVICE_PATH
-#  error No GPIO support for this board.
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/*
+ * PX4FMUv2 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1		(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2		(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3		(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4		(1<<3)		/**< servo 4 output */
+# define GPIO_SERVO_5		(1<<4)		/**< servo 5 output */
+# define GPIO_SERVO_6		(1<<5)		/**< servo 6 output */
+
+# define GPIO_5V_PERIPH_EN	(1<<6)		/**< PA8 - !VDD_5V_PERIPH_EN */
+# define GPIO_3V3_SENSORS_EN	(1<<7)		/**< PE3 - VDD_3V3_SENSORS_EN */
+# define GPIO_BRICK_VALID	(1<<8)		/**< PB5 - !VDD_BRICK_VALID */
+# define GPIO_SERVO_VALID	(1<<9)		/**< PB7 - !VDD_SERVO_VALID */
+# define GPIO_5V_HIPOWER_OC	(1<<10)		/**< PE10 - !VDD_5V_HIPOWER_OC */
+# define GPIO_5V_PERIPH_OC	(1<<11)		/**< PE10 - !VDD_5V_PERIPH_OC */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+# define PX4IO_DEVICE_PATH	"/dev/px4io"
+
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+/* no GPIO driver on the PX4IOv1 board */
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+/* no GPIO driver on the PX4IOv2 board */
 #endif
 
 /*
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..06e3535b3 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file GPS driver interface.
+ * @file drv_gps.h
+ *
+ * GPS driver interface.
  */
 
 #ifndef _DRV_GPS_H
@@ -51,8 +53,7 @@
 typedef enum {
 	GPS_DRIVER_MODE_NONE = 0,
 	GPS_DRIVER_MODE_UBX,
-	GPS_DRIVER_MODE_MTK,
-	GPS_DRIVER_MODE_NMEA,
+	GPS_DRIVER_MODE_MTK
 } gps_driver_mode_t;
 
 
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 1d0fef6fc..2ae8c7058 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Gyroscope driver interface.
+ * @file drv_gyro.h
+ *
+ * Gyroscope driver interface.
  */
 
 #ifndef _DRV_GYRO_H
@@ -52,6 +54,7 @@
  */
 struct gyro_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< angular velocity in the NED X board axis in rad/s */
 	float y;		/**< angular velocity in the NED Y board axis in rad/s */
 	float z;		/**< angular velocity in the NED Z board axis in rad/s */
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
index 21044f620..4ce04696e 100644
--- a/src/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
@@ -54,12 +54,13 @@
 
 #define LED_ON			_IOC(_LED_BASE, 0)
 #define LED_OFF			_IOC(_LED_BASE, 1)
+#define LED_TOGGLE		_IOC(_LED_BASE, 2)
 
 __BEGIN_DECLS
 
 /*
  * Initialise the LED driver.
  */
-__EXPORT extern void drv_led_start(void);
+__EXPORT void drv_led_start(void);
 
 __END_DECLS
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a1..06107bd3d 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -54,6 +54,7 @@
  */
 struct mag_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;
 	float y;
 	float z;
@@ -90,25 +91,37 @@ ORB_DECLARE(sensor_mag);
 /** set the mag internal sample rate to at least (arg) Hz */
 #define MAGIOCSSAMPLERATE	_MAGIOC(0)
 
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE	_MAGIOC(1)
+
 /** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS		_MAGIOC(1)
+#define MAGIOCSLOWPASS		_MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS		_MAGIOC(3)
 
 /** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE		_MAGIOC(2)
+#define MAGIOCSSCALE		_MAGIOC(4)
 
 /** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE		_MAGIOC(3)
+#define MAGIOCGSCALE		_MAGIOC(5)
 
 /** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE		_MAGIOC(4)
+#define MAGIOCSRANGE		_MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE		_MAGIOC(7)
 
 /** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE		_MAGIOC(5)
+#define MAGIOCCALIBRATE		_MAGIOC(8)
 
 /** excite strap */
-#define MAGIOCEXSTRAP		_MAGIOC(6)
+#define MAGIOCEXSTRAP		_MAGIOC(9)
 
 /** perform self test and report status */
-#define MAGIOCSELFTEST		_MAGIOC(7)
+#define MAGIOCSELFTEST		_MAGIOC(10)
+
+/** determine if mag is external or onboard */
+#define MAGIOCGEXTERNAL		_MAGIOC(11)
 
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index ec9d4ca09..51f916f37 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -65,6 +65,36 @@ __BEGIN_DECLS
 #define PWM_OUTPUT_MAX_CHANNELS	16
 
 /**
+ * Lowest minimum PWM in us
+ */
+#define PWM_LOWEST_MIN 900
+
+/**
+ * Default minimum PWM in us
+ */
+#define PWM_DEFAULT_MIN 1000
+
+/**
+ * Highest PWM allowed as the minimum PWM
+ */
+#define PWM_HIGHEST_MIN 1300
+
+/**
+ * Highest maximum PWM in us
+ */
+#define PWM_HIGHEST_MAX 2100
+
+/**
+ * Default maximum PWM in us
+ */
+#define PWM_DEFAULT_MAX 2000
+
+/**
+ * Lowest PWM allowed as the maximum PWM
+ */
+#define PWM_LOWEST_MAX 1700
+
+/**
  * Servo output signal type, value is actual servo output pulse
  * width in microseconds.
  */
@@ -79,6 +109,7 @@ typedef uint16_t	servo_position_t;
 struct pwm_output_values {
 	/** desired pulse widths for each of the supported channels */
 	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
+	unsigned			channel_count;
 };
 
 /*
@@ -100,26 +131,63 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
+/** get default servo update rate */
+#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
 /** set alternate servo update rate */
-#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3)
+
+/** get alternate servo update rate */
+#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
 
 /** get the number of servos in *(unsigned *)arg */
-#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
+#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 5)
 
 /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
-#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6)
+
+/** check the selected update rates */
+#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7)
 
 /** set the 'ARM ok' bit, which activates the safety switch */
-#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 5)
+#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 8)
 
 /** clear the 'ARM ok' bit, which deactivates the safety switch */
-#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
+#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 9)
 
 /** start DSM bind */
-#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
+#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 10)
+
+#define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
+#define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+#define DSMX8_BIND_PULSES 10	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
+
+/** power up DSM receiver */
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
+
+/** set the PWM value for failsafe */
+#define PWM_SERVO_SET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 12)
+
+/** get the PWM value for failsafe */
+#define PWM_SERVO_GET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 13)
+
+/** set the PWM value when disarmed - should be no PWM (zero) by default */
+#define PWM_SERVO_SET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 14)
+
+/** get the PWM value when disarmed */
+#define PWM_SERVO_GET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 15)
+
+/** set the minimum PWM value the output will send */
+#define PWM_SERVO_SET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 16)
+
+/** get the minimum PWM value the output will send */
+#define PWM_SERVO_GET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 17)
+
+/** set the maximum PWM value the output will send */
+#define PWM_SERVO_SET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 18)
 
-/** Power up DSM receiver */
-#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
+/** get the maximum PWM value the output will send */
+#define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 19)
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 03a82ec5d..cf91f7030 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -52,6 +52,7 @@
  */
 struct range_finder_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float distance; 			/** in meters */
 	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
 };
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 4decc324e..78ffad9d7 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -32,8 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file R/C input interface.
+ * @file drv_rc_input.h
  *
+ * R/C input interface.
  */
 
 #ifndef _DRV_RC_INPUT_H
diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h
new file mode 100644
index 000000000..07c6186dd
--- /dev/null
+++ b/src/drivers/drv_rgbled.h
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_rgbled.h
+ *
+ * RGB led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* more devices will be 1, 2, etc */
+#define RGBLED_DEVICE_PATH "/dev/rgbled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _RGBLEDIOCBASE		(0x2900)
+#define _RGBLEDIOC(_n)		(_IOC(_RGBLEDIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define RGBLED_PLAY_SCRIPT_NAMED	_RGBLEDIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define RGBLED_PLAY_SCRIPT		_RGBLEDIOC(2)
+
+/** 
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define RGBLED_SET_USER_SCRIPT		_RGBLEDIOC(3)
+
+/** set constant RGB values */
+#define RGBLED_SET_RGB			_RGBLEDIOC(4)
+
+/** set color */
+#define RGBLED_SET_COLOR		_RGBLEDIOC(5)
+
+/** set blink speed */
+#define RGBLED_SET_MODE			_RGBLEDIOC(6)
+
+/** set pattern */
+#define RGBLED_SET_PATTERN		_RGBLEDIOC(7)
+
+
+/* 
+  structure passed to RGBLED_SET_RGB ioctl()
+  Note that the driver scales the brightness to 0 to 255, regardless
+  of the hardware scaling
+ */
+typedef struct {
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+} rgbled_rgbset_t;
+
+/* enum passed to RGBLED_SET_COLOR ioctl()*/
+typedef enum {
+	RGBLED_COLOR_OFF,
+	RGBLED_COLOR_RED,
+	RGBLED_COLOR_YELLOW,
+	RGBLED_COLOR_PURPLE,
+	RGBLED_COLOR_GREEN,
+	RGBLED_COLOR_BLUE,
+	RGBLED_COLOR_WHITE,
+	RGBLED_COLOR_AMBER,
+	RGBLED_COLOR_DIM_RED,
+	RGBLED_COLOR_DIM_YELLOW,
+	RGBLED_COLOR_DIM_PURPLE,
+	RGBLED_COLOR_DIM_GREEN,
+	RGBLED_COLOR_DIM_BLUE,
+	RGBLED_COLOR_DIM_WHITE,
+	RGBLED_COLOR_DIM_AMBER
+} rgbled_color_t;
+
+/* enum passed to RGBLED_SET_MODE ioctl()*/
+typedef enum {
+	RGBLED_MODE_OFF,
+	RGBLED_MODE_ON,
+	RGBLED_MODE_BLINK_SLOW,
+	RGBLED_MODE_BLINK_NORMAL,
+	RGBLED_MODE_BLINK_FAST,
+	RGBLED_MODE_BREATHE,
+	RGBLED_MODE_PATTERN
+} rgbled_mode_t;
+
+#define RGBLED_PATTERN_LENGTH 20
+
+typedef struct {
+	rgbled_color_t color[RGBLED_PATTERN_LENGTH];
+	unsigned duration[RGBLED_PATTERN_LENGTH];
+} rgbled_pattern_t;
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 3a89cab9d..8e8b2c1b9 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Common sensor API and ioctl definitions.
+ * @file drv_sensor.h
+ *
+ * Common sensor API and ioctl definitions.
  */
 
 #ifndef _DRV_SENSOR_H
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 0c6afc64c..cb5fd53a5 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -31,7 +31,9 @@
  *
  ****************************************************************************/
 
-/*
+/**
+ * @file drv_tone_alarm.h
+ *
  * Driver for the PX4 audio alarm port, /dev/tone_alarm.
  *
  * The tone_alarm driver supports a set of predefined "alarm"
@@ -60,6 +62,8 @@
 
 #include <sys/ioctl.h>
 
+#define TONEALARM_DEVICE_PATH "/dev/tone_alarm"
+
 #define _TONE_ALARM_BASE	0x7400
 #define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
 
@@ -125,4 +129,25 @@ enum tone_pitch {
 	TONE_NOTE_MAX
 };
 
+enum {
+	TONE_STOP_TUNE = 0,
+	TONE_STARTUP_TUNE,
+	TONE_ERROR_TUNE,
+	TONE_NOTIFY_POSITIVE_TUNE,
+	TONE_NOTIFY_NEUTRAL_TUNE,
+	TONE_NOTIFY_NEGATIVE_TUNE,
+	/* Do not include these unused tunes
+	TONE_CHARGE_TUNE,
+	TONE_DIXIE_TUNE,
+	TONE_CUCURACHA_TUNE,
+	TONE_YANKEE_TUNE,
+	TONE_DAISY_TUNE,
+	TONE_WILLIAM_TELL_TUNE, */
+	TONE_ARMING_WARNING_TUNE,
+	TONE_BATTERY_WARNING_SLOW_TUNE,
+	TONE_BATTERY_WARNING_FAST_TUNE,
+	TONE_GPS_WARNING_TUNE,
+	TONE_NUMBER_OF_TUNES
+};
+
 #endif /* DRV_TONE_ALARM_H_ */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index cd72d9d23..9d8ad084e 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -59,7 +59,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
@@ -68,6 +68,7 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -131,11 +132,8 @@ ETSAirspeed::measure()
 	if (OK != ret) {
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
-		return ret;
 	}
 
-	ret = OK;
-
 	return ret;
 }
 
@@ -152,48 +150,44 @@ ETSAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
 		return ret;
 	}
 
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
         if (diff_pres_pa == 0) {
-            // a zero value means the pressure sensor cannot give us a
-            // value. We need to return, and not report a value or the
-            // caller could end up using this value as part of an
-            // average
-            log("zero value from sensor"); 
-            return -1;
+		// a zero value means the pressure sensor cannot give us a
+		// value. We need to return, and not report a value or the
+		// caller could end up using this value as part of an
+		// average
+		perf_count(_comms_errors);
+		log("zero value from sensor"); 
+		return -1;
         }
 
 	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
-
 	} else {
 		diff_pres_pa -= _diff_pres_offset;
 	}
 
-	// XXX we may want to smooth out the readings to remove noise.
-
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
-
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	if (diff_pres_pa > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_pres_pa;
 	}
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+	// XXX we may want to smooth out the readings to remove noise.
+	differential_pressure_s report;
+	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
+	report.differential_pressure_pa = diff_pres_pa;
+	report.voltage = 0;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	/* announce the airspeed if needed, just publish else */
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
-	}
+	new_report(report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
@@ -213,7 +207,7 @@ ETSAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
 			return;
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 38835418b..fc500a9ec 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -85,7 +85,7 @@ static const int ERROR = -1;
 class GPS : public device::CDev
 {
 public:
-	GPS(const char* uart_path);
+	GPS(const char *uart_path);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -156,7 +156,7 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char* uart_path) :
+GPS::GPS(const char *uart_path) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
@@ -192,6 +192,7 @@ GPS::~GPS()
 	/* well, kill it anyway, though this will probably crash */
 	if (_task != -1)
 		task_delete(_task);
+
 	g_dev = nullptr;
 
 }
@@ -270,19 +271,20 @@ GPS::task_main()
 		}
 
 		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
-				break;
-			case GPS_DRIVER_MODE_MTK:
-				_Helper = new MTK(_serial_fd, &_report);
-				break;
-			case GPS_DRIVER_MODE_NMEA:
-				//_Helper = new NMEA(); //TODO: add NMEA
-				break;
-			default:
-				break;
+		case GPS_DRIVER_MODE_UBX:
+			_Helper = new UBX(_serial_fd, &_report);
+			break;
+
+		case GPS_DRIVER_MODE_MTK:
+			_Helper = new MTK(_serial_fd, &_report);
+			break;
+
+		default:
+			break;
 		}
+
 		unlock();
+
 		if (_Helper->configure(_baudrate) == 0) {
 			unlock();
 
@@ -294,6 +296,7 @@ GPS::task_main()
 				/* opportunistic publishing - else invalid data would end up on the bus */
 				if (_report_pub > 0) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
 				} else {
 					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
 				}
@@ -310,10 +313,26 @@ GPS::task_main()
 				}
 
 				if (!_healthy) {
-					warnx("module found");
+					char *mode_str = "unknown";
+
+					switch (_mode) {
+					case GPS_DRIVER_MODE_UBX:
+						mode_str = "UBX";
+						break;
+
+					case GPS_DRIVER_MODE_MTK:
+						mode_str = "MTK";
+						break;
+
+					default:
+						break;
+					}
+
+					warnx("module found: %s", mode_str);
 					_healthy = true;
 				}
 			}
+
 			if (_healthy) {
 				warnx("module lost");
 				_healthy = false;
@@ -322,25 +341,26 @@ GPS::task_main()
 
 			lock();
 		}
+
 		lock();
 
 		/* select next mode */
 		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_mode = GPS_DRIVER_MODE_MTK;
-				break;
-			case GPS_DRIVER_MODE_MTK:
-				_mode = GPS_DRIVER_MODE_UBX;
-				break;
-		//				case GPS_DRIVER_MODE_NMEA:
-		//					_mode = GPS_DRIVER_MODE_UBX;
-		//					break;
-			default:
-				break;
+		case GPS_DRIVER_MODE_UBX:
+			_mode = GPS_DRIVER_MODE_MTK;
+			break;
+
+		case GPS_DRIVER_MODE_MTK:
+			_mode = GPS_DRIVER_MODE_UBX;
+			break;
+
+		default:
+			break;
 		}
 
 	}
-	debug("exiting");
+
+	warnx("exiting");
 
 	::close(_serial_fd);
 
@@ -361,23 +381,25 @@ void
 GPS::print_info()
 {
 	switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			warnx("protocol: UBX");
-			break;
-		case GPS_DRIVER_MODE_MTK:
-			warnx("protocol: MTK");
-			break;
-		case GPS_DRIVER_MODE_NMEA:
-			warnx("protocol: NMEA");
-			break;
-		default:
-			break;
+	case GPS_DRIVER_MODE_UBX:
+		warnx("protocol: UBX");
+		break;
+
+	case GPS_DRIVER_MODE_MTK:
+		warnx("protocol: MTK");
+		break;
+
+	default:
+		break;
 	}
+
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+
 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
-			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
+				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -428,6 +450,7 @@ start(const char *uart_path)
 		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
 		goto fail;
 	}
+
 	exit(0);
 
 fail:
@@ -503,7 +526,7 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char* device_name = GPS_DEFAULT_UART_PORT;
+	char *device_name = GPS_DEFAULT_UART_PORT;
 
 	/*
 	 * Start/load the driver.
@@ -513,15 +536,18 @@ gps_main(int argc, char *argv[])
 		if (argc > 3) {
 			if (!strcmp(argv[2], "-d")) {
 				device_name = argv[3];
+
 			} else {
 				goto out;
 			}
 		}
+
 		gps::start(device_name);
 	}
 
 	if (!strcmp(argv[1], "stop"))
 		gps::stop();
+
 	/*
 	 * Test the driver/device.
 	 */
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index ba86d370a..2e2cbc8dd 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
-	warnx("try baudrate: %d\n", speed);
+		warnx("try baudrate: %d\n", speed);
 
 	default:
 		warnx("ERROR: Unsupported baudrate: %d\n", baud);
 		return -EINVAL;
 	}
+
 	struct termios uart_config;
+
 	int termios_state;
 
 	/* fill the struct for the new configuration */
@@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
 		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERROR setting baudrate (tcsetattr)\n");
 		return -1;
 	}
+
 	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
 	return 0;
 }
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index defc1a074..73d4b889c 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/** 
+/**
  * @file gps_helper.h
  */
 
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 62941d74b..56b702ea6 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -48,9 +48,9 @@
 
 
 MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_mtk_revision(0)
+	_fd(fd),
+	_gps_position(gps_position),
+	_mtk_revision(0)
 {
 	decode_init();
 }
@@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
@@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
 					handle_message(packet);
 					return 1;
 				}
+
 				/* in case we keep trying but only get crap from GPS */
-				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				if (time_started + timeout * 1000 < hrt_absolute_time()) {
 					return -1;
 				}
+
 				j++;
 			}
+
 			/* everything is read */
 			j = count = 0;
 		}
@@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 		if (b == MTK_SYNC1_V16) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 16;
+
 		} else if (b == MTK_SYNC1_V19) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 19;
@@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			add_byte_to_checksum(b);
 
 		// Fill packet buffer
-		((uint8_t*)(&packet))[_rx_count] = b;
+		((uint8_t *)(&packet))[_rx_count] = b;
 		_rx_count++;
 
 		/* Packet size minus checksum, XXX ? */
@@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			/* Compare checksum */
 			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
 				ret = 1;
+
 			} else {
 				warnx("MTK Checksum invalid");
 				ret = -1;
 			}
+
 			// Reset state machine to decode next packet
 			decode_init();
 		}
 	}
+
 	return ret;
 }
 
@@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	if (_mtk_revision == 16) {
 		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
 		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+
 	} else if (_mtk_revision == 19) {
 		_gps_position->lat = packet.latitude; // both degrees*1e7
 		_gps_position->lon = packet.longitude; // both degrees*1e7
+
 	} else {
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
 	}
+
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
 	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
 	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
-	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
 
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index b579db715..86291901c 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -42,13 +42,14 @@
  *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
  */
-
-#include <unistd.h>
-#include <stdio.h>
-#include <poll.h>
+#include <assert.h>
 #include <math.h>
+#include <poll.h>
+#include <stdio.h>
 #include <string.h>
-#include <assert.h>
+#include <time.h>
+#include <unistd.h>
+
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -59,13 +60,14 @@
 #define UBX_CONFIG_TIMEOUT		200		// ms, timeout for waiting ACK
 #define UBX_PACKET_TIMEOUT		2		// ms, if now data during this delay assume that full update received
 #define UBX_WAIT_BEFORE_READ	20		// ms, wait before reading to save read() calls
+#define DISABLE_MSG_INTERVAL	1000000	// us, try to disable message with this interval
 
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 	_fd(fd),
 	_gps_position(gps_position),
 	_configured(false),
 	_waiting_for_ack(false),
-	_disable_cmd_counter(0)
+	_disable_cmd_last(0)
 {
 	decode_init();
 }
@@ -190,35 +192,35 @@ UBX::configure(unsigned &baudrate)
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
+		warnx("ubx: msg rate configuration failed: NAV POSLLH");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
+		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SOL\n");
+		warnx("ubx: msg rate configuration failed: NAV SOL");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV VELNED\n");
+		warnx("ubx: msg rate configuration failed: NAV VELNED");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
+		warnx("ubx: msg rate configuration failed: NAV SVINFO");
 		return 1;
 	}
 
@@ -270,11 +272,17 @@ UBX::receive(unsigned timeout)
 
 		if (ret < 0) {
 			/* something went wrong when polling */
+			warnx("ubx: poll error");
 			return -1;
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			return handled ? 1 : -1;
+			if (handled) {
+				return 1;
+
+			} else {
+				return -1;
+			}
 
 		} else if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
@@ -291,8 +299,6 @@ UBX::receive(unsigned timeout)
 				/* pass received bytes to the packet decoder */
 				for (int i = 0; i < count; i++) {
 					if (parse_char(buf[i]) > 0) {
-						/* return to configure during configuration or to the gps driver during normal work
-						 * if a packet has arrived */
 						if (handle_message() > 0)
 							handled = true;
 					}
@@ -302,6 +308,7 @@ UBX::receive(unsigned timeout)
 
 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
+			warnx("ubx: timeout - no useful messages");
 			return -1;
 		}
 	}
@@ -453,6 +460,15 @@ UBX::handle_message()
 					timeinfo.tm_sec = packet->sec;
 					time_t epoch = mktime(&timeinfo);
 
+#ifndef CONFIG_RTC
+					//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+					//TODO generalize this by moving into gps.cpp?
+					timespec ts;
+					ts.tv_sec = epoch;
+					ts.tv_nsec = packet->time_nanoseconds;
+					clock_settime(CLOCK_REALTIME, &ts);
+#endif
+
 					_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
 					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 					_gps_position->timestamp_time = hrt_absolute_time();
@@ -554,10 +570,13 @@ UBX::handle_message()
 
 		if (ret == 0) {
 			/* message not handled */
-			warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
+			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+
+			hrt_abstime t = hrt_absolute_time();
 
-			if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
+			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
 				/* don't attempt for every message to disable, some might not be disabled */
+				_disable_cmd_last = t;
 				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
 				configure_message_rate(_message_class, _message_id, 0);
 			}
@@ -630,7 +649,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
 		ck_b = ck_b + ck_a;
 	}
 
-	/* The checksum is written to the last to bytes of a message */
+	/* the checksum is written to the last to bytes of a message */
 	message[length - 2] = ck_a;
 	message[length - 1] = ck_b;
 }
@@ -659,17 +678,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
 	ssize_t ret = 0;
 
-	/* Calculate the checksum now */
+	/* calculate the checksum now */
 	add_checksum_to_message(packet, length);
 
 	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
 
-	/* Start with the two sync bytes */
+	/* start with the two sync bytes */
 	ret += write(fd, sync_bytes, sizeof(sync_bytes));
 	ret += write(fd, packet, length);
 
 	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: config write fail");
+		warnx("ubx: configuration write fail");
 }
 
 void
@@ -686,7 +705,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
 	add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
 
-	// Configure receive check
+	/* configure ACK check */
 	_message_class_needed = msg_class;
 	_message_id_needed = msg_id;
 
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 4fc276975..76ef873a3 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -347,7 +347,7 @@ private:
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void			add_checksum_to_message(uint8_t* message, const unsigned length);
+	void			add_checksum_to_message(uint8_t *message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
@@ -358,7 +358,7 @@ private:
 
 	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
 
-	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+	void			add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
 
 	int			wait_for_ack(unsigned timeout);
 
@@ -376,7 +376,7 @@ private:
 	uint8_t			_message_class;
 	uint8_t			_message_id;
 	unsigned		_payload_size;
-	uint8_t			_disable_cmd_counter;
+	uint8_t			_disable_cmd_last;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index bd027ce0b..c1d73dd87 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,7 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
@@ -403,7 +404,7 @@ HIL::task_main()
 				for (unsigned i = 0; i < num_outputs; i++) {
 
 					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < (unsigned)outputs.noutputs &&
+					if (i < outputs.noutputs &&
 						isfinite(outputs.output[i]) &&
 						outputs.output[i] >= -1.0f &&
 						outputs.output[i] <= 1.0f) {
@@ -516,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		g_hil->set_pwm_rate(arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		// HIL always outputs at the alternate (usually faster) rate 
 		break;
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index ac3bdc132..5f0ce4ff8 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -58,13 +58,14 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
@@ -77,8 +78,8 @@
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
 
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
+/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
+#define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
 
 #define ADDR_CONF_A			0x00
 #define ADDR_CONF_B			0x01
@@ -148,10 +149,7 @@ private:
 	work_s			_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	mag_report		*_reports;
+	RingBuffer		*_reports;
 	mag_scale		_scale;
 	float 			_range_scale;
 	float 			_range_ga;
@@ -167,6 +165,8 @@ private:
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
+	int			_bus;			/**< the bus the device is connected to */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -190,6 +190,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the device
+	 */
+	int			reset();
+
+	/**
 	 * Perform the on-sensor scale calibration routine.
 	 *
 	 * @note The sensor will continue to provide measurements, these
@@ -303,9 +308,6 @@ private:
 
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
 /*
  * Driver 'main' command.
  */
@@ -315,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 HMC5883::HMC5883(int bus) :
 	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
@@ -326,7 +325,8 @@ HMC5883::HMC5883(int bus) :
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
 	_sensor_ok(false),
-	_calibrated(false)
+	_calibrated(false),
+	_bus(bus)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -348,9 +348,13 @@ HMC5883::~HMC5883()
 	/* make sure we are truly inactive */
 	stop();
 
-	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
 }
 
 int
@@ -363,24 +367,21 @@ HMC5883::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct mag_report[_num_reports];
-
+	_reports = new RingBuffer(2, sizeof(mag_report));
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
+	/* reset the device configuration */
+	reset();
 
 	/* 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]);
+	struct mag_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
 
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
-	/* set range */
-	set_range(_range_ga);
-
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -480,6 +481,7 @@ ssize_t
 HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct mag_report);
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -488,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
-
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
 		 * Note that we may be pre-empted by the workq thread while we are doing this;
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(mag_buf)) {
+				ret += sizeof(struct mag_report);
+				mag_buf++;
 			}
 		}
 
@@ -509,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 	/* manual measurement - run one conversion */
 	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -526,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 			break;
 		}
 
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
+		if (_reports->get(mag_buf)) {
+			ret = sizeof(struct mag_report);
+		}
 	} while (0);
 
 	return ret;
@@ -539,108 +538,105 @@ int
 HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
-
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
 
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
-				return OK;
+			/* switching to manual polling */
+		case SENSOR_POLLRATE_MANUAL:
+			stop();
+			_measure_ticks = 0;
+			return OK;
 
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
+			/* external signalling (DRDY) not supported */
+		case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
+			/* zero would be bad */
+		case 0:
+			return -EINVAL;
 
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* set default/max polling rate */
+		case SENSOR_POLLRATE_MAX:
+		case SENSOR_POLLRATE_DEFAULT: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+				/* set interval for next measurement to minimum legal value */
+				_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
+			}
 
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* adjust to a legal polling interval in Hz */
+		default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+				/* convert hz to tick interval via microseconds */
+				unsigned ticks = USEC2TICK(1000000 / arg);
 
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
-						return -EINVAL;
+				/* check against maximum rate */
+				if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+					return -EINVAL;
 
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
+				/* update interval for next measurement */
+				_measure_ticks = ticks;
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
 
-		return (1000 / _measure_ticks);
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
 			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
+			if ((arg < 1) || (arg > 100))
 				return -EINVAL;
 
-			/* allocate new buffer */
-			struct mag_report *buf = new struct mag_report[arg];
-
-			if (nullptr == buf)
+			irqstate_t flags = irqsave();
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
 				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
+			}
+			irqrestore(flags);
 
 			return OK;
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		return reset();
 
 	case MAGIOCSSAMPLERATE:
-		/* not supported, always 1 sample per poll */
-		return -EINVAL;
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+	case MAGIOCGSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case MAGIOCSRANGE:
 		return set_range(arg);
 
+	case MAGIOCGRANGE:
+		return _range_ga;
+
 	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
 		/* not supported, no internal filtering */
 		return -EINVAL;
 
@@ -665,6 +661,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSELFTEST:
 		return check_calibration();
 
+	case MAGIOCGEXTERNAL:
+		if (_bus == PX4_I2C_BUS_EXPANSION)
+			return 1;
+		else
+			return 0;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -676,7 +678,7 @@ HMC5883::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
@@ -688,6 +690,13 @@ HMC5883::stop()
 	work_cancel(HPWORK, &_work);
 }
 
+int
+HMC5883::reset()
+{
+	/* set range */
+	return set_range(_range_ga);
+}
+
 void
 HMC5883::cycle_trampoline(void *arg)
 {
@@ -778,9 +787,11 @@ HMC5883::collect()
 
 
 	perf_begin(_sample_perf);
+	struct mag_report new_report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	new_report.timestamp = hrt_absolute_time();
+        new_report.error_count = perf_event_count(_comms_errors);
 
 	/*
 	 * @note  We could read the status register here, which could tell us that
@@ -810,8 +821,10 @@ HMC5883::collect()
 	 */
 	if ((abs(report.x) > 2048) ||
 	    (abs(report.y) > 2048) ||
-	    (abs(report.z) > 2048))
+	    (abs(report.z) > 2048)) {
+		perf_count(_comms_errors);
 		goto out;
+	}
 
 	/*
 	 * RAW outputs
@@ -819,10 +832,10 @@ HMC5883::collect()
 	 * to align the sensor axes with the board, x and y need to be flipped
 	 * and y needs to be negated
 	 */
-	_reports[_next_report].x_raw = report.y;
-	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
+	new_report.x_raw = report.y;
+	new_report.y_raw = -report.x;
 	/* z remains z */
-	_reports[_next_report].z_raw = report.z;
+	new_report.z_raw = report.z;
 
 	/* scale values for output */
 
@@ -844,33 +857,30 @@ HMC5883::collect()
 #ifdef PX4_I2C_BUS_ONBOARD
 	if (_bus == PX4_I2C_BUS_ONBOARD) {
 		/* to align the sensor axes with the board, x and y need to be flipped */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 	} else {
 #endif
-		/* XXX axis assignment of external sensor is yet unknown */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
+		 * therefore switch x and y and invert y */
+		new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 #ifdef PX4_I2C_BUS_ONBOARD
 	}
 #endif
 
 	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
+	/* post a report to the ring */
+	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -929,11 +939,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	warnx("sampling 500 samples for scaling offset");
 
 	/* set the queue depth to 10 */
-	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
-		warn("failed to set queue depth");
-		ret = 1;
-		goto out;
-	}
+	/* don't do this for now, it can lead to a crash in start() respectively work_queue() */
+//	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
+//		warn("failed to set queue depth");
+//		ret = 1;
+//		goto out;
+//	}
 
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
@@ -1188,8 +1199,7 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
@@ -1293,7 +1303,12 @@ test()
 	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 	warnx("time:        %lld", report.timestamp);
 
-	/* set the queue depth to 10 */
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
+	/* set the queue depth to 5 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
 		errx(1, "failed to set queue depth");
 
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 57c256339..bb8d45bea 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -42,7 +42,7 @@
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <unistd.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 1ffca2f43..8f5674823 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -59,11 +59,12 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/device/ringbuffer.h>
 
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -71,6 +72,12 @@
 #endif
 static const int ERROR = -1;
 
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG	0
+#define SENSOR_BOARD_ROTATION_090_DEG	1
+#define SENSOR_BOARD_ROTATION_180_DEG	2
+#define SENSOR_BOARD_ROTATION_270_DEG	3
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -78,6 +85,7 @@ static const int ERROR = -1;
 
 /* register addresses */
 #define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
 
 #define ADDR_CTRL_REG1			0x20
@@ -85,8 +93,8 @@ static const int ERROR = -1;
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
-#define RATE_380HZ_LP_30HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
-#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_380HZ_LP_20HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (0<<5) | (0<<4))
 
 #define ADDR_CTRL_REG2			0x21
 #define ADDR_CTRL_REG3			0x22
@@ -147,6 +155,10 @@ static const int ERROR = -1;
 #define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
+#define L3GD20_DEFAULT_RATE			760
+#define L3GD20_DEFAULT_RANGE_DPS		2000
+#define L3GD20_DEFAULT_FILTER_FREQ		30
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -172,11 +184,8 @@ private:
 
 	struct hrt_call		_call;
 	unsigned		_call_interval;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct gyro_report	*_reports;
+	
+	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
@@ -184,10 +193,16 @@ private:
 	orb_advert_t		_gyro_topic;
 
 	unsigned		_current_rate;
-	unsigned		_current_range;
+	unsigned		_orientation;
+
+	unsigned		_read;
 
 	perf_counter_t		_sample_perf;
 
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -199,6 +214,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the driver
+	 */
+	void			reset();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -260,25 +280,37 @@ private:
 	 * @return		OK if the value can be supported.
 	 */
 	int			set_samplerate(unsigned frequency);
-};
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+	/**
+	 * Set the lowpass filter of the driver
+	 *
+	 * @param samplerate	The current samplerate
+	 * @param frequency	The cutoff frequency for the lowpass filter
+	 */
+	void			set_driver_lowpass_filter(float samplerate, float bandwidth);
 
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
+};
 
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
 	_current_rate(0),
-	_current_range(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
+	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
+	_read(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -299,7 +331,7 @@ L3GD20::~L3GD20()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -315,34 +347,17 @@ L3GD20::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct gyro_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(gyro_report));
 
 	if (_reports == nullptr)
 		goto out;
 
 	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
+	struct gyro_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
 
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
-
-        /* disable FIFO. This makes things simpler and ensures we
-         * aren't getting stale data. It means we must run the hrt
-         * callback fast enough to not miss data. */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);	
-
-	set_range(500);				/* default to 500dps */
-	set_samplerate(0);			/* max sample rate */
+	reset();
 
 	ret = OK;
 out:
@@ -355,8 +370,29 @@ L3GD20::probe()
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
-	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+	bool success = false;
+
+	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#else
+			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+		#endif
+
+		success = true;
+	}
+
+
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
+		success = true;
+	}
+
+	if (success)
 		return OK;
 
 	return -EIO;
@@ -366,6 +402,7 @@ ssize_t
 L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct gyro_report);
+	struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -381,10 +418,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(gbuf)) {
+				ret += sizeof(*gbuf);
+				gbuf++;
 			}
 		}
 
@@ -393,12 +429,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+	if (_reports->get(gbuf)) {
+		ret = sizeof(*gbuf);
+	}
 
 	return ret;
 }
@@ -427,8 +464,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -446,6 +482,11 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
 
+					/* adjust filters */
+					float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
 						start();
@@ -462,35 +503,26 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct gyro_report *buf = new struct gyro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
 
 	case GYROIOCSSAMPLERATE:
 		return set_samplerate(arg);
@@ -498,10 +530,16 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCGSAMPLERATE:
 		return _current_rate;
 
-	case GYROIOCSLOWPASS:
+	case GYROIOCSLOWPASS: {
+		float cutoff_freq_hz = arg;
+		float sample_rate = 1.0e6f / _call_interval;
+		set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+		return OK;
+	}
+
 	case GYROIOCGLOWPASS:
-		/* XXX not implemented due to wacky interaction with samplerate */
-		return -EINVAL;
+		return _gyro_filter_x.get_cutoff_freq();
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
@@ -514,10 +552,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
+		/* arg should be in dps */
 		return set_range(arg);
 
 	case GYROIOCGRANGE:
-		return _current_range;
+		/* convert to dps and round */
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
+
+	case GYROIOCSELFTEST:
+		return self_test();
 
 	default:
 		/* give it to the superclass */
@@ -563,28 +606,33 @@ int
 L3GD20::set_range(unsigned max_dps)
 {
 	uint8_t bits = REG4_BDU;
+	float new_range_scale_dps_digit;
+	float new_range;
 
-	if (max_dps == 0)
+	if (max_dps == 0) {
 		max_dps = 2000;
-
+	}
 	if (max_dps <= 250) {
-		_current_range = 250;
+		new_range = 250;
 		bits |= RANGE_250DPS;
+		new_range_scale_dps_digit = 8.75e-3f;
 
 	} else if (max_dps <= 500) {
-		_current_range = 500;
+		new_range = 500;
 		bits |= RANGE_500DPS;
+		new_range_scale_dps_digit = 17.5e-3f;
 
 	} else if (max_dps <= 2000) {
-		_current_range = 2000;
+		new_range = 2000;
 		bits |= RANGE_2000DPS;
+		new_range_scale_dps_digit = 70e-3f;
 
 	} else {
 		return -EINVAL;
 	}
 
-	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
-	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
+	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
+	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
 	write_reg(ADDR_CTRL_REG4, bits);
 
 	return OK;
@@ -598,19 +646,20 @@ L3GD20::set_samplerate(unsigned frequency)
 	if (frequency == 0)
 		frequency = 760;
 
-	if (frequency <= 95) {
+	/* use limits good for H or non-H models */
+	if (frequency <= 100) {
 		_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
 
-	} else if (frequency <= 190) {
+	} else if (frequency <= 200) {
 		_current_rate = 190;
 		bits |= RATE_190HZ_LP_25HZ;
 
-	} else if (frequency <= 380) {
+	} else if (frequency <= 400) {
 		_current_rate = 380;
-		bits |= RATE_380HZ_LP_30HZ;
+		bits |= RATE_380HZ_LP_20HZ;
 
-	} else if (frequency <= 760) {
+	} else if (frequency <= 800) {
 		_current_rate = 760;
 		bits |= RATE_760HZ_LP_30HZ;
 
@@ -624,13 +673,21 @@ L3GD20::set_samplerate(unsigned frequency)
 }
 
 void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
+void
 L3GD20::start()
 {
 	/* make sure we are stopped first */
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
@@ -643,6 +700,30 @@ L3GD20::stop()
 }
 
 void
+L3GD20::reset()
+{
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_reg(ADDR_CTRL_REG5, 0);
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+	/* disable FIFO. This makes things simpler and ensures we
+	 * aren't getting stale data. It means we must run the hrt
+	 * callback fast enough to not miss data. */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+	set_samplerate(L3GD20_DEFAULT_RATE);
+	set_range(L3GD20_DEFAULT_RANGE_DPS);
+	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+	_read = 0;
+}
+
+void
 L3GD20::measure_trampoline(void *arg)
 {
 	L3GD20 *dev = (L3GD20 *)arg;
@@ -666,7 +747,7 @@ L3GD20::measure()
 	} raw_report;
 #pragma pack(pop)
 
-	gyro_report		*report = &_reports[_next_report];
+	gyro_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -689,31 +770,59 @@ L3GD20::measure()
 	 *	 	  the offset is 74 from the origin and subtracting
 	 *		  74 from all measurements centers them around zero.
 	 */
-	report->timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
+        report.error_count = 0; // not recorded
 	
-	/* swap x and y and negate y */
-	report->x_raw = raw_report.y;
-	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
-	report->z_raw = raw_report.z;
+	switch (_orientation) {
+
+		case SENSOR_BOARD_ROTATION_000_DEG:
+			/* keep axes in place */
+			report.x_raw = raw_report.x;
+			report.y_raw = raw_report.y;
+			break;
+
+		case SENSOR_BOARD_ROTATION_090_DEG:
+			/* swap x and y */
+			report.x_raw = raw_report.y;
+			report.y_raw = raw_report.x;
+			break;
+
+		case SENSOR_BOARD_ROTATION_180_DEG:
+			/* swap x and y and negate both */
+			report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+			break;
+
+		case SENSOR_BOARD_ROTATION_270_DEG:
+			/* swap x and y and negate y */
+			report.x_raw = raw_report.y;
+			report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			break;
+	}
+
+	report.z_raw = raw_report.z;
+
+	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 
-	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	report->scaling = _gyro_range_scale;
-	report->range_rad_s = _gyro_range_rad_s;
+	report.x = _gyro_filter_x.apply(report.x);
+	report.y = _gyro_filter_y.apply(report.y);
+	report.z = _gyro_filter_z.apply(report.z);
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	report.scaling = _gyro_range_scale;
+	report.range_rad_s = _gyro_range_rad_s;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+	if (_gyro_topic > 0)
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+
+	_read++;
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -722,9 +831,31 @@ L3GD20::measure()
 void
 L3GD20::print_info()
 {
+	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
+}
+
+int
+L3GD20::self_test()
+{
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
 }
 
 /**
@@ -749,7 +880,7 @@ start()
 	int fd;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		errx(0, "already started");
 
 	/* create the driver */
 	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
@@ -838,7 +969,7 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+		err(1, "accel pollrate reset failed");
 
 	exit(0);
 }
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
index 04b565358..a37eaca53 100644
--- a/src/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -52,6 +52,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 class LED : device::CDev
@@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		led_off(arg);
 		break;
 
+	case LED_TOGGLE:
+		led_toggle(arg);
+		break;
+
+
 	default:
 		result = CDev::ioctl(filp, cmd, arg);
 	}
@@ -110,11 +116,11 @@ LED	*gLED;
 }
 
 void
-drv_led_start()
+drv_led_start(void)
 {
 	if (gLED == nullptr) {
 		gLED = new LED;
 		if (gLED != nullptr)
 			gLED->init();
 	}
-}
\ No newline at end of file
+}
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 000000000..60601e22c
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,1638 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* SPI protocol address bits */
+#define DIR_READ				(1<<7)
+#define DIR_WRITE				(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+
+
+/* register addresses: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM				0x49
+
+#define ADDR_OUT_L_T         	0x05
+#define ADDR_OUT_H_T        	0x06
+#define ADDR_STATUS_M           0x07
+#define ADDR_OUT_X_L_M          0x08
+#define ADDR_OUT_X_H_M          0x09
+#define ADDR_OUT_Y_L_M          0x0A
+#define ADDR_OUT_Y_H_M          0x0B
+#define ADDR_OUT_Z_L_M          0x0C
+#define ADDR_OUT_Z_H_M          0x0D
+
+#define ADDR_OUT_TEMP_A			0x26
+#define ADDR_STATUS_A			0x27
+#define ADDR_OUT_X_L_A			0x28
+#define ADDR_OUT_X_H_A			0x29
+#define ADDR_OUT_Y_L_A			0x2A
+#define ADDR_OUT_Y_H_A			0x2B
+#define ADDR_OUT_Z_L_A			0x2C
+#define ADDR_OUT_Z_H_A			0x2D
+
+#define ADDR_CTRL_REG0			0x1F
+#define ADDR_CTRL_REG1			0x20
+#define ADDR_CTRL_REG2			0x21
+#define ADDR_CTRL_REG3			0x22
+#define ADDR_CTRL_REG4			0x23
+#define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG6			0x25
+#define ADDR_CTRL_REG7			0x26
+
+#define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A		((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A		((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A		((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_BDU_UPDATE			(1<<3)
+#define REG1_Z_ENABLE_A			(1<<2)
+#define REG1_Y_ENABLE_A			(1<<1)
+#define REG1_X_ENABLE_A			(1<<0)
+
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A	((1<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_BITS_A	((1<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A	((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A	((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T			(1<<7)
+
+#define REG5_RES_HIGH_M			((1<<6) | (1<<5))
+#define REG5_RES_LOW_M			((0<<6) | (0<<5))
+
+#define REG5_RATE_BITS_M		((1<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M		((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M		((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_BITS_M	((1<<6) | (1<<5))
+#define REG6_FULL_SCALE_2GA_M	((0<<6) | (0<<5))
+#define REG6_FULL_SCALE_4GA_M	((0<<6) | (1<<5))
+#define REG6_FULL_SCALE_8GA_M	((1<<6) | (0<<5))
+#define REG6_FULL_SCALE_12GA_M	((1<<6) | (1<<5))
+
+#define REG7_CONT_MODE_M		((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M              0x12
+#define INT_SRC_M               0x13
+
+/* default values for this device */
+#define LSM303D_ACCEL_DEFAULT_RANGE_G			8
+#define LSM303D_ACCEL_DEFAULT_RATE			800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define LSM303D_MAG_DEFAULT_RANGE_GA			2
+#define LSM303D_MAG_DEFAULT_RATE			100
+
+#define LSM303D_ONE_G					9.80665f
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+	LSM303D(int bus, const char* path, spi_dev_e device);
+	virtual ~LSM303D();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+	friend class 		LSM303D_mag;
+
+	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+	LSM303D_mag		*_mag;
+
+	struct hrt_call		_accel_call;
+	struct hrt_call		_mag_call;
+
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
+
+	RingBuffer		*_accel_reports;
+	RingBuffer		*_mag_reports;
+
+	struct accel_scale	_accel_scale;
+	unsigned		_accel_range_m_s2;
+	float			_accel_range_scale;
+	unsigned		_accel_samplerate;
+	unsigned		_accel_onchip_filter_bandwith;
+
+	struct mag_scale	_mag_scale;
+	unsigned		_mag_range_ga;
+	float			_mag_range_scale;
+	unsigned		_mag_samplerate;
+
+	orb_advert_t		_accel_topic;
+	orb_advert_t		_mag_topic;
+
+	unsigned		_accel_read;
+	unsigned		_mag_read;
+
+	perf_counter_t		_accel_sample_perf;
+	perf_counter_t		_mag_sample_perf;
+	perf_counter_t		_reg7_resets;
+	perf_counter_t		_reg1_resets;
+
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+
+	// expceted values of reg1 and reg7 to catch in-flight
+	// brownouts of the sensor
+	uint8_t			_reg7_expected;
+	uint8_t			_reg1_expected;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Static trampoline for the mag because it runs at a lower rate
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		mag_measure_trampoline(void *arg);
+
+	/**
+	 * Fetch accel measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Fetch mag measurements from the sensor and update the report ring.
+	 */
+	void			mag_measure();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			accel_self_test();
+
+	/**
+	 * Mag self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			mag_self_test();
+
+	/**
+	 * Read a register from the LSM303D
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the LSM303D
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the LSM303D
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the LSM303D accel measurement range.
+	 *
+	 * @param max_g	The measurement range of the accel is in g (9.81m/s^2)
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D mag measurement range.
+	 *
+	 * @param max_ga	The measurement range of the mag is in Ga
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			mag_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D on-chip anti-alias filter bandwith.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
+
+	/**
+	 * Set the driver lowpass filter bandwidth.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+	/**
+	 * Set the LSM303D internal accel sampling frequency.
+	 *
+	 * @param frequency	The internal accel sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			accel_set_samplerate(unsigned frequency);
+
+	/**
+	 * Set the LSM303D internal mag sampling frequency.
+	 *
+	 * @param frequency	The internal mag sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			mag_set_samplerate(unsigned frequency);
+};
+
+/**
+ * Helper class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+	LSM303D_mag(LSM303D *parent);
+	~LSM303D_mag();
+
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+	friend class LSM303D;
+
+	void				parent_poll_notify();
+private:
+	LSM303D				*_parent;
+
+	void				measure();
+
+	void				measure_trampoline(void *arg);
+};
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+	_mag(new LSM303D_mag(this)),
+	_call_accel_interval(0),
+	_call_mag_interval(0),
+	_accel_reports(nullptr),
+	_mag_reports(nullptr),
+	_accel_range_m_s2(0.0f),
+	_accel_range_scale(0.0f),
+	_accel_samplerate(0),
+	_accel_onchip_filter_bandwith(0),
+	_mag_range_ga(0.0f),
+	_mag_range_scale(0.0f),
+	_mag_samplerate(0),
+	_accel_topic(-1),
+	_mag_topic(-1),
+	_accel_read(0),
+	_mag_read(0),
+	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
+	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
+	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_reg1_expected(0),
+	_reg7_expected(0)
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_accel_scale.x_offset = 0.0f;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0.0f;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0.0f;
+	_accel_scale.z_scale  = 1.0f;
+
+	_mag_scale.x_offset = 0.0f;
+	_mag_scale.x_scale = 1.0f;
+	_mag_scale.y_offset = 0.0f;
+	_mag_scale.y_scale = 1.0f;
+	_mag_scale.z_offset = 0.0f;
+	_mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete _accel_reports;
+	if (_mag_reports != nullptr)
+		delete _mag_reports;
+
+	delete _mag;
+
+	/* delete the perf counter */
+	perf_free(_accel_sample_perf);
+	perf_free(_mag_sample_perf);
+}
+
+int
+LSM303D::init()
+{
+	int ret = ERROR;
+	int mag_ret;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK) {
+		warnx("SPI init failed");
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
+
+	if (_accel_reports == nullptr)
+		goto out;
+
+	/* advertise accel topic */
+	struct accel_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
+
+	_mag_reports = new RingBuffer(2, sizeof(mag_report));
+
+	if (_mag_reports == nullptr)
+		goto out;
+
+	reset();
+
+	/* advertise mag topic */
+	struct mag_report zero_mag_report;
+	memset(&zero_mag_report, 0, sizeof(zero_mag_report));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
+
+	/* do CDev init for the mag device node, keep it optional */
+	mag_ret = _mag->init();
+
+	if (mag_ret != OK) {
+		_mag_topic = -1;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+void
+LSM303D::reset()
+{
+	/* enable accel*/
+	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
+	write_reg(ADDR_CTRL_REG1, _reg1_expected);
+
+	/* enable mag */
+	_reg7_expected = REG7_CONT_MODE_M;
+	write_reg(ADDR_CTRL_REG7, _reg7_expected);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+
+	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+
+	_accel_read = 0;
+	_mag_read = 0;
+}
+
+int
+LSM303D::probe()
+{
+	/* read dummy value to void to clear SPI statemachine on sensor */
+	(void)read_reg(ADDR_WHO_AM_I);
+
+	/* verify that the device is attached and functioning */
+	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
+	
+	if (success)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+LSM303D::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	accel_report *arb = reinterpret_cast<accel_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_accel_interval > 0) {
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 */
+		while (count--) {
+			if (_accel_reports->get(arb)) {
+				ret += sizeof(*arb);
+				arb++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	if (_accel_reports->get(arb))
+		ret = sizeof(*arb);
+
+	return ret;
+}
+
+ssize_t
+LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct mag_report);
+	mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_mag_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 */
+		while (count--) {
+			if (_mag_reports->get(mrb)) {
+				ret += sizeof(*mrb);
+				mrb++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_mag_reports->flush();
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	if (_mag_reports->get(mrb))
+		ret = sizeof(*mrb);
+
+	return ret;
+}
+
+int
+LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_accel_interval = 0;
+				return OK;
+
+			/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
+
+			case SENSOR_POLLRATE_DEFAULT:
+				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_call_accel_interval == 0);
+
+				/* convert hz to hrt interval via microseconds */
+				unsigned ticks = 1000000 / arg;
+
+				/* check against maximum sane rate */
+				if (ticks < 500)
+					return -EINVAL;
+
+				/* adjust filters */
+				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
+
+				/* update interval for next measurement */
+				/* XXX this is a bit shady, but no other way to adjust... */
+				_accel_call.period = _call_accel_interval = ticks;
+
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
+
+				return OK;
+			}
+		}
+	}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_accel_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_accel_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_accel_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _accel_reports->size();
+
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
+	case ACCELIOCSSAMPLERATE:
+		return accel_set_samplerate(arg);
+
+	case ACCELIOCGSAMPLERATE:
+		return _accel_samplerate;
+
+	case ACCELIOCSLOWPASS: {
+		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
+	}
+
+	case ACCELIOCGLOWPASS:
+		return _accel_filter_x.get_cutoff_freq();
+
+	case ACCELIOCSSCALE: {
+		/* copy scale, but only if off by a few percent */
+		struct accel_scale *s = (struct accel_scale *) arg;
+		float sum = s->x_scale + s->y_scale + s->z_scale;
+		if (sum > 2.0f && sum < 4.0f) {
+			memcpy(&_accel_scale, s, sizeof(_accel_scale));
+			return OK;
+		} else {
+			return -EINVAL;
+		}
+	}
+
+	case ACCELIOCSRANGE:
+		/* arg needs to be in G */
+		return accel_set_range(arg);
+
+	case ACCELIOCGRANGE:
+		/* convert to m/s^2 and return rounded in G */
+		return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCSELFTEST:
+		return accel_self_test();
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_mag_interval = 0;
+				return OK;
+
+			/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* 100 Hz is max for mag */
+				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_mag_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_mag_call.period = _call_mag_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_mag_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_mag_interval;
+	
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_mag_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _mag_reports->size();
+
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
+	case MAGIOCSSAMPLERATE:
+		return mag_set_samplerate(arg);
+
+	case MAGIOCGSAMPLERATE:
+		return _mag_samplerate;
+
+	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
+		/* not supported, no internal filtering */
+		return -EINVAL;
+
+	case MAGIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCSRANGE:
+		return mag_set_range(arg);
+
+	case MAGIOCGRANGE:
+		return _mag_range_ga;
+
+	case MAGIOCSELFTEST:
+		return mag_self_test();
+
+	case MAGIOCGEXTERNAL:
+		/* no external mag board yet */
+		return 0;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::accel_self_test()
+{
+	if (_accel_read == 0)
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+	if (_mag_read == 0)
+		return 1;
+
+	/**
+	 * inspect mag offsets
+	 * don't check mag scale because it seems this is calibrated on chip
+	 */
+	if (fabsf(_mag_scale.x_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.y_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.z_offset) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+LSM303D::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+LSM303D::accel_set_range(unsigned max_g)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+	float new_scale_g_digit = 0.0f;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g <= 2) {
+		_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_2G_A;
+		new_scale_g_digit = 0.061e-3f;
+
+	} else if (max_g <= 4) {
+		_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_4G_A;
+		new_scale_g_digit = 0.122e-3f;
+
+	} else if (max_g <= 6) {
+		_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_6G_A;
+		new_scale_g_digit = 0.183e-3f;
+
+	} else if (max_g <= 8) {
+		_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_8G_A;
+		new_scale_g_digit = 0.244e-3f;
+
+	} else if (max_g <= 16) {
+		_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_16G_A;
+		new_scale_g_digit = 0.732e-3f;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
+
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+	float new_scale_ga_digit = 0.0f;
+
+	if (max_ga == 0)
+		max_ga = 12;
+
+	if (max_ga <= 2) {
+		_mag_range_ga = 2;
+		setbits |= REG6_FULL_SCALE_2GA_M;
+		new_scale_ga_digit = 0.080e-3f;
+
+	} else if (max_ga <= 4) {
+		_mag_range_ga = 4;
+		setbits |= REG6_FULL_SCALE_4GA_M;
+		new_scale_ga_digit = 0.160e-3f;
+
+	} else if (max_ga <= 8) {
+		_mag_range_ga = 8;
+		setbits |= REG6_FULL_SCALE_8GA_M;
+		new_scale_ga_digit = 0.320e-3f;
+
+	} else if (max_ga <= 12) {
+		_mag_range_ga = 12;
+		setbits |= REG6_FULL_SCALE_12GA_M;
+		new_scale_ga_digit = 0.479e-3f;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_mag_range_scale = new_scale_ga_digit;
+
+	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+	if (bandwidth == 0)
+		bandwidth = 773;
+
+	if (bandwidth <= 50) {
+		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+		_accel_onchip_filter_bandwith = 50;
+
+	} else if (bandwidth <= 194) {
+		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+		_accel_onchip_filter_bandwith = 194;
+
+	} else if (bandwidth <= 362) {
+		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+		_accel_onchip_filter_bandwith = 362;
+
+	} else if (bandwidth <= 773) {
+		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+		_accel_onchip_filter_bandwith = 773;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG1_RATE_BITS_A;
+
+	if (frequency == 0)
+		frequency = 1600;
+
+	if (frequency <= 100) {
+		setbits |= REG1_RATE_100HZ_A;
+		_accel_samplerate = 100;
+
+	} else if (frequency <= 200) {
+		setbits |= REG1_RATE_200HZ_A;
+		_accel_samplerate = 200;
+
+	} else if (frequency <= 400) {
+		setbits |= REG1_RATE_400HZ_A;
+		_accel_samplerate = 400;
+
+	} else if (frequency <= 800) {
+		setbits |= REG1_RATE_800HZ_A;
+		_accel_samplerate = 800;
+
+	} else if (frequency <= 1600) {
+		setbits |= REG1_RATE_1600HZ_A;
+		_accel_samplerate = 1600;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+	_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG5_RATE_BITS_M;
+
+	if (frequency == 0)
+		frequency = 100;
+
+	if (frequency <= 25) {
+		setbits |= REG5_RATE_25HZ_M;
+		_mag_samplerate = 25;
+
+	} else if (frequency <= 50) {
+		setbits |= REG5_RATE_50HZ_M;
+		_mag_samplerate = 50;
+
+	} else if (frequency <= 100) {
+		setbits |= REG5_RATE_100HZ_M;
+		_mag_samplerate = 100;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
+
+	return OK;
+}
+
+void
+LSM303D::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_accel_reports->flush();
+	_mag_reports->flush();
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+	hrt_cancel(&_accel_call);
+	hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
+		perf_count(_reg1_resets);
+		reset();
+		return;
+	}
+
+	/* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_accel_report;
+#pragma pack(pop)
+
+	accel_report accel_report;
+
+	/* start the performance counter */
+	perf_begin(_accel_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	accel_report.timestamp = hrt_absolute_time();
+        accel_report.error_count = 0; // not reported
+
+	accel_report.x_raw = raw_accel_report.x;
+	accel_report.y_raw = raw_accel_report.y;
+	accel_report.z_raw = raw_accel_report.z;
+
+	float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+
+	accel_report.x = _accel_filter_x.apply(x_in_new);
+	accel_report.y = _accel_filter_y.apply(y_in_new);
+	accel_report.z = _accel_filter_z.apply(z_in_new);
+
+	accel_report.scaling = _accel_range_scale;
+	accel_report.range_m_s2 = _accel_range_m_s2;
+
+	_accel_reports->force(&accel_report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+
+	_accel_read++;
+
+	/* stop the perf counter */
+	perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+	if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
+		perf_count(_reg7_resets);
+		reset();
+		return;
+	}
+
+	/* status register and data as read back from the device */
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_mag_report;
+#pragma pack(pop)
+
+	mag_report mag_report;
+
+	/* start the performance counter */
+	perf_begin(_mag_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	mag_report.timestamp = hrt_absolute_time();
+
+	mag_report.x_raw = raw_mag_report.x;
+	mag_report.y_raw = raw_mag_report.y;
+	mag_report.z_raw = raw_mag_report.z;
+	mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+	mag_report.scaling = _mag_range_scale;
+	mag_report.range_ga = (float)_mag_range_ga;
+
+	_mag_reports->force(&mag_report);
+
+	/* XXX please check this poll_notify, is it the right one? */
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+
+	_mag_read++;
+
+	/* stop the perf counter */
+	perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+	printf("accel reads:          %u\n", _accel_read);
+	printf("mag reads:            %u\n", _mag_read);
+	perf_print_counter(_accel_sample_perf);
+	_accel_reports->print_info("accel reports");
+	_mag_reports->print_info("mag reports");
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+	CDev("LSM303D_mag", MAG_DEVICE_PATH),
+	_parent(parent)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+	_parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+	_parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd, fd_mag;
+
+	if (g_dev != nullptr)
+		errx(0, "already started");
+
+	/* create the driver */
+	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+
+	if (g_dev == nullptr) {
+		warnx("failed instantiating LSM303D obj");
+		goto fail;
+	}
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	/* don't fail if open cannot be opened */
+	if (0 <= fd_mag) {
+		if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+			goto fail;
+		}
+	}
+
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd_accel = -1;
+	struct accel_report accel_report;
+	ssize_t sz;
+	int ret;
+
+	/* get the driver */
+	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd_accel < 0)
+		err(1, "%s open failed", ACCEL_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd_accel, &accel_report, sizeof(accel_report));
+
+	if (sz != sizeof(accel_report))
+		err(1, "immediate read failed");
+
+
+	warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
+	warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
+	warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
+	warnx("accel x: \t%d\traw", (int)accel_report.x_raw);
+	warnx("accel y: \t%d\traw", (int)accel_report.y_raw);
+	warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
+
+	warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
+	if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+		warnx("accel antialias filter bandwidth: fail");
+	else
+		warnx("accel antialias filter bandwidth: %d Hz", ret);
+
+	int fd_mag = -1;
+	struct mag_report m_report;
+
+	/* get the driver */
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd_mag < 0)
+		err(1, "%s open failed", MAG_DEVICE_PATH);
+
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("mag device active: %s", ret ? "external" : "onboard");
+
+	/* do a simple demand read */
+	sz = read(fd_mag, &m_report, sizeof(m_report));
+
+	if (sz != sizeof(m_report))
+		err(1, "immediate read failed");
+
+	warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+	warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+	warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
+	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
+
+	/* XXX add poll-rate tests here too */
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "accel pollrate reset failed");
+
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		warnx("mag could not be opened, external mag might be used");
+	} else {
+		/* no need to reset the mag as well, the reset() is the same */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			err(1, "mag pollrate reset failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		lsm303d::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		lsm303d::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		lsm303d::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		lsm303d::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
new file mode 100644
index 000000000..e40f718c5
--- /dev/null
+++ b/src/drivers/lsm303d/module.mk
@@ -0,0 +1,8 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND	 = lsm303d
+SRCS		 = lsm303d.cpp
+
+
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 397686e8b..c910e2890 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -59,17 +59,18 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
-
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
 
+#include <board_config.h>
+
 /* Configuration Constants */
 #define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
@@ -119,10 +120,7 @@ private:
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
-	unsigned			_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	range_finder_report	*_reports;
+	RingBuffer		*_reports;
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
@@ -183,9 +181,6 @@ private:
 	
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
 /*
  * Driver 'main' command.
  */
@@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) :
 	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
 	_min_distance(MB12XX_MIN_DISTANCE),
 	_max_distance(MB12XX_MAX_DISTANCE),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -221,7 +213,7 @@ MB12XX::~MB12XX()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 }
 
 int
@@ -234,17 +226,15 @@ MB12XX::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct range_finder_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the range finder topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
+	struct range_finder_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
 
 	if (_range_finder_topic < 0)
 		debug("failed to create sensor_range_finder object. Did you start uOrb?");
@@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct range_finder_report *buf = new struct range_finder_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 		
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -406,6 +387,7 @@ ssize_t
 MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
 			}
 		}
 
@@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
 
 	} while (0);
 
@@ -498,26 +479,26 @@ MB12XX::collect()
 	if (ret < 0)
 	{
 		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return ret;
 	}
 	
 	uint16_t distance = val[0] << 8 | val[1];
 	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	struct range_finder_report report;
+
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].distance = si_units;
-	_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
 	
 	/* publish it */
-	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
+	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -525,11 +506,8 @@ MB12XX::collect()
 
 	ret = OK;
 
-out:
 	perf_end(_sample_perf);
 	return ret;
-	
-	return ret;
 }
 
 void
@@ -537,7 +515,7 @@ MB12XX::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
@@ -626,8 +604,7 @@ MB12XX::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 68d2c5d65..b3003fc1b 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -70,7 +70,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
@@ -138,12 +138,8 @@ MEASAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
-		return ret;
 	}
 
-	ret = OK;
-
 	return ret;
 }
 
@@ -161,7 +157,8 @@ MEASAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
 		return ret;
 	}
 
@@ -169,9 +166,14 @@ MEASAirspeed::collect()
 
 	if (status == 2) {
 		log("err: stale data");
-
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
+		return ret;
 	} else if (status == 3) {
 		log("err: fault");
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
+		return ret;                
 	}
 
 	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
@@ -192,27 +194,24 @@ MEASAirspeed::collect()
 	// Calculate differential pressure. As its centered around 8000
 	// and can go positive or negative, enforce absolute value
 	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
-
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].temperature = temperature;
-	_reports[_next_report].differential_pressure_pa = diff_press_pa;
+	struct differential_pressure_s report;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
+	if (diff_press_pa > _max_differential_pressure_pa) {
+	    _max_differential_pressure_pa = diff_press_pa;
 	}
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
+	report.temperature = temperature;
+	report.differential_pressure_pa = diff_press_pa;
+	report.voltage = 0;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	/* announce the airspeed if needed, just publish else */
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
-	}
+	new_report(report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
@@ -232,7 +231,6 @@ MEASAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index e78d96569..b93f38cf6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -59,10 +59,11 @@
 #include <nuttx/arch.h>
 #include <nuttx/i2c.h>
 
+#include <board_config.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 
@@ -74,6 +75,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
@@ -94,9 +96,10 @@ class MK : public device::I2C
 {
 public:
 	enum Mode {
+		MODE_NONE,
 		MODE_2PWM,
 		MODE_4PWM,
-		MODE_NONE
+		MODE_6PWM,
 	};
 
 	enum MappingMode {
@@ -134,7 +137,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
@@ -247,7 +250,7 @@ MK::MK(int bus) :
 	_update_rate(50),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_t_esc_status(0),
@@ -512,8 +515,8 @@ MK::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -539,7 +542,7 @@ MK::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 	log("starting");
@@ -653,7 +656,7 @@ MK::task_main()
 			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
 			mk_servo_arm(aa.armed && !aa.lockdown);
@@ -699,7 +702,7 @@ MK::task_main()
 	//::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 
 	/* make sure servos are off */
@@ -1021,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 		return ret;
 
 	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	/*
 	switch (_mode) {
 	case MODE_2PWM:
 	case MODE_4PWM:
+	case MODE_6PWM:
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -1031,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 		debug("not in a PWM mode");
 		break;
 	}
+	*/
+	ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
 	if (ret == -ENOTTY)
@@ -1064,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		ret = OK;
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = OK;
 		break;
 
@@ -1345,44 +1352,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	gpio_bits = 0;
 	servo_mode = MK::MODE_NONE;
 
-	switch (new_mode) {
-	case PORT_FULL_GPIO:
-	case PORT_MODE_UNSET:
-		/* nothing more to do here */
-		break;
-
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_FULL_PWM:
-		/* select 4-pin PWM mode */
-		servo_mode = MK::MODE_4PWM;
-		break;
-
-	case PORT_GPIO_AND_SERIAL:
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_SERIAL:
-		/* select 2-pin PWM mode */
-		servo_mode = MK::MODE_2PWM;
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_GPIO:
-		/* select 2-pin PWM mode */
-		servo_mode = MK::MODE_2PWM;
-		break;
-	}
-
-	/* adjust GPIO config for serial mode(s) */
-	if (gpio_bits != 0)
-		g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
-
 	/* native PX4 addressing) */
 	g_mk->set_px4mode(px4mode);
 
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index c4e331a30..70359110e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -63,7 +63,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
@@ -149,6 +149,17 @@
 #define MPU6000_REV_D9			0x59
 #define MPU6000_REV_D10			0x5A
 
+#define MPU6000_ACCEL_DEFAULT_RANGE_G			8
+#define MPU6000_ACCEL_DEFAULT_RATE			1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G			8
+#define MPU6000_GYRO_DEFAULT_RATE			1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
+
+#define MPU6000_ONE_G					9.80665f
 
 class MPU6000_gyro;
 
@@ -183,16 +194,14 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	typedef RingBuffer<accel_report> AccelReportBuffer;
-	AccelReportBuffer	*_accel_reports;
+	RingBuffer		*_accel_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	typedef RingBuffer<gyro_report> GyroReportBuffer;
-	GyroReportBuffer	*_gyro_reports;
+	RingBuffer		*_gyro_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
@@ -285,12 +294,26 @@ private:
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
 	/**
-	 * Self test
+	 * Measurement self test
 	 *
 	 * @return 0 on success, 1 on failure
 	 */
 	 int 			self_test();
 
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			accel_self_test();
+
+	/**
+	 * Gyro self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			gyro_self_test();
+
 	/*
 	  set low pass filter frequency
 	 */
@@ -321,6 +344,7 @@ protected:
 	void			parent_poll_notify();
 private:
 	MPU6000			*_parent;
+
 };
 
 /** driver 'main' command */
@@ -342,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_reads(0),
 	_sample_rate(1000),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
-	_accel_filter_x(1000, 30),
-	_accel_filter_y(1000, 30),
-	_accel_filter_z(1000, 30),
-	_gyro_filter_x(1000, 30),
-        _gyro_filter_y(1000, 30),
-        _gyro_filter_z(1000, 30)
+	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -405,11 +429,11 @@ MPU6000::init()
 	}
 
 	/* allocate basic report buffers */
-	_accel_reports = new AccelReportBuffer(2);
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
 	if (_accel_reports == nullptr)
 		goto out;
 
-	_gyro_reports = new GyroReportBuffer(2);
+	_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
 	if (_gyro_reports == nullptr)
 		goto out;
 
@@ -440,14 +464,14 @@ MPU6000::init()
 		_gyro_topic = -1;
 	} else {
 		gyro_report gr;
-		_gyro_reports->get(gr);
+		_gyro_reports->get(&gr);
 
 		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
 	}
 
 	/* advertise accel topic */
 	accel_report ar;
-	_accel_reports->get(ar);
+	_accel_reports->get(&ar);
 	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
 
 out:
@@ -470,14 +494,13 @@ void MPU6000::reset()
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
-	_set_sample_rate(_sample_rate); // default sample rate = 200Hz
+	_set_sample_rate(_sample_rate);
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	_set_dlpf_filter(42);
+	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -520,8 +543,8 @@ void MPU6000::reset()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_range_scale = (9.81f / 4096.0f);
-	_accel_range_m_s2 = 8.0f * 9.81f;
+	_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+	_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
 
 	usleep(1000);
 
@@ -633,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_accel_reports->get(*arp++))
+		if (!_accel_reports->get(arp))
 			break;
 		transferred++;
+		arp++;
 	}
 
 	/* return the number of bytes transferred */
@@ -653,6 +677,56 @@ MPU6000::self_test()
 	return (_reads > 0) ? 0 : 1;
 }
 
+int
+MPU6000::accel_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+MPU6000::gyro_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -673,12 +747,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 		return -EAGAIN;
 
 	/* copy reports out of our buffer to the caller */
-	gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
+	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_gyro_reports->get(*arp++))
+		if (!_gyro_reports->get(grp))
 			break;
 		transferred++;
+		grp++;
 	}
 
 	/* return the number of bytes transferred */
@@ -712,9 +787,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
 			case SENSOR_POLLRATE_DEFAULT:
-				/* set to same as sample rate per default */
-				return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
+				return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -761,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			AccelReportBuffer *buf = new AccelReportBuffer(arg);
-
-			if (nullptr == buf)
-				return -ENOMEM;
-			if (buf->size() == 0) {
-				delete buf;
-				return -ENOMEM;
-			}
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete _accel_reports;
-			_accel_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_accel_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _accel_reports->size();
@@ -802,9 +869,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// XXX decide on relationship of both filters
 		// i.e. disable the on-chip filter
 		//_set_dlpf_filter((uint16_t)arg);
-                _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-                _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-                _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		return OK;
 
 	case ACCELIOCSSCALE:
@@ -832,10 +899,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
 	case ACCELIOCGRANGE:
-		return _accel_range_m_s2;
+		return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
 
 	case ACCELIOCSELFTEST:
-		return self_test();
+		return accel_self_test();
 
 	default:
 		/* give it to the superclass */
@@ -855,28 +922,19 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return ioctl(filp, cmd, arg);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			GyroReportBuffer *buf = new GyroReportBuffer(arg);
-
-			if (nullptr == buf)
-				return -ENOMEM;
-			if (buf->size() == 0) {
-				delete buf;
-				return -ENOMEM;
-			}
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete _gyro_reports;
-			_gyro_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_gyro_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _gyro_reports->size();
@@ -915,10 +973,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _gyro_range_rad_s = xx
 		return -EINVAL;
 	case GYROIOCGRANGE:
-		return _gyro_range_rad_s;
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
-		return self_test();
+		return gyro_self_test();
 
 	default:
 		/* give it to the superclass */
@@ -1128,7 +1186,7 @@ MPU6000::measure()
 	 * Adjust and scale results to m/s^2.
 	 */
 	grb.timestamp = arb.timestamp = hrt_absolute_time();
-
+        grb.error_count = arb.error_count = 0; // not reported
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1184,8 +1242,8 @@ MPU6000::measure()
 	grb.temperature_raw = report.temp;
 	grb.temperature = (report.temp) / 361.0f + 35.0f;
 
-	_accel_reports->put(arb);
-	_gyro_reports->put(grb);
+	_accel_reports->force(&arb);
+	_gyro_reports->force(&grb);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
@@ -1204,7 +1262,10 @@ MPU6000::measure()
 void
 MPU6000::print_info()
 {
+	perf_print_counter(_sample_perf);
 	printf("reads:          %u\n", _reads);
+	_accel_reports->print_info("accel queue");
+	_gyro_reports->print_info("gyro queue");
 }
 
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
@@ -1335,7 +1396,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 / MPU6000_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
index 3c4b0f093..20f8aa173 100644
--- a/src/drivers/ms5611/module.mk
+++ b/src/drivers/ms5611/module.mk
@@ -37,4 +37,4 @@
 
 MODULE_COMMAND	= ms5611
 
-SRCS		= ms5611.cpp
+SRCS		= ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index c13b65d5a..938786d3f 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,13 +33,11 @@
 
 /**
  * @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C.
+ * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI.
  */
 
 #include <nuttx/config.h>
 
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
@@ -59,12 +57,15 @@
 
 #include <arch/board/board.h>
 
+#include <drivers/device/device.h>
+#include <drivers/drv_baro.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
-#include <drivers/drv_baro.h>
+#include "ms5611.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -76,35 +77,25 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-/**
- * Calibration PROM as reported by the device.
- */
-#pragma pack(push,1)
-struct ms5611_prom_s {
-	uint16_t factory_setup;
-	uint16_t c1_pressure_sens;
-	uint16_t c2_pressure_offset;
-	uint16_t c3_temp_coeff_pres_sens;
-	uint16_t c4_temp_coeff_pres_offset;
-	uint16_t c5_reference_temp;
-	uint16_t c6_temp_coeff_temp;
-	uint16_t serial_and_crc;
-};
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
 
-/**
- * Grody hack for crc4()
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+/*
+ * MS5611 internal constants and data structures.
  */
-union ms5611_prom_u {
-	uint16_t c[8];
-	struct ms5611_prom_s s;
-};
-#pragma pack(pop)
 
-class MS5611 : public device::I2C
+/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
+#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+
+class MS5611 : public device::CDev
 {
 public:
-	MS5611(int bus);
-	virtual ~MS5611();
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf);
+	~MS5611();
 
 	virtual int		init();
 
@@ -117,18 +108,14 @@ public:
 	void			print_info();
 
 protected:
-	virtual int		probe();
+	Device			*_interface;
 
-private:
-	union ms5611_prom_u	_prom;
+	ms5611::prom_s		_prom;
 
 	struct work_s		_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct baro_report	*_reports;
+	RingBuffer		*_reports;
 
 	bool			_collect_phase;
 	unsigned		_measure_phase;
@@ -149,16 +136,7 @@ private:
 	perf_counter_t		_buffer_overflows;
 
 	/**
-	 * Test whether the device supported by the driver is present at a
-	 * specific address.
-	 *
-	 * @param address	The I2C bus address to probe.
-	 * @return		True if the device is present.
-	 */
-	int			probe_address(uint8_t address);
-
-	/**
-	 * Initialise the automatic measurement state machine and start it.
+	 * Initialize the automatic measurement state machine and start it.
 	 *
 	 * @note This function is called at open and error time.  It might make sense
 	 *       to make it more aggressive about resetting the bus in case of errors.
@@ -198,74 +176,24 @@ private:
 	 *
 	 * @return		OK if the measurement command was successful.
 	 */
-	int			measure();
+	virtual int		measure();
 
 	/**
 	 * Collect the result of the most recent measurement.
 	 */
-	int			collect();
-
-	/**
-	 * Send a reset command to the MS5611.
-	 *
-	 * This is required after any bus reset.
-	 */
-	int			cmd_reset();
-
-	/**
-	 * Read the MS5611 PROM
-	 *
-	 * @return		OK if the PROM reads successfully.
-	 */
-	int			read_prom();
-
-	/**
-	 * PROM CRC routine ported from MS5611 application note
-	 *
-	 * @param n_prom	Pointer to words read from PROM.
-	 * @return		True if the CRC matches.
-	 */
-	bool			crc4(uint16_t *n_prom);
-
+	virtual int		collect();
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
-
-/*
- * MS5611 internal constants and data structures.
- */
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-
-#define MS5611_BUS			PX4_I2C_BUS_ONBOARD
-#define MS5611_ADDRESS_1		PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
-
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
-#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
-
 /*
  * Driver 'main' command.
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-
-MS5611::MS5611(int bus) :
-	I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
+	CDev("MS5611", BARO_DEVICE_PATH),
+	_interface(interface),
+	_prom(prom_buf.s),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_collect_phase(false),
 	_measure_phase(0),
@@ -279,10 +207,7 @@ MS5611::MS5611(int bus) :
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
 {
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// work_cancel in the dtor will explode if we don't do this...
+	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
 
@@ -293,78 +218,58 @@ MS5611::~MS5611()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+
+	delete _interface;
 }
 
 int
 MS5611::init()
 {
-	int ret = ERROR;
+	int ret;
 
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	ret = CDev::init();
+	if (ret != OK) {
+		debug("CDev init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct baro_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(baro_report));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
+		debug("can't get memory for reports");
+		ret = -ENOMEM;
 		goto out;
-
-	_oldest_report = _next_report = 0;
+	}
 
 	/* get a publish handle on the baro topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+	struct baro_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
 
-	if (_baro_topic < 0)
+	if (_baro_topic < 0) {
 		debug("failed to create sensor_baro object");
+		ret = -ENOSPC;
+		goto out;
+	}
 
 	ret = OK;
 out:
 	return ret;
 }
 
-int
-MS5611::probe()
-{
-	_retries = 10;
-
-	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
-	    (OK == probe_address(MS5611_ADDRESS_2))) {
-		/*
-	    	 * Disable retries; we may enable them selectively in some cases,
-		 * but the device gets confused if we retry some of the commands.
-	    	 */
-		_retries = 0;
-		return OK;
-	}
-
-	return -EIO;
-}
-
-int
-MS5611::probe_address(uint8_t address)
-{
-	/* select the address we are going to try */
-	set_address(address);
-
-	/* send reset command */
-	if (OK != cmd_reset())
-		return -EIO;
-
-	/* read PROM */
-	if (OK != read_prom())
-		return -EIO;
-
-	return OK;
-}
-
 ssize_t
 MS5611::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct baro_report);
+	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -380,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(brp)) {
+				ret += sizeof(*brp);
+				brp++;
 			}
 		}
 
@@ -392,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
 		_measure_phase = 0;
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* do temperature first */
 		if (OK != measure()) {
@@ -424,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(brp))
+			ret = sizeof(*brp);
 
 	} while (0);
 
@@ -500,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct baro_report *buf = new struct baro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop_cycle();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start_cycle();
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
 
-			return OK;
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -546,8 +439,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	/* give it to the superclass */
-	return I2C::ioctl(filp, cmd, arg);
+	/* give it to the bus-specific superclass */
+	// return bus_ioctl(filp, cmd, arg);
+	return CDev::ioctl(filp, cmd, arg);
 }
 
 void
@@ -557,7 +451,7 @@ MS5611::start_cycle()
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_measure_phase = 0;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
@@ -572,7 +466,7 @@ MS5611::stop_cycle()
 void
 MS5611::cycle_trampoline(void *arg)
 {
-	MS5611 *dev = (MS5611 *)arg;
+	MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
 
 	dev->cycle();
 }
@@ -592,7 +486,7 @@ MS5611::cycle()
 				/*
 				 * The ms5611 seems to regularly fail to respond to
 				 * its address; this happens often enough that we'd rather not
-				 * spam the console with the message.
+				 * spam the console with a message for this.
 				 */
 			} else {
 				//log("collection error %d", ret);
@@ -654,17 +548,12 @@ MS5611::measure()
 	/*
 	 * In phase zero, request temperature; in other phases, request pressure.
 	 */
-	uint8_t	cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
 
 	/*
 	 * Send the command to begin measuring.
-	 *
-	 * Disable retries on this command; we can't know whether failure 
-	 * means the device did or did not see the write.
 	 */
-	_retries = 0;
-	ret = transfer(&cmd_data, 1, nullptr, 0);
-
+	ret = _interface->ioctl(IOCTL_MEASURE, addr);
 	if (OK != ret)
 		perf_count(_comms_errors);
 
@@ -677,47 +566,35 @@ int
 MS5611::collect()
 {
 	int ret;
-	uint8_t cmd;
-	uint8_t data[3];
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} cvt;
-
-	/* read the most recent measurement */
-	cmd = 0;
+	uint32_t raw;
 
 	perf_begin(_sample_perf);
 
+	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 
-	ret = transfer(&cmd, 1, &data[0], 3);
-	if (ret != OK) {
+	/* read the most recent measurement - read offset/size are hardcoded in the interface */
+	ret = _interface->read(0, (void *)&raw, 0);
+	if (ret < 0) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
 
-	/* fetch the raw value */
-	cvt.b[0] = data[2];
-	cvt.b[1] = data[1];
-	cvt.b[2] = data[0];
-	cvt.b[3] = 0;
-	uint32_t raw = cvt.w;
-
 	/* handle a measurement */
 	if (_measure_phase == 0) {
 
 		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
 
 		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
 
 		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+		_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
+		_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
 
 		/* temperature compensation */
 		if (_TEMP < 2000) {
@@ -745,8 +622,8 @@ MS5611::collect()
 		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
 
 		/* generate a new report */
-		_reports[_next_report].temperature = _TEMP / 100.0f;
-		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
+		report.temperature = _TEMP / 100.0f;
+		report.pressure = P / 100.0f;		/* convert to millibar */
 
 		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
 
@@ -761,30 +638,7 @@ MS5611::collect()
 		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
 		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
 		 */
-#if 0/* USE_FLOAT */
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
-		const float a  = -6.5f / 1000f;	/* temperature gradient in degrees per metre */
-		const float g  = 9.80665f;	/* gravity constant in m/s/s */
-		const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		float p1 = _msl_pressure / 1000.0f;
-
-		/* measured pressure in kPa */
-		float p = P / 1000.0f;
 
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#else
 		/* tropospheric properties (0-11km) for standard atmosphere */
 		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
 		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
@@ -806,18 +660,13 @@ MS5611::collect()
 		 * h = -------------------------------  + h1
 		 *                   a
 		 */
-		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#endif
-		/* publish it */
-		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
+		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
-		/* post a report to the ring - note, not locked */
-		INCREMENT(_next_report, _num_reports);
+		/* publish it */
+		orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 
-		/* if we are running up against the oldest report, toss it */
-		if (_next_report == _oldest_report) {
+		if (_reports->force(&report)) {
 			perf_count(_buffer_overflows);
-			INCREMENT(_oldest_report, _num_reports);
 		}
 
 		/* notify anyone waiting for data */
@@ -832,55 +681,48 @@ MS5611::collect()
 	return OK;
 }
 
-int
-MS5611::cmd_reset()
+void
+MS5611::print_info()
 {
-	unsigned	old_retrycount = _retries;
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
-
-	/* bump the retry count */
-	_retries = 10;
-	result = transfer(&cmd, 1, nullptr, 0);
-	_retries = old_retrycount;
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+	printf("TEMP:           %d\n", _TEMP);
+	printf("SENS:           %lld\n", _SENS);
+	printf("OFF:            %lld\n", _OFF);
+	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
-	return result;
+	printf("factory_setup             %u\n", _prom.factory_setup);
+	printf("c1_pressure_sens          %u\n", _prom.c1_pressure_sens);
+	printf("c2_pressure_offset        %u\n", _prom.c2_pressure_offset);
+	printf("c3_temp_coeff_pres_sens   %u\n", _prom.c3_temp_coeff_pres_sens);
+	printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
+	printf("c5_reference_temp         %u\n", _prom.c5_reference_temp);
+	printf("c6_temp_coeff_temp        %u\n", _prom.c6_temp_coeff_temp);
+	printf("serial_and_crc            %u\n", _prom.serial_and_crc);
 }
 
-int
-MS5611::read_prom()
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ms5611
 {
-	uint8_t		prom_buf[2];
-	union {
-		uint8_t		b[2];
-		uint16_t	w;
-	} cvt;
-
-	/*
-	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
-	 * called immediately after reset.
-	 */
-	usleep(3000);
-
-	/* read and convert PROM words */
-	for (int i = 0; i < 8; i++) {
-		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
 
-		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
-			break;
-
-		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		cvt.b[0] = prom_buf[1];
-		cvt.b[1] = prom_buf[0];
-		_prom.c[i] = cvt.w;
-	}
+MS5611	*g_dev;
 
-	/* calculate CRC and return success/failure accordingly */
-	return crc4(&_prom.c[0]) ? OK : -EIO;
-}
+void	start();
+void	test();
+void	reset();
+void	info();
+void	calibrate(unsigned altitude);
 
+/**
+ * MS5611 crc4 cribbed from the datasheet
+ */
 bool
-MS5611::crc4(uint16_t *n_prom)
+crc4(uint16_t *n_prom)
 {
 	int16_t cnt;
 	uint16_t n_rem;
@@ -922,43 +764,6 @@ MS5611::crc4(uint16_t *n_prom)
 	return (0x000F & crc_read) == (n_rem ^ 0x00);
 }
 
-void
-MS5611::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-	printf("TEMP:           %d\n", _TEMP);
-	printf("SENS:           %lld\n", _SENS);
-	printf("OFF:            %lld\n", _OFF);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
-
-	printf("factory_setup             %u\n", _prom.s.factory_setup);
-	printf("c1_pressure_sens          %u\n", _prom.s.c1_pressure_sens);
-	printf("c2_pressure_offset        %u\n", _prom.s.c2_pressure_offset);
-	printf("c3_temp_coeff_pres_sens   %u\n", _prom.s.c3_temp_coeff_pres_sens);
-	printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
-	printf("c5_reference_temp         %u\n", _prom.s.c5_reference_temp);
-	printf("c6_temp_coeff_temp        %u\n", _prom.s.c6_temp_coeff_temp);
-	printf("serial_and_crc            %u\n", _prom.s.serial_and_crc);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace ms5611
-{
-
-MS5611	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-void	calibrate(unsigned altitude);
 
 /**
  * Start the driver.
@@ -967,28 +772,46 @@ void
 start()
 {
 	int fd;
+	prom_u prom_buf;
 
 	if (g_dev != nullptr)
 		/* if already started, the still command succeeded */
 		errx(0, "already started");
 
-	/* create the driver */
-	g_dev = new MS5611(MS5611_BUS);
+	device::Device *interface = nullptr;
 
-	if (g_dev == nullptr)
-		goto fail;
+	/* create the driver, try SPI first, fall back to I2C if unsuccessful */
+	if (MS5611_spi_interface != nullptr)
+		interface = MS5611_spi_interface(prom_buf);
+	if (interface == nullptr && (MS5611_i2c_interface != nullptr))
+		interface = MS5611_i2c_interface(prom_buf);
+
+	if (interface == nullptr)
+		errx(1, "failed to allocate an interface");
+
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
 
-	if (OK != g_dev->init())
+	g_dev = new MS5611(interface, prom_buf);
+	if (g_dev == nullptr) {
+		delete interface;
+		errx(1, "failed to allocate driver");
+	}
+	if (g_dev->init() != OK)
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
+	if (fd < 0) {
+		warnx("can't open baro device");
 		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	}
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		warnx("failed setting default poll rate");
 		goto fail;
+	}
 
 	exit(0);
 
@@ -1155,11 +978,11 @@ calibrate(unsigned altitude)
 	const float g  = 9.80665f;	/* gravity constant in m/s/s */
 	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
 
-	warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
+	warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
 
 	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
 
-	warnx("calculated MSL pressure %10.4fkPa", p1);
+	warnx("calculated MSL pressure %10.4fkPa", (double)p1);
 
 	/* save as integer Pa */
 	p1 *= 1000.0f;
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
new file mode 100644
index 000000000..76fb84de8
--- /dev/null
+++ b/src/drivers/ms5611/ms5611.h
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5611.h
+ *
+ * Shared defines for the ms5611 driver.
+ */
+
+#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
+#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+
+/* interface ioctls */
+#define IOCTL_RESET			2
+#define IOCTL_MEASURE			3
+
+namespace ms5611
+{
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct prom_s {
+	uint16_t factory_setup;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t serial_and_crc;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union prom_u {
+	uint16_t c[8];
+	prom_s s;
+};
+#pragma pack(pop)
+
+extern bool crc4(uint16_t *n_prom);
+
+} /* namespace */
+
+/* interface factories */
+extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function;
+extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
+
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
new file mode 100644
index 000000000..87d9b94a6
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file ms5611_i2c.cpp
+  *
+  * I2C interface for MS5611
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+
+#include "ms5611.h"
+
+#include "board_config.h"
+
+#ifdef PX4_I2C_OBDEV_MS5611
+
+#ifndef PX4_I2C_BUS_ONBOARD
+	#define MS5611_BUS		1
+#else
+	#define MS5611_BUS		PX4_I2C_BUS_ONBOARD
+#endif
+
+#define MS5611_ADDRESS_1		0x76	/* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
+
+
+
+device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_I2C : public device::I2C
+{
+public:
+	MS5611_I2C(int bus, ms5611::prom_u &prom_buf);
+	virtual ~MS5611_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+protected:
+	virtual int	probe();
+
+private:
+	ms5611::prom_u	&_prom;
+
+	int		_probe_address(uint8_t address);
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int		_reset();
+
+	/**
+	 * Send a measure command to the MS5611.
+	 *
+	 * @param addr		Which address to use for the measure operation.
+	 */
+	int		_measure(unsigned addr);
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int		_read_prom();
+
+};
+
+device::Device *
+MS5611_i2c_interface(ms5611::prom_u &prom_buf)
+{
+	return new MS5611_I2C(MS5611_BUS, prom_buf);
+}
+
+MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) :
+	I2C("MS5611_I2C", nullptr, bus, 0, 400000),
+	_prom(prom)
+{
+}
+
+MS5611_I2C::~MS5611_I2C()
+{
+}
+
+int
+MS5611_I2C::init()
+{
+	/* this will call probe(), and thereby _probe_address */
+	return I2C::init();
+}
+
+int
+MS5611_I2C::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[3];
+
+	/* read the most recent measurement */
+	uint8_t cmd = 0;
+	int ret = transfer(&cmd, 1, &buf[0], 3);
+	if (ret == OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[2];
+		cvt->b[1] = buf[1];
+		cvt->b[2] = buf[0];
+		cvt->b[3] = 0;
+	}
+
+	return ret;
+}
+
+int
+MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+	case IOCTL_RESET:
+		ret = _reset();
+		break;
+
+	case IOCTL_MEASURE:
+		ret = _measure(arg);
+		break;
+
+	default:
+		ret = EINVAL;
+	}
+
+	return ret;
+}
+
+int
+MS5611_I2C::probe()
+{
+	_retries = 10;
+
+	if ((OK == _probe_address(MS5611_ADDRESS_1)) ||
+	    (OK == _probe_address(MS5611_ADDRESS_2))) {
+		/*
+	    	 * Disable retries; we may enable them selectively in some cases,
+		 * but the device gets confused if we retry some of the commands.
+	    	 */
+		_retries = 0;
+		return OK;
+	}
+
+	return -EIO;
+}
+
+int
+MS5611_I2C::_probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_address(address);
+
+	/* send reset command */
+	if (OK != _reset())
+		return -EIO;
+
+	/* read PROM */
+	if (OK != _read_prom())
+		return -EIO;
+
+	return OK;
+}
+
+
+int
+MS5611_I2C::_reset()
+{
+	unsigned	old_retrycount = _retries;
+	uint8_t		cmd = ADDR_RESET_CMD;
+	int		result;
+
+	/* bump the retry count */
+	_retries = 10;
+	result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int
+MS5611_I2C::_measure(unsigned addr)
+{
+	/*
+	 * Disable retries on this command; we can't know whether failure 
+	 * means the device did or did not see the command.
+	 */
+	_retries = 0;
+
+	uint8_t cmd = addr;
+	return transfer(&cmd, 1, nullptr, 0);
+}
+
+int
+MS5611_I2C::_read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+
+		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
+			break;
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+#endif /* PX4_I2C_OBDEV_MS5611 */
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
new file mode 100644
index 000000000..e547c913b
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file ms5611_spi.cpp
+  *
+  * SPI interface for MS5611
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+
+#include "ms5611.h"
+#include "board_config.h"
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#ifdef PX4_SPIDEV_BARO
+
+device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_SPI : public device::SPI
+{
+public:
+	MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf);
+	virtual ~MS5611_SPI();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+	ms5611::prom_u	&_prom;
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int		_reset();
+
+	/**
+	 * Send a measure command to the MS5611.
+	 *
+	 * @param addr		Which address to use for the measure operation.
+	 */
+	int		_measure(unsigned addr);
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int		_read_prom();
+
+	/**
+	 * Read a 16-bit register value.
+	 *
+	 * @param reg		The register to read.
+	 */
+	uint16_t	_reg16(unsigned reg);
+
+	/**
+	 * Wrapper around transfer() that prevents interrupt-context transfers
+	 * from pre-empting us. The sensor may (does) share a bus with sensors
+	 * that are polled from interrupt context (or we may be pre-empted)
+	 * so we need to guarantee that transfers complete without interruption.
+	 */
+	int		_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+};
+
+device::Device *
+MS5611_spi_interface(ms5611::prom_u &prom_buf)
+{
+	return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+}
+
+MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000),
+	_prom(prom_buf)
+{
+}
+
+MS5611_SPI::~MS5611_SPI()
+{
+}
+
+int
+MS5611_SPI::init()
+{
+	int ret;
+	irqstate_t flags;
+
+	ret = SPI::init();
+	if (ret != OK) {
+		debug("SPI init failed");
+		goto out;
+	}
+
+	/* send reset command */
+	ret = _reset();
+	if (ret != OK) {
+		debug("reset failed");
+		goto out;
+	}
+
+	/* read PROM */
+	ret = _read_prom();
+	if (ret != OK) {
+		debug("prom readout failed");
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+int
+MS5611_SPI::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[4];
+
+	/* read the most recent measurement */
+	buf[0] = 0 | DIR_WRITE;
+	int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
+
+	if (ret == OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[3];
+		cvt->b[1] = buf[2];
+		cvt->b[2] = buf[1];
+		cvt->b[3] = 0;
+
+		ret = count;
+	}
+
+	return ret;
+}
+
+int
+MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+	case IOCTL_RESET:
+		ret = _reset();
+		break;
+
+	case IOCTL_MEASURE:
+		ret = _measure(arg);
+		break;
+
+	default:
+		ret = EINVAL;
+	}
+
+	if (ret != OK) {
+		errno = ret;
+		return -1;
+	}
+	return 0;
+}
+
+int
+MS5611_SPI::_reset()
+{
+	uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE;
+
+	return  _transfer(&cmd, nullptr, 1);
+}
+
+int
+MS5611_SPI::_measure(unsigned addr)
+{
+	uint8_t cmd = addr | DIR_WRITE;
+
+	return _transfer(&cmd, nullptr, 1);
+}
+
+
+int
+MS5611_SPI::_read_prom()
+{
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
+		_prom.c[i] = _reg16(cmd);
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+uint16_t
+MS5611_SPI::_reg16(unsigned reg)
+{
+	uint8_t cmd[3];
+
+	cmd[0] = reg | DIR_READ;
+
+	_transfer(cmd, cmd, sizeof(cmd));
+
+	return (uint16_t)(cmd[1] << 8) | cmd[2];
+}
+
+int
+MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+	return transfer(send, recv, len);
+}
+
+#endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a98b527b1..0441566e9 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,7 +34,7 @@
 /**
  * @file fmu.cpp
  *
- * Driver/configurator for the PX4 FMU multi-purpose port.
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
  */
 
 #include <nuttx/config.h>
@@ -57,29 +57,41 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 
+#include <board_config.h>
+
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 
-#include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
+
+#ifdef HRT_PPM_CHANNEL
+# include <systemlib/ppm_decode.h>
+#endif
+
+/*
+ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
+ */
+
+#define CONTROL_INPUT_DROP_LIMIT_MS		20
 
 class PX4FMU : public device::CDev
 {
 public:
 	enum Mode {
+		MODE_NONE,
 		MODE_2PWM,
 		MODE_4PWM,
-		MODE_NONE
+		MODE_6PWM,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -95,7 +107,12 @@ public:
 	int		set_pwm_alt_channels(uint32_t channels);
 
 private:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	static const unsigned _max_actuators = 4;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	static const unsigned _max_actuators = 6;
+#endif
 
 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -104,7 +121,7 @@ private:
 	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
@@ -112,11 +129,20 @@ private:
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
+	bool		_pwm_on;
 
 	MixerGroup	*_mixers;
 
 	actuator_controls_s _controls;
 
+	pwm_limit_t	_pwm_limit;
+	uint16_t	_failsafe_pwm[_max_actuators];
+	uint16_t	_disarmed_pwm[_max_actuators];
+	uint16_t	_min_pwm[_max_actuators];
+	uint16_t	_max_pwm[_max_actuators];
+	unsigned	_num_failsafe_set;
+	unsigned	_num_disarmed_set;
+
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main() __attribute__((noreturn));
 
@@ -146,6 +172,7 @@ private:
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
 	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
 	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
@@ -154,6 +181,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
 	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
 	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+
+	{0,                      GPIO_VDD_5V_PERIPH_EN,   0},
+	{0,                      GPIO_VDD_3V3_SENSORS_EN, 0},
+	{GPIO_VDD_BRICK_VALID,   0,                       0},
+	{GPIO_VDD_SERVO_VALID,   0,                       0},
+	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
+	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
+#endif
 };
 
 const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -174,15 +217,25 @@ PX4FMU::PX4FMU() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
-	_mixers(nullptr)
+	_pwm_on(false),
+	_mixers(nullptr),
+	_failsafe_pwm({0}),
+	_disarmed_pwm({0}),
+	_num_failsafe_set(0),
+	_num_disarmed_set(0)
 {
+	for (unsigned i = 0; i < _max_actuators; i++) {
+		_min_pwm[i] = PWM_DEFAULT_MIN;
+		_max_pwm[i] = PWM_DEFAULT_MAX;
+	}
+
 	_debug_enabled = true;
 }
 
@@ -271,9 +324,36 @@ PX4FMU::set_mode(Mode mode)
 	 * are presented on the output pins.
 	 */
 	switch (mode) {
-	case MODE_2PWM:	// multi-port with flow control lines as PWM
-	case MODE_4PWM: // multi-port as 4 PWM outs
-		debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
+		debug("MODE_2PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x3);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_4PWM: // v1 multi-port as 4 PWM outs
+		debug("MODE_4PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xf);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_6PWM: // v2 PWMs as 6 PWM outs
+		debug("MODE_6PWM");
 		
 		/* default output rates */
 		_pwm_default_rate = 50;
@@ -281,7 +361,7 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_alt_rate_channels = 0;
 
 		/* XXX magic numbers */
-		up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+		up_pwm_servo_init(0x3f);
 		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 
 		break;
@@ -376,8 +456,8 @@ PX4FMU::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -396,17 +476,20 @@ PX4FMU::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
-	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
+#ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
 	struct rc_input_values rc_in;
 	orb_advert_t to_input_rc = 0;
 
 	memset(&rc_in, 0, sizeof(rc_in));
 	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+#endif
+
+	/* initialize PWM limit lib */
+	pwm_limit_init(&_pwm_limit);
 
 	log("starting");
 
@@ -442,76 +525,107 @@ PX4FMU::task_main()
 
 		/* sleep waiting for data, stopping to check for PPM
 		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, 10);
+		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
-		}
+		} else if (ret == 0) {
+			/* timeout: no control data, switch to failsafe values */
+//			warnx("no PWM: failsafe");
 
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
-			/* can we mix? */
-			if (_mixers != nullptr) {
-
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-				outputs.timestamp = hrt_absolute_time();
-
-				// XXX output actual limited values
-				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
-
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
-				/* iterate actuators */
-				for (unsigned i = 0; i < num_outputs; i++) {
-
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < outputs.noutputs &&
-						isfinite(outputs.output[i]) &&
-						outputs.output[i] >= -1.0f &&
-						outputs.output[i] <= 1.0f) {
-						/* scale for PWM output 900 - 2100us */
-						outputs.output[i] = 1500 + (600 * outputs.output[i]);
-					} else {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = 900;
+		} else {
+
+			/* do we have a control update? */
+			if (fds[0].revents & POLLIN) {
+
+				/* get controls - must always do this to avoid spinning */
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+				/* can we mix? */
+				if (_mixers != nullptr) {
+
+					unsigned num_outputs;
+
+					switch (_mode) {
+					case MODE_2PWM:
+						num_outputs = 2;
+						break;
+					case MODE_4PWM:
+						num_outputs = 4;
+						break;
+					case MODE_6PWM:
+						num_outputs = 6;
+						break;
+					default:
+						num_outputs = 0;
+						break;
 					}
 
-					/* output to the servo */
-					up_pwm_servo_set(i, outputs.output[i]);
-				}
+					/* do mixing */
+					outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+					outputs.timestamp = hrt_absolute_time();
+
+					/* iterate actuators */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						/* last resort: catch NaN, INF and out-of-band errors */
+						if (i >= outputs.noutputs ||
+							!isfinite(outputs.output[i]) ||
+							outputs.output[i] < -1.0f ||
+							outputs.output[i] > 1.0f) {
+							/*
+							 * Value is NaN, INF or out of band - set to the minimum value.
+							 * This will be clearly visible on the servo status and will limit the risk of accidentally
+							 * spinning motors. It would be deadly in flight.
+							 */
+							outputs.output[i] = -1.0f;
+						}
+					}
+
+					uint16_t pwm_limited[num_outputs];
+
+					pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
-				/* and publish for anyone that cares to see */
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+					/* output actual limited values */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						controls_effective.control_effective[i] = (float)pwm_limited[i];
+					}
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+					/* output to the servos */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						up_pwm_servo_set(i, pwm_limited[i]);
+					}
+
+					/* and publish for anyone that cares to see */
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+				}
 			}
-		}
 
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			/* how about an arming update? */
+			if (fds[1].revents & POLLIN) {
+				actuator_armed_s aa;
+
+				/* get new value */
+				orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+				/* update the armed status and check that we're not locked down */
+				bool set_armed = aa.armed && !aa.lockdown;
+				if (_armed != set_armed)
+					_armed = set_armed;
 
-			/* update PWM servo armed status if armed and not locked down */
-			bool set_armed = aa.armed && !aa.lockdown;
-			if (set_armed != _armed) {
-				_armed = set_armed;
-				up_pwm_servo_arm(set_armed);
+				/* update PWM status if armed or if disarmed PWM values are set */
+				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+				if (_pwm_on != pwm_on) {
+					_pwm_on = pwm_on;
+					up_pwm_servo_arm(pwm_on);
+				}
 			}
 		}
 
+#ifdef HRT_PPM_CHANNEL
 		// see if we have new PPM input data
 		if (ppm_last_valid_decode != rc_in.timestamp) {
 			// we have a new PPM frame. Publish it.
@@ -531,11 +645,13 @@ PX4FMU::task_main()
 				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
 			}
 		}
+#endif
+
 	}
 
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -579,6 +695,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (_mode) {
 	case MODE_2PWM:
 	case MODE_4PWM:
+	case MODE_6PWM:
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -615,25 +732,183 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_default_rate;
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_alt_rate;
+		break;
+
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
 		break;
 
-	case PWM_SERVO_SET(2):
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_alt_rate_channels;
+		break;
+
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+				_failsafe_pwm[i] = PWM_HIGHEST_MAX;
+			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				_failsafe_pwm[i] = PWM_LOWEST_MIN;
+			} else {
+				_failsafe_pwm[i] = pwm->values[i];
+			}
+		}
+
+		/*
+		 * update the counter
+		 * this is needed to decide if disarmed PWM output should be turned on or not
+		 */
+		_num_failsafe_set = 0;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			if (_failsafe_pwm[i] > 0)
+				_num_failsafe_set++;
+		}
+		break;
+	}
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _failsafe_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		break;
+	}
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+				_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				_disarmed_pwm[i] = PWM_LOWEST_MIN;
+			} else {
+				_disarmed_pwm[i] = pwm->values[i];
+			}
+		}
+
+		/*
+		 * update the counter
+		 * this is needed to decide if disarmed PWM output should be turned on or not
+		 */
+		_num_disarmed_set = 0;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			if (_disarmed_pwm[i] > 0)
+				_num_disarmed_set++;
+		}
+		break;
+	}
+	case PWM_SERVO_GET_DISARMED_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _disarmed_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		break;
+	}
+
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+				_min_pwm[i] = PWM_HIGHEST_MIN;
+			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				_min_pwm[i] = PWM_LOWEST_MIN;
+			} else {
+				_min_pwm[i] = pwm->values[i];
+			}
+		}
+		break;
+	}
+	case PWM_SERVO_GET_MIN_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _min_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		arg = (unsigned long)&pwm;
+		break;
+	}
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] < PWM_LOWEST_MAX) {
+				_max_pwm[i] = PWM_LOWEST_MAX;
+			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+				_max_pwm[i] = PWM_HIGHEST_MAX;
+			} else {
+				_max_pwm[i] = pwm->values[i];
+			}
+		}
+		break;
+	}
+	case PWM_SERVO_GET_MAX_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _max_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		arg = (unsigned long)&pwm;
+		break;
+	}
+
+	case PWM_SERVO_SET(5):
+	case PWM_SERVO_SET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
 	case PWM_SERVO_SET(3):
-		if (_mode != MODE_4PWM) {
+	case PWM_SERVO_SET(2):
+		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
 		/* FALLTHROUGH */
-	case PWM_SERVO_SET(0):
 	case PWM_SERVO_SET(1):
-		if (arg < 2100) {
+	case PWM_SERVO_SET(0):
+		if (arg <= 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
 		} else {
 			ret = -EINVAL;
@@ -641,16 +916,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
-	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(5):
+	case PWM_SERVO_GET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
 	case PWM_SERVO_GET(3):
-		if (_mode != MODE_4PWM) {
+	case PWM_SERVO_GET(2):
+		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
 		/* FALLTHROUGH */
-	case PWM_SERVO_GET(0):
 	case PWM_SERVO_GET(1):
+	case PWM_SERVO_GET(0):
 		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
 		break;
 
@@ -658,16 +941,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(1):
 	case PWM_SERVO_GET_RATEGROUP(2):
 	case PWM_SERVO_GET_RATEGROUP(3):
+	case PWM_SERVO_GET_RATEGROUP(4):
+	case PWM_SERVO_GET_RATEGROUP(5):
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
 	case PWM_SERVO_GET_COUNT:	
 	case MIXERIOCGETOUTPUTCOUNT:
-		if (_mode == MODE_4PWM) {
+		switch (_mode) {
+		case MODE_6PWM:
+			*(unsigned *)arg = 6;
+			break;
+		case MODE_4PWM:
 			*(unsigned *)arg = 4;
-
-		} else {
+			break;
+		case MODE_2PWM:
 			*(unsigned *)arg = 2;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
 		}
 
 		break;
@@ -743,17 +1036,17 @@ ssize_t
 PX4FMU::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	uint16_t values[4];
+	uint16_t values[6];
 
-	if (count > 4) {
-		// we only have 4 PWM outputs on the FMU
-		count = 4;
+	if (count > 6) {
+		// we have at most 6 outputs
+		count = 6;
 	}
 
 	// allow for misaligned values
-	memcpy(values, buffer, count*2);
+	memcpy(values, buffer, count * 2);
 
-	for (uint8_t i=0; i<count; i++) {
+	for (uint8_t i = 0; i < count; i++) {
 		up_pwm_servo_set(i, values[i]);
 	}
 	return count * 2;
@@ -763,19 +1056,28 @@ void
 PX4FMU::gpio_reset(void)
 {
 	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
+	 * Setup default GPIO config - all pins as GPIOs, input if
+	 * possible otherwise output if possible.
 	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (_gpio_tab[i].input != 0) {
+			stm32_configgpio(_gpio_tab[i].input);
+		} else if (_gpio_tab[i].output != 0) {
+			stm32_configgpio(_gpio_tab[i].output);
+		}
+	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	/* if we have a GPIO direction control, set it to zero (input) */
 	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
 	stm32_configgpio(GPIO_GPIO_DIR);
+#endif
 }
 
 void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
@@ -787,6 +1089,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		if (GPIO_SET_OUTPUT == function)
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 	}
+#endif
 
 	/* configure selected GPIOs as required */
 	for (unsigned i = 0; i < _ngpio; i++) {
@@ -809,9 +1112,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		}
 	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	/* flip buffer to input mode if required */
 	if ((GPIO_SET_INPUT == function) && (gpios & 3))
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+#endif
 }
 
 void
@@ -912,14 +1217,21 @@ fmu_new_mode(PortMode new_mode)
 		/* nothing more to do here */
 		break;
 
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
 	case PORT_FULL_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+		servo_mode = PX4FMU::MODE_6PWM;
+#endif
+		break;
+
+	/* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
 		break;
 
 	case PORT_GPIO_AND_SERIAL:
@@ -938,6 +1250,9 @@ fmu_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode */
 		servo_mode = PX4FMU::MODE_2PWM;
 		break;
+#endif
+	default:
+		return -1;
 	}
 
 	/* adjust GPIO config for serial mode(s) */
@@ -975,25 +1290,104 @@ fmu_start(void)
 	return ret;
 }
 
+int
+fmu_stop(void)
+{
+	int ret = OK;
+
+	if (g_fmu != nullptr) {
+
+		delete g_fmu;
+		g_fmu = nullptr;
+	}
+
+	return ret;
+}
+
 void
 test(void)
 {
-	int	fd;
-
-	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+	int	 fd;
+        unsigned servo_count = 0;
+	unsigned pwm_value = 1000;
+	int	 direction = 1;
+	int	 ret;
+        
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0)
 		errx(1, "open fail");
 
 	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
 
-	if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+        if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+            err(1, "Unable to get servo count\n");        
+        }
+
+	warnx("Testing %u servos", (unsigned)servo_count);
+
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+
+	warnx("Press CTRL-C or 'c' to abort.");
+
+	for (;;) {
+		/* sweep all servos between 1000..2000 */
+		servo_position_t servos[servo_count];
+		for (unsigned i = 0; i < servo_count; i++)
+			servos[i] = pwm_value;
+
+                if (direction == 1) {
+                    // use ioctl interface for one direction
+                    for (unsigned i=0; i < servo_count;	i++) {
+                        if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+                            err(1, "servo %u set failed", i);
+                        }
+                    }
+                } else {
+                    // and use write interface for the other direction
+                    ret = write(fd, servos, sizeof(servos));
+                    if (ret != (int)sizeof(servos))
+			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+                }
+
+		if (direction > 0) {
+			if (pwm_value < 2000) {
+				pwm_value++;
+			} else {
+				direction = -1;
+			}
+		} else {
+			if (pwm_value > 1000) {
+				pwm_value--;
+			} else {
+				direction = 1;
+			}
+		}
 
-	if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+		/* readback servo values */
+		for (unsigned i = 0; i < servo_count; i++) {
+			servo_position_t value;
 
-	if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+				err(1, "error reading PWM servo %d", i);
+			if (value != servos[i])
+				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+		}
 
-	if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+		/* Check if user wants to quit */
+		char c;
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
+			if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("User abort\n");
+                                break;
+			}
+		}
+	}
 
 	close(fd);
 
@@ -1044,6 +1438,12 @@ fmu_main(int argc, char *argv[])
 	PortMode new_mode = PORT_MODE_UNSET;
 	const char *verb = argv[1];
 
+	if (!strcmp(verb, "stop")) {
+		fmu_stop();
+		errx(0, "FMU driver stopped");
+	}
+
+
 	if (fmu_start() != OK)
 		errx(1, "failed to start the FMU driver");
 
@@ -1053,12 +1453,13 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "mode_gpio")) {
 		new_mode = PORT_FULL_GPIO;
 
-	} else if (!strcmp(verb, "mode_serial")) {
-		new_mode = PORT_FULL_SERIAL;
-
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	} else if (!strcmp(verb, "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
+
 	} else if (!strcmp(verb, "mode_gpio_serial")) {
 		new_mode = PORT_GPIO_AND_SERIAL;
 
@@ -1067,6 +1468,7 @@ fmu_main(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "mode_pwm_gpio")) {
 		new_mode = PORT_PWM_AND_GPIO;
+#endif
 	}
 
 	/* was a new mode set? */
@@ -1088,6 +1490,10 @@ fmu_main(int argc, char *argv[])
 		fake(argc - 1, argv + 1);
 
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	fprintf(stderr, "  mode_gpio, mode_pwm, test\n");
+#endif
 	exit(1);
 }
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index 05bc7a5b3..d918abd57 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -3,4 +3,5 @@
 #
 
 MODULE_COMMAND	 = fmu
-SRCS		 = fmu.cpp
+SRCS		 = fmu.cpp \
+			../../modules/systemlib/pwm_limit/pwm_limit.c
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 328e5a684..2054faa12 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -38,4 +38,9 @@
 MODULE_COMMAND		= px4io
 
 SRCS			= px4io.cpp \
-			  uploader.cpp
+			  px4io_uploader.cpp \
+			  px4io_serial.cpp \
+			  px4io_i2c.cpp
+
+# XXX prune to just get UART registers
+INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index fed83ea1a..08e697b9f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -58,7 +58,6 @@
 #include <arch/board/board.h>
 
 #include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
@@ -75,51 +74,71 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
 #include <uORB/topics/parameter_update.h>
+
 #include <debug.h>
 
 #include <mavlink/mavlink_log.h>
-#include "uploader.h"
 #include <modules/px4iofirmware/protocol.h>
 
+#include "uploader.h"
+
+extern device::Device *PX4IO_i2c_interface() weak_function;
+extern device::Device *PX4IO_serial_interface() weak_function;
+
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+#define UPDATE_INTERVAL_MIN		2			// 2 ms	-> 500 Hz
+#define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
+#define IO_POLL_INTERVAL		20000		// 20 ms -> 50 Hz
+
 /**
  * The PX4IO class.
  *
  * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
  */
-class PX4IO : public device::I2C
+class PX4IO : public device::CDev
 {
 public:
 	/**
 	 * Constructor.
-	 * 
+	 *
 	 * Initialize all class variables.
 	 */
-	PX4IO();
+	PX4IO(device::Device *interface);
+
 	/**
 	 * Destructor.
-	 * 
+	 *
 	 * Wait for worker thread to terminate.
 	 */
 	virtual ~PX4IO();
 
 	/**
 	 * Initialize the PX4IO class.
-	 * 
-	 * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 *
+	 * Retrieve relevant initial system parameters. Initialize PX4IO registers.
 	 */
 	virtual int		init();
 
 	/**
+	 * Detect if a PX4IO is connected.
+	 *
+	 * Only validate if there is a PX4IO to talk to.
+	 */
+	virtual int		detect();
+
+	/**
 	 * IO Control handler.
-	 * 
+	 *
 	 * Handle all IOCTL calls to the PX4IO file descriptor.
 	 *
 	 * @param[in] filp file handle (not used). This function is always called directly through object reference
@@ -130,7 +149,7 @@ public:
 
 	/**
 	 * write handler.
-	 * 
+	 *
 	 * Handle writes to the PX4IO file descriptor.
 	 *
 	 * @param[in] filp file handle (not used). This function is always called directly through object reference
@@ -143,8 +162,8 @@ public:
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param[in] rate The rate in Hz actuator output are sent to IO.
-	*      Min 10 Hz, max 400 Hz
+	* @param[in] rate		The rate in Hz actuator outpus are sent to IO.
+	* 			Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
 
@@ -159,12 +178,17 @@ public:
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param[in] len Number of channels, could up to 8
+	 * @param[in] vals	Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] len	Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
+	 * Disable RC input handling
+	 */
+	int			disable_rc_handling();
+
+	/**
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
@@ -172,12 +196,17 @@ public:
 	void			print_status();
 
 	/**
+	 * Fetch and print debug console output.
+	 */
+	int			print_debug();
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	/**
 	 * Set the DSM VCC is controlled by relay one flag
 	 *
 	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
 	 */
-	inline void		set_dsm_vcc_ctl(bool enable)
-	{
+	inline void		set_dsm_vcc_ctl(bool enable) {
 		_dsm_vcc_ctl = enable;
 	};
 
@@ -186,25 +215,32 @@ public:
 	 *
 	 * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
 	 */
-	inline bool		get_dsm_vcc_ctl()
-	{
+	inline bool		get_dsm_vcc_ctl() {
 		return _dsm_vcc_ctl;
 	};
+#endif
+
+	inline uint16_t		system_status() const {return _status;}
 
 private:
+	device::Device		*_interface;
+
 	// XXX
+	unsigned		_hardware;			///< Hardware revision
 	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
 	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
 	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
 	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
 	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval;	///<Subscription interval limiting send rate
+	unsigned 		_update_interval;	///< Subscription interval limiting send rate
+	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
 
 	volatile int		_task;			///<worker task id
 	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;		///<mavlink file descriptor
+	int			_mavlink_fd;		///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+	int			_thread_mavlink_fd;	///<mavlink file descriptor for thread.
 
 	perf_counter_t		_perf_update;		///<local performance counter
 
@@ -213,16 +249,19 @@ private:
 	uint16_t		_alarms;		///<Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;		///<actuator controls topic
-	int			_t_armed;		///<system armed control topic
-	int 			_t_vstatus;		///<system / vehicle status
-	int			_t_param;		///<parameter update topic
+	int			_t_actuators;		///< actuator controls topic
+	int			_t_actuator_armed;	///< system armed control topic
+	int 			_t_vehicle_control_mode;///< vehicle control mode topic
+	int			_t_param;		///< parameter update topic
+	int			_t_vehicle_command;	///< vehicle command topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;		///<rc inputs from IO topic
-	orb_advert_t 		_to_actuators_effective; ///<effective actuator controls topic
-	orb_advert_t		_to_outputs;		///<mixed servo outputs topic
-	orb_advert_t		_to_battery;		///<battery status / voltage topic
+	orb_advert_t 		_to_input_rc;		///< rc inputs from io
+	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
+	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
+	orb_advert_t		_to_battery;		///< battery status / voltage
+	orb_advert_t		_to_servorail;		///< servorail status
+	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
 	actuator_controls_effective_s _controls_effective; ///<effective controls
@@ -234,7 +273,9 @@ private:
 	float			_battery_mamphour_total;///<amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
+#endif
 
 	/**
 	 * Trampoline to the worker task
@@ -269,6 +310,11 @@ private:
 	int			io_get_status();
 
 	/**
+	 * Disable RC input handling
+	 */
+	int			io_disable_rc_handling();
+
+	/**
 	 * Fetch RC inputs from IO.
 	 *
 	 * @param input_rc	Input structure to populate.
@@ -298,7 +344,7 @@ private:
 	 * @param offset	Register offset to start writing at.
 	 * @param values	Pointer to array of values to write.
 	 * @param num_values	The number of values to write.
-	 * @return		Zero if all values were successfully written.
+	 * @return		OK if all values were successfully written.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 
@@ -308,7 +354,7 @@ private:
 	 * @param page		Register page to write to.
 	 * @param offset	Register offset to write to.
 	 * @param value		Value to write.
-	 * @return		Zero if the value was written successfully.
+	 * @return		OK if the value was written successfully.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
 
@@ -319,7 +365,7 @@ private:
 	 * @param offset	Register offset to start reading from.
 	 * @param values	Pointer to array where values should be stored.
 	 * @param num_values	The number of values to read.
-	 * @return		Zero if all values were successfully read.
+	 * @return		OK if all values were successfully read.
 	 */
 	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
 
@@ -346,7 +392,7 @@ private:
 	/**
 	 * Send mixer definition text to IO
 	 */
-	int			mixer_send(const char *buf, unsigned buflen);
+	int			mixer_send(const char *buf, unsigned buflen, unsigned retries = 3);
 
 	/**
 	 * Handle a status update from IO.
@@ -366,8 +412,34 @@ private:
 	 */
 	int			io_handle_alarms(uint16_t alarms);
 
-};
+	/**
+	 * Handle issuing dsm bind ioctl to px4io.
+	 *
+	 * @param dsmMode	0:dsm2, 1:dsmx
+	 */
+	void			dsm_bind_ioctl(int dsmMode);
 
+	/**
+	 * Handle a battery update from IO.
+	 *
+	 * Publish IO battery information if necessary.
+	 *
+	 * @param vbatt		vbatt register
+	 * @param ibatt		ibatt register
+	 */
+	void			io_handle_battery(uint16_t vbatt, uint16_t ibatt);
+
+	/**
+	 * Handle a servorail update from IO.
+	 *
+	 * Publish servo rail information if necessary.
+	 *
+	 * @param vservo	vservo register
+	 * @param vrssi 	vrssi register
+	 */
+	void			io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
+
+};
 
 namespace
 {
@@ -376,34 +448,44 @@ PX4IO	*g_dev;
 
 }
 
-PX4IO::PX4IO() :
-	I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+PX4IO::PX4IO(device::Device *interface) :
+	CDev("px4io", PX4IO_DEVICE_PATH),
+	_interface(interface),
+	_hardware(0),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
+	_rc_handling_disabled(false),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
+	_thread_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
 	_status(0),
 	_alarms(0),
 	_t_actuators(-1),
-	_t_armed(-1),
-	_t_vstatus(-1),
+	_t_actuator_armed(-1),
+	_t_vehicle_control_mode(-1),
 	_t_param(-1),
+	_t_vehicle_command(-1),
 	_to_input_rc(0),
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_servorail(0),
+	_to_safety(0),
 	_primary_pwm_device(false),
-	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
+	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_battery_last_timestamp(0)
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	, _dsm_vcc_ctl(false)
+#endif
+
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -429,10 +511,47 @@ PX4IO::~PX4IO()
 	if (_task != -1)
 		task_delete(_task);
 
+	if (_interface != nullptr)
+		delete _interface;
+
 	g_dev = nullptr;
 }
 
 int
+PX4IO::detect()
+{
+	int ret;
+
+	if (_task == -1) {
+
+		/* do regular cdev init */
+		ret = CDev::init();
+
+		if (ret != OK)
+			return ret;
+
+		/* get some parameters */
+		unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+
+		if (protocol != PX4IO_PROTOCOL_VERSION) {
+			if (protocol == _io_reg_get_error) {
+				log("IO not installed");
+
+			} else {
+				log("IO version error");
+				mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
+			}
+
+			return -1;
+		}
+	}
+
+	log("IO found");
+
+	return 0;
+}
+
+int
 PX4IO::init()
 {
 	int ret;
@@ -440,33 +559,37 @@ PX4IO::init()
 	ASSERT(_task == -1);
 
 	/* do regular cdev init */
-	ret = I2C::init();
+	ret = CDev::init();
+
 	if (ret != OK)
 		return ret;
 
-	/*
-	 * Enable a couple of retries for operations to IO.
-	 *
-	 * Register read/write operations are intentionally idempotent
-	 * so this is safe as designed.
-	 */
-	_retries = 2;
-
 	/* get some parameters */
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		log("protocol/firmware mismatch");
+		mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
+		return -1;
+	}
+
+	_hardware      = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
-	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
+	_max_controls  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
 	_max_relays    = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
+
 	if ((_max_actuators < 1) || (_max_actuators > 255) ||
-	    (_max_relays < 1)    || (_max_relays > 255)    ||
-	    (_max_transfer < 16)   || (_max_transfer > 255)    ||
+	    (_max_relays > 32)   ||
+	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
-		log("failed getting parameters from PX4IO");
-		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+		log("config read error");
+		mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
 		return -1;
 	}
+
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
 		_max_rc_input = RC_INPUT_MAX_CHANNELS;
 
@@ -481,6 +604,7 @@ PX4IO::init()
 
 	/* get IO's last seen FMU state */
 	ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
+
 	if (ret != OK)
 		return ret;
 
@@ -491,27 +615,31 @@ PX4IO::init()
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
 	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
-	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+		/* get a status update from IO */
+		io_get_status();
+
+		mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+		log("INAIR RESTART RECOVERY (needs commander app running)");
 
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
 		/* fill with initial values, clear updated flag */
-		vehicle_status_s status;
+		struct actuator_armed_s safety;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
-		
-		/* keep checking for an update, ensure we got a recent state,
+
+		/* keep checking for an update, ensure we got a arming information,
 		   not something that was published a long time ago. */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 				break;
 			}
 
@@ -519,7 +647,7 @@ PX4IO::init()
 			usleep(10000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) {
 				log("failed to recover from in-air restart (1), aborting IO driver init.");
 				return 1;
 			}
@@ -537,52 +665,65 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		cmd.target_system = status.system_id;
-		cmd.target_component = status.component_id;
-		cmd.source_system = status.system_id;
-		cmd.source_component = status.component_id;
+		// cmd.target_system = status.system_id;
+		// cmd.target_component = status.component_id;
+		// cmd.source_system = status.system_id;
+		// cmd.source_component = status.component_id;
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 
 		/* send command once */
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
 
 		/* spin here until IO's state has propagated into the system */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 			}
 
-			/* wait 10 ms */
-			usleep(10000);
+			/* wait 50 ms */
+			usleep(50000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
 				log("failed to recover from in-air restart (2), aborting IO driver init.");
 				return 1;
 			}
 
-		/* keep waiting for state change for 10 s */
-		} while (!status.flag_system_armed);
+			/* re-send if necessary */
+			if (!safety.armed) {
+				orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+				log("re-sending arm cmd");
+			}
 
-	/* regular boot, no in-air restart, init IO */
-	} else {
+			/* keep waiting for state change for 2 s */
+		} while (!safety.armed);
 
+		/* regular boot, no in-air restart, init IO */
+
+	} else {
 
 		/* dis-arm IO before touching anything */
-		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
-			PX4IO_P_SETUP_ARMING_FMU_ARMED |
-			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
+			      PX4IO_P_SETUP_ARMING_FMU_ARMED |
+			      PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
+			      PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+			      PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
 
-		/* publish RC config to IO */
-		ret = io_set_rc_config();
-		if (ret != OK) {
-			log("failed to update RC input config");
-			mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
-			return ret;
+		if (_rc_handling_disabled) {
+			ret = io_disable_rc_handling();
+
+		} else {
+			/* publish RC config to IO */
+			ret = io_set_rc_config();
+
+			if (ret != OK) {
+				log("failed to update RC input config");
+				mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+				return ret;
+			}
 		}
 
 	}
@@ -596,7 +737,7 @@ PX4IO::init()
 	}
 
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -617,11 +758,12 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
 void
 PX4IO::task_main()
 {
-	hrt_abstime last_poll_time = 0;
-	int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+	hrt_abstime poll_last = 0;
+	hrt_abstime orb_check_last = 0;
 
 	log("starting");
 
+	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -630,28 +772,26 @@ PX4IO::task_main()
 	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
 				     ORB_ID(actuator_controls_1));
 	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
-
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
-
-	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
-
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
-	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
+	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+
+	if ((_t_actuators < 0) ||
+	    (_t_actuator_armed < 0) ||
+	    (_t_vehicle_control_mode < 0) ||
+	    (_t_param < 0) ||
+	    (_t_vehicle_command < 0)) {
+		log("subscription(s) failed");
+		goto out;
+	}
 
 	/* poll descriptor */
-	pollfd fds[4];
+	pollfd fds[1];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
-	fds[1].events = POLLIN;
-	fds[2].fd = _t_vstatus;
-	fds[2].events = POLLIN;
-	fds[3].fd = _t_param;
-	fds[3].events = POLLIN;
 
-	debug("ready");
+	log("ready");
 
 	/* lock against the ioctl handler */
 	lock();
@@ -661,17 +801,19 @@ PX4IO::task_main()
 
 		/* adjust update interval */
 		if (_update_interval != 0) {
-			if (_update_interval < 5)
-				_update_interval = 5;
+			if (_update_interval < UPDATE_INTERVAL_MIN)
+				_update_interval = UPDATE_INTERVAL_MIN;
+
 			if (_update_interval > 100)
 				_update_interval = 100;
+
 			orb_set_interval(_t_actuators, _update_interval);
 			_update_interval = 0;
 		}
 
 		/* sleep waiting for topic updates, but no more than 20ms */
 		unlock();
-		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20);
+		int ret = ::poll(fds, 1, 20);
 		lock();
 
 		/* this would be bad... */
@@ -684,67 +826,79 @@ PX4IO::task_main()
 		hrt_abstime now = hrt_absolute_time();
 
 		/* if we have new control data from the ORB, handle it */
-		if (fds[0].revents & POLLIN)
+		if (fds[0].revents & POLLIN) {
 			io_set_control_state();
+		}
 
-		/* if we have an arming state update, handle it */
-		if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
-			io_set_arming_state();
-
-		/*
-		 * If it's time for another tick of the polling status machine,
-		 * try it now.
-		 */
-		if ((now - last_poll_time) >= 20000) {
+		if (now >= poll_last + IO_POLL_INTERVAL) {
+			/* run at 50Hz */
+			poll_last = now;
 
-			/*
-			 * Pull status and alarms from IO.
-			 */
+			/* pull status and alarms from IO */
 			io_get_status();
 
-			/*
-			 * Get raw R/C input from IO.
-			 */
+			/* get raw R/C input from IO */
 			io_publish_raw_rc();
 
-			/*
-			 * Fetch mixed servo controls and PWM outputs from IO.
-			 *
-			 * XXX We could do this at a reduced rate in many/most cases.
-			 */
+			/* fetch mixed servo controls and PWM outputs from IO */
 			io_publish_mixed_controls();
 			io_publish_pwm_outputs();
+		}
+
+		if (now >= orb_check_last + ORB_CHECK_INTERVAL) {
+			/* run at 5Hz */
+			orb_check_last = now;
+
+			/* check updates on uORB topics and handle it */
+			bool updated = false;
+
+			/* arming state */
+			orb_check(_t_actuator_armed, &updated);
+
+			if (!updated) {
+				orb_check(_t_vehicle_control_mode, &updated);
+			}
+
+			if (updated) {
+				io_set_arming_state();
+			}
+
+			/* vehicle command */
+			orb_check(_t_vehicle_command, &updated);
+
+			if (updated) {
+				struct vehicle_command_s cmd;
+				orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+
+				// Check for a DSM pairing command
+				if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) {
+					dsm_bind_ioctl((int)cmd.param2);
+				}
+			}
 
 			/*
 			 * If parameters have changed, re-send RC mappings to IO
 			 *
 			 * XXX this may be a bit spammy
 			 */
-			if (fds[3].revents & POLLIN) {
+			orb_check(_t_param, &updated);
+
+			if (updated) {
 				parameter_update_s pupdate;
+				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
+
 				int32_t dsm_bind_val;
 				param_t dsm_bind_param;
 
-				// See if bind parameter has been set, and reset it to 0
+				/* see if bind parameter has been set, and reset it to -1 */
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
-				if (dsm_bind_val) {
-					if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) {
-						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
-							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
-							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
-						} else {
-							mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
-						}
-					} else {
-						mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); 
-					}
-					dsm_bind_val = 0;
+
+				if (dsm_bind_val > -1) {
+					dsm_bind_ioctl(dsm_bind_val);
+					dsm_bind_val = -1;
 					param_set(dsm_bind_param, &dsm_bind_val);
 				}
 
-				/* copy to reset the notification */
-				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
-
 				/* re-upload RC input config as it may have changed */
 				io_set_rc_config();
 			}
@@ -755,6 +909,7 @@ PX4IO::task_main()
 
 	unlock();
 
+out:
 	debug("exiting");
 
 	/* clean up the alternate device node */
@@ -774,7 +929,7 @@ PX4IO::io_set_control_state()
 
 	/* get controls */
 	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-	     ORB_ID(actuator_controls_1), _t_actuators, &controls);
+		 ORB_ID(actuator_controls_1), _t_actuators, &controls);
 
 	for (unsigned i = 0; i < _max_controls; i++)
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
@@ -783,43 +938,36 @@ PX4IO::io_set_control_state()
 	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
 }
 
-int
-PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
-{
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
-}
 
 int
 PX4IO::io_set_arming_state()
 {
 	actuator_armed_s	armed;		///< system armed state
-	vehicle_status_s	vstatus;	///< overall system state
+	vehicle_control_mode_s	control_mode;	///< vehicle_control_mode
 
-	orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
-	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+	orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
+	orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
+
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
 
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
 
-	if (vstatus.flag_external_manual_override_ok) {
+	if (control_mode.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	}
@@ -828,6 +976,21 @@ PX4IO::io_set_arming_state()
 }
 
 int
+PX4IO::disable_rc_handling()
+{
+	return io_disable_rc_handling();
+}
+
+int
+PX4IO::io_disable_rc_handling()
+{
+	uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
+	uint16_t clear = 0;
+
+	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
+int
 PX4IO::io_set_rc_config()
 {
 	unsigned offset = 0;
@@ -849,22 +1012,27 @@ PX4IO::io_set_rc_config()
 	 *       autopilots / GCS'.
 	 */
 	param_get(param_find("RC_MAP_ROLL"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 0;
 
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 1;
 
 	param_get(param_find("RC_MAP_YAW"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 2;
 
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
 	ichan = 4;
+
 	for (unsigned i = 0; i < _max_rc_input; i++)
 		if (input_map[i] == -1)
 			input_map[i] = ichan++;
@@ -919,6 +1087,7 @@ PX4IO::io_set_rc_config()
 
 		/* send channel config to IO */
 		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+
 		if (ret != OK) {
 			log("rc config upload failed");
 			break;
@@ -945,14 +1114,15 @@ PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
-		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+	    && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+
 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 
 		/* set the sync flag */
@@ -967,9 +1137,50 @@ PX4IO::io_handle_status(uint16_t status)
 		_status = status;
 	}
 
+	/**
+	 * Get and handle the safety status
+	 */
+	struct safety_s safety;
+	safety.timestamp = hrt_absolute_time();
+
+	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+		safety.safety_off = true;
+		safety.safety_switch_available = true;
+
+	} else {
+		safety.safety_off = false;
+		safety.safety_switch_available = true;
+	}
+
+	/* lazily publish the safety status */
+	if (_to_safety > 0) {
+		orb_publish(ORB_ID(safety), _to_safety, &safety);
+
+	} else {
+		_to_safety = orb_advertise(ORB_ID(safety), &safety);
+	}
+
 	return ret;
 }
 
+void
+PX4IO::dsm_bind_ioctl(int dsmMode)
+{
+	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+		/* 0: dsm2, 1:dsmx */
+		if ((dsmMode == 0) || (dsmMode == 1)) {
+			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8"));
+			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
+		} else {
+			mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
+		}
+
+	} else {
+		mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
+	}
+}
+
+
 int
 PX4IO::io_handle_alarms(uint16_t alarms)
 {
@@ -983,53 +1194,89 @@ PX4IO::io_handle_alarms(uint16_t alarms)
 	return 0;
 }
 
+void
+PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
+{
+	/* only publish if battery has a valid minimum voltage */
+	if (vbatt <= 3300) {
+		return;
+	}
+
+	battery_status_s	battery_status;
+	battery_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	battery_status.voltage_v = vbatt / 1000.0f;
+
+	/*
+	  ibatt contains the raw ADC count, as 12 bit ADC
+	  value, with full range being 3.3v
+	*/
+	battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
+	battery_status.current_a += _battery_amp_bias;
+
+	/*
+	  integrate battery over time to get total mAh used
+	*/
+	if (_battery_last_timestamp != 0) {
+		_battery_mamphour_total += battery_status.current_a *
+					   (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+	}
+
+	battery_status.discharged_mah = _battery_mamphour_total;
+	_battery_last_timestamp = battery_status.timestamp;
+
+	/* lazily publish the battery voltage */
+	if (_to_battery > 0) {
+		orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+
+	} else {
+		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+	}
+}
+
+void
+PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
+{
+	servorail_status_s servorail_status;
+	servorail_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	servorail_status.voltage_v = vservo * 0.001f;
+	servorail_status.rssi_v    = vrssi * 0.001f;
+
+	/* lazily publish the servorail voltages */
+	if (_to_servorail > 0) {
+		orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+
+	} else {
+		_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+	}
+}
+
 int
 PX4IO::io_get_status()
 {
-	uint16_t	regs[4];
+	uint16_t	regs[6];
 	int		ret;
 
 	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
 	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
+
 	if (ret != OK)
 		return ret;
 
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
 
-	/* only publish if battery has a valid minimum voltage */
-	if (regs[2] > 3300) {
-		battery_status_s	battery_status;
-
-		battery_status.timestamp = hrt_absolute_time();
-
-		/* voltage is scaled to mV */
-		battery_status.voltage_v = regs[2] / 1000.0f;
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	io_handle_battery(regs[2], regs[3]);
+#endif
 
-		/*
-		  regs[3] contains the raw ADC count, as 12 bit ADC
-		  value, with full range being 3.3v
-		 */
-		battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
-		battery_status.current_a += _battery_amp_bias;
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	io_handle_vservo(regs[4], regs[5]);
+#endif
 
-		/*
-		  integrate battery over time to get total mAh used
-		 */
-		if (_battery_last_timestamp != 0) {
-			_battery_mamphour_total += battery_status.current_a * 
-				(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
-		}
-		battery_status.discharged_mah = _battery_mamphour_total;
-		_battery_last_timestamp = battery_status.timestamp;
-
-		/* lazily publish the battery voltage */
-		if (_to_battery > 0) {
-			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
-		} else {
-			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
-		}
-	}
 	return ret;
 }
 
@@ -1037,38 +1284,41 @@ int
 PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 {
 	uint32_t channel_count;
-	int	ret = OK;
+	int	ret;
 
 	/* we don't have the status bits, so input_source has to be set elsewhere */
 	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
-	
+
+	static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
+
 	/*
-	 * XXX Because the channel count and channel data are fetched
-	 *     separately, there is a risk of a race between the two
-	 *     that could leave us with channel data and a count that 
-	 *     are out of sync.
-	 *     Fixing this would require a guarantee of atomicity from
-	 *     IO, and a single fetch for both count and channels.
+	 * Read the channel count and the first 9 channels.
 	 *
-	 * XXX Since IO has the input calibration info, we ought to be
-	 *     able to get the pre-fixed-up controls directly.
-	 *
-	 * XXX can we do this more cheaply? If we knew we had DMA, it would
-	 *     almost certainly be better to just get all the inputs...
+	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	channel_count =  io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
-	if (channel_count == _io_reg_get_error)
-		return -EIO;
-	if (channel_count > RC_INPUT_MAX_CHANNELS)
-		channel_count = RC_INPUT_MAX_CHANNELS;
-	input_rc.channel_count = channel_count;
+	input_rc.timestamp = hrt_absolute_time();
+	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
 
-	if (channel_count > 0) {
-		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
-		if (ret == OK)
-			input_rc.timestamp = hrt_absolute_time();
+	if (ret != OK)
+		return ret;
+
+	/*
+	 * Get the channel count any any extra channels. This is no more expensive than reading the
+	 * channel count once.
+	 */
+	channel_count = regs[0];
+
+	if (channel_count > 9) {
+		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
+
+		if (ret != OK)
+			return ret;
 	}
 
+	input_rc.channel_count = channel_count;
+	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+
 	return ret;
 }
 
@@ -1084,16 +1334,20 @@ PX4IO::io_publish_raw_rc()
 	rc_val.timestamp = hrt_absolute_time();
 
 	int ret = io_get_raw_rc_input(rc_val);
+
 	if (ret != OK)
 		return ret;
 
 	/* sort out the source of the values */
 	if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 	}
@@ -1101,7 +1355,8 @@ PX4IO::io_publish_raw_rc()
 	/* lazily advertise on first publication */
 	if (_to_input_rc == 0) {
 		_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
-	} else { 
+
+	} else {
 		orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
 	}
 
@@ -1126,6 +1381,7 @@ PX4IO::io_publish_mixed_controls()
 	/* get actuator controls from IO */
 	uint16_t act[_max_actuators];
 	int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
+
 	if (ret != OK)
 		return ret;
 
@@ -1135,16 +1391,17 @@ PX4IO::io_publish_mixed_controls()
 
 	/* laxily advertise on first publication */
 	if (_to_actuators_effective == 0) {
-		_to_actuators_effective = 
-			orb_advertise((_primary_pwm_device ? 
-				ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
-				ORB_ID(actuator_controls_effective_1)),
-					   &controls_effective);
+		_to_actuators_effective =
+			orb_advertise((_primary_pwm_device ?
+				       ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
+				       ORB_ID(actuator_controls_effective_1)),
+				      &controls_effective);
+
 	} else {
-		orb_publish((_primary_pwm_device ? 
-			ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
-			ORB_ID(actuator_controls_effective_1)),
-			_to_actuators_effective, &controls_effective);
+		orb_publish((_primary_pwm_device ?
+			     ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
+			     ORB_ID(actuator_controls_effective_1)),
+			    _to_actuators_effective, &controls_effective);
 	}
 
 	return OK;
@@ -1164,26 +1421,29 @@ PX4IO::io_publish_pwm_outputs()
 	/* get servo values from IO */
 	uint16_t ctl[_max_actuators];
 	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
+
 	if (ret != OK)
 		return ret;
 
 	/* convert from register format to float */
 	for (unsigned i = 0; i < _max_actuators; i++)
 		outputs.output[i] = ctl[i];
+
 	outputs.noutputs = _max_actuators;
 
 	/* lazily advertise on first publication */
 	if (_to_outputs == 0) {
 		_to_outputs = orb_advertise((_primary_pwm_device ?
-			ORB_ID_VEHICLE_CONTROLS :
-			ORB_ID(actuator_outputs_1)),
-			&outputs);
+					     ORB_ID_VEHICLE_CONTROLS :
+					     ORB_ID(actuator_outputs_1)),
+					    &outputs);
+
 	} else {
 		orb_publish((_primary_pwm_device ?
-			ORB_ID_VEHICLE_CONTROLS :
-			ORB_ID(actuator_outputs_1)),
-			_to_outputs,
-			&outputs);
+			     ORB_ID_VEHICLE_CONTROLS :
+			     ORB_ID(actuator_outputs_1)),
+			    _to_outputs,
+			    &outputs);
 	}
 
 	return OK;
@@ -1198,25 +1458,14 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
 		return -EINVAL;
 	}
 
-	/* set up the transfer */
-	uint8_t 	addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
+	int ret =  _interface->write((page << 8) | offset, (void *)values, num_values);
 
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_NORESTART;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
+	if (ret != (int)num_values) {
+		debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
+		return -1;
+	}
 
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_set: error %d", ret);
-	return ret;
+	return OK;
 }
 
 int
@@ -1228,25 +1477,20 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
 int
 PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
 {
-	/* set up the transfer */
-	uint8_t		addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
+	/* range check the transfer */
+	if (num_values > ((_max_transfer) / sizeof(*values))) {
+		debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+		return -EINVAL;
+	}
 
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_READ;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
+	int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
 
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_get: data error %d", ret);
-	return ret;
+	if (ret != (int)num_values) {
+		debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
+		return -1;
+	}
+
+	return OK;
 }
 
 uint32_t
@@ -1254,7 +1498,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset)
 {
 	uint16_t value;
 
-	if (io_reg_get(page, offset, &value, 1))
+	if (io_reg_get(page, offset, &value, 1) != OK)
 		return _io_reg_get_error;
 
 	return value;
@@ -1267,8 +1511,10 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 	uint16_t value;
 
 	ret = io_reg_get(page, offset, &value, 1);
-	if (ret)
+
+	if (ret != OK)
 		return ret;
+
 	value &= ~clearbits;
 	value |= setbits;
 
@@ -1276,61 +1522,135 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 }
 
 int
-PX4IO::mixer_send(const char *buf, unsigned buflen)
+PX4IO::print_debug()
 {
-	uint8_t	frame[_max_transfer];
-	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
-	unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	int io_fd = -1;
 
-	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
-	msg->action = F2I_MIXER_ACTION_RESET;
+	if (io_fd < 0) {
+		io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
+	}
 
-	do {
-		unsigned count = buflen;
+	/* read IO's output */
+	if (io_fd > 0) {
+		pollfd fds[1];
+		fds[0].fd = io_fd;
+		fds[0].events = POLLIN;
 
-		if (count > max_len)
-			count = max_len;
+		usleep(500);
+		int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0);
 
-		if (count > 0) {
-			memcpy(&msg->text[0], buf, count);
-			buf += count;
-			buflen -= count;
-		}
+		if (pret > 0) {
+			int count;
+			char buf[65];
 
-		/*
-		 * We have to send an even number of bytes.  This
-		 * will only happen on the very last transfer of a
-		 * mixer, and we are guaranteed that there will be
-		 * space left to round up as _max_transfer will be
-		 * even.
-		 */
-		unsigned total_len = sizeof(px4io_mixdata) + count;
-		if (total_len % 1) {
-			msg->text[count] = '\0';
-			total_len++;
-		}
+			do {
+				count = ::read(io_fd, buf, sizeof(buf) - 1);
 
-		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+				if (count > 0) {
+					/* enforce null termination */
+					buf[count] = '\0';
+					warnx("IO CONSOLE: %s", buf);
+				}
 
-		if (ret) {
-			log("mixer send error %d", ret);
-			return ret;
+			} while (count > 0);
 		}
 
-		msg->action = F2I_MIXER_ACTION_APPEND;
+		::close(io_fd);
+		return 0;
+	}
+
+#endif
+	return 1;
+
+}
+
+int
+PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
+{
+	/* get debug level */
+	int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG);
+
+	uint8_t	frame[_max_transfer];
 
-	} while (buflen > 0);
+	do {
+
+		px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+		unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
+
+		msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+		msg->action = F2I_MIXER_ACTION_RESET;
+
+		do {
+			unsigned count = buflen;
+
+			if (count > max_len)
+				count = max_len;
+
+			if (count > 0) {
+				memcpy(&msg->text[0], buf, count);
+				buf += count;
+				buflen -= count;
+
+			} else {
+				continue;
+			}
+
+			/*
+			 * We have to send an even number of bytes.  This
+			 * will only happen on the very last transfer of a
+			 * mixer, and we are guaranteed that there will be
+			 * space left to round up as _max_transfer will be
+			 * even.
+			 */
+			unsigned total_len = sizeof(px4io_mixdata) + count;
+
+			if (total_len % 2) {
+				msg->text[count] = '\0';
+				total_len++;
+			}
+
+			int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+			/* print mixer chunk */
+			if (debuglevel > 5 || ret) {
+
+				warnx("fmu sent: \"%s\"", msg->text);
+
+				/* read IO's output */
+				print_debug();
+			}
+
+			if (ret) {
+				log("mixer send error %d", ret);
+				return ret;
+			}
+
+			msg->action = F2I_MIXER_ACTION_APPEND;
+
+		} while (buflen > 0);
+
+		/* ensure a closing newline */
+		msg->text[0] = '\n';
+		msg->text[1] = '\0';
+
+		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+
+		retries--;
+
+		log("mixer sent");
+
+	} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
 
 	/* check for the mixer-OK flag */
 	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
-		debug("mixer upload OK");
 		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
 		return 0;
-	} else {
-		debug("mixer rejected by IO");
-		mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
 	}
 
+	log("mixer rejected by IO");
+	mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+
 	/* load must have failed for some reason */
 	return -EINVAL;
 }
@@ -1339,120 +1659,162 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u software %u bootloader %u buffer %uB\n",
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
+	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
 
 	/* status */
 	printf("%u bytes free\n",
-		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
+	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
 	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
-		flags,
-		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
-		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
-		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
+	       flags,
+	       ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
+	       (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+	       (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
-		alarms,
-		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+	printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
+	       alarms,
+	       ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT)  ? " VSERVO_FAULT" : ""));
 	/* now clear alarms */
 	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
 
-	printf("vbatt %u ibatt %u vbatt scale %u\n",
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
-	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
-	printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
-	       (double)_battery_amp_per_volt,
-	       (double)_battery_amp_bias,
-	       (double)_battery_mamphour_total);
+	if (_hardware == 1) {
+		printf("vbatt mV %u ibatt mV %u vbatt scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+		printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
+		       (double)_battery_amp_per_volt,
+		       (double)_battery_amp_bias,
+		       (double)_battery_mamphour_total);
+
+	} else if (_hardware == 2) {
+		printf("vservo %u mV vservo scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE));
+		printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
+	}
+
 	printf("actuators");
+
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
+
 	printf("\n");
 	printf("servos");
+
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));
+
 	printf("\n");
 	uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
 	printf("%d raw R/C inputs", raw_inputs);
+
 	for (unsigned i = 0; i < raw_inputs; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
+
 	printf("\n");
 	uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
 	printf("mapped R/C inputs 0x%04x", mapped_inputs);
+
 	for (unsigned i = 0; i < _max_rc_input; i++) {
 		if (mapped_inputs & (1 << i))
 			printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)));
 	}
+
 	printf("\n");
 	uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT);
 	printf("ADC inputs");
+
 	for (unsigned i = 0; i < adc_inputs; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i));
+
 	printf("\n");
 
 	/* setup and state */
 	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s\n",
-		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
+	printf("arming 0x%04x%s%s%s%s%s%s\n",
+	       arming,
+	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : " FMU_DISARMED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : " IO_ARM_DENIED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	printf("rates 0x%04x default %u alt %u\n",
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
+#endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
 	printf("controls");
+
 	for (unsigned i = 0; i < _max_controls; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
+
 	printf("\n");
+
 	for (unsigned i = 0; i < _max_rc_input; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
 		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
-			i,
-			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
-			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
-			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
-			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
-			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
-			options,
-			((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
-			((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+		       i,
+		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+		       options,
+		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
 	}
+
 	printf("failsafe");
+
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+
+	printf("\ndisarmed values");
+
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
+
 	printf("\n");
 }
 
@@ -1484,38 +1846,131 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
 		break;
 
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		/* get the default update rate */
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		/* set the requested alternate rate */
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE: {
-
-		/* blindly clear the PWM update alarm - might be set for some other reason */
-		io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+	case PWM_SERVO_GET_UPDATE_RATE:
+		/* get the alternative update rate */
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
+		break;
 
-		/* attempt to set the rate map */
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
 
-		/* check that the changes took */
-		uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-		if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
-			ret = -EINVAL;
+			/* blindly clear the PWM update alarm - might be set for some other reason */
 			io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+
+			/* attempt to set the rate map */
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+
+			/* check that the changes took */
+			uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
+
+			if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
+				ret = -EINVAL;
+				io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+			}
+
+			break;
 		}
+
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
+		break;
+
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
 		break;
 	}
 
+	case PWM_SERVO_GET_FAILSAFE_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_DISARMED_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_MIN_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_MAX_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
 	case PWM_SERVO_GET_COUNT:
 		*(unsigned *)arg = _max_actuators;
 		break;
 
 	case DSM_BIND_START:
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); 
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 		usleep(500000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
-		usleep(50000);
+		usleep(72000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
 		usleep(50000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
@@ -1527,77 +1982,115 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
-		/* TODO: we could go lower for e.g. TurboPWM */
-		unsigned channel = cmd - PWM_SERVO_SET(0);
-		if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
-			ret = -EINVAL;
-		} else {
-			/* send a direct PWM value */
-			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
-		}
+			/* TODO: we could go lower for e.g. TurboPWM */
+			unsigned channel = cmd - PWM_SERVO_SET(0);
 
-		break;
-	}
+			if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) {
+				ret = -EINVAL;
+
+			} else {
+				/* send a direct PWM value */
+				ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+			}
+
+			break;
+		}
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
-		unsigned channel = cmd - PWM_SERVO_GET(0);
+			unsigned channel = cmd - PWM_SERVO_GET(0);
+
+			if (channel >= _max_actuators) {
+				ret = -EINVAL;
 
-		if (channel >= _max_actuators) {
-			ret = -EINVAL;
-		} else {
-			/* fetch a current PWM value */
-			uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
-			if (value == _io_reg_get_error) {
-				ret = -EIO;
 			} else {
-				*(servo_position_t *)arg = value;
+				/* fetch a current PWM value */
+				uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
+
+				if (value == _io_reg_get_error) {
+					ret = -EIO;
+
+				} else {
+					*(servo_position_t *)arg = value;
+				}
 			}
+
+			break;
 		}
-		break;
-	}
 
 	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
-		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+			unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
 
-		uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+			*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
 
-		*(uint32_t *)arg = value;
-		break;
-	}
+			if (*(uint32_t *)arg == _io_reg_get_error)
+				ret = -EIO;
+
+			break;
+		}
 
 	case GPIO_RESET: {
-		uint32_t bits = (1 << _max_relays) - 1;
-		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl)
-			bits &= ~PX4IO_RELAY1;
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
-		break;
-	}
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			uint32_t bits = (1 << _max_relays) - 1;
+
+			/* don't touch relay1 if it's controlling RX vcc */
+			if (_dsm_vcc_ctl)
+				bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
+
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+			ret = -EINVAL;
+#endif
+			break;
+		}
 
 	case GPIO_SET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
+
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+			break;
+		}
+
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_CLEAR:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
+
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+			break;
+		}
+
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_GET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
+
 		if (*(uint32_t *)arg == _io_reg_get_error)
 			ret = -EIO;
+
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
@@ -1609,40 +2102,44 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCLOADBUF: {
-		const char *buf = (const char *)arg;
-		ret = mixer_send(buf, strnlen(buf, 2048));
-		break;
-	}
+			const char *buf = (const char *)arg;
+			ret = mixer_send(buf, strnlen(buf, 2048));
+			break;
+		}
 
 	case RC_INPUT_GET: {
-		uint16_t status;
-		rc_input_values *rc_val = (rc_input_values *)arg;
+			uint16_t status;
+			rc_input_values *rc_val = (rc_input_values *)arg;
 
-		ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1);
-		if (ret != OK)
-			break;
+			ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1);
 
-		/* if no R/C input, don't try to fetch anything */
-		if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) {
-			ret = -ENOTCONN;
-			break;
-		}
+			if (ret != OK)
+				break;
 
-		/* sort out the source of the values */
-		if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
-			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
-		} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
-			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
-		} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
-			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
-		} else {
-			rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
-		}
+			/* if no R/C input, don't try to fetch anything */
+			if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+				ret = -ENOTCONN;
+				break;
+			}
 
-		/* read raw R/C input values */
-		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input);
-		break;
-	}
+			/* sort out the source of the values */
+			if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+
+			} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+
+			} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+
+			} else {
+				rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
+			}
+
+			/* read raw R/C input values */
+			ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input);
+			break;
+		}
 
 	case PX4IO_SET_DEBUG:
 		/* set the debug level */
@@ -1650,12 +2147,15 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	case PX4IO_INAIR_RESTART_ENABLE:
+
 		/* set/clear the 'in-air restart' bit */
 		if (arg) {
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+
 		} else {
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
 		}
+
 		break;
 
 	default:
@@ -1674,11 +2174,14 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 
 	if (count > _max_actuators)
 		count = _max_actuators;
+
 	if (count > 0) {
 		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+
 		if (ret != OK)
 			return ret;
 	}
+
 	return count * 2;
 }
 
@@ -1686,8 +2189,9 @@ int
 PX4IO::set_update_rate(int rate)
 {
 	int interval_ms = 1000 / rate;
-	if (interval_ms < 5) {
-		interval_ms = 5;
+
+	if (interval_ms < UPDATE_INTERVAL_MIN) {
+		interval_ms = UPDATE_INTERVAL_MIN;
 		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
 	}
 
@@ -1712,23 +2216,75 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 namespace
 {
 
+device::Device *
+get_interface()
+{
+	device::Device *interface = nullptr;
+
+#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	/* try for a serial interface */
+	if (PX4IO_serial_interface != nullptr)
+		interface = PX4IO_serial_interface();
+
+	if (interface != nullptr)
+		goto got;
+
+#endif
+
+	/* try for an I2C interface if we haven't got a serial one */
+	if (PX4IO_i2c_interface != nullptr)
+		interface = PX4IO_i2c_interface();
+
+	if (interface != nullptr)
+		goto got;
+
+	errx(1, "cannot alloc interface");
+
+got:
+
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
+
+	return interface;
+}
+
 void
 start(int argc, char *argv[])
 {
 	if (g_dev != nullptr)
 		errx(1, "already loaded");
 
+	/* allocate the interface */
+	device::Device *interface = get_interface();
+
 	/* create the driver - it will set g_dev */
-	(void)new PX4IO();
+	(void)new PX4IO(interface);
 
 	if (g_dev == nullptr)
 		errx(1, "driver alloc failed");
 
 	if (OK != g_dev->init()) {
 		delete g_dev;
+		g_dev = nullptr;
 		errx(1, "driver init failed");
 	}
 
+	/* disable RC handling on request */
+	if (argc > 1) {
+		if (!strcmp(argv[1], "norc")) {
+
+			if (g_dev->disable_rc_handling())
+				warnx("Failed disabling RC handling");
+
+		} else {
+			warnx("unknown argument: %s", argv[1]);
+		}
+	}
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	int dsm_vcc_ctl;
 
 	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -1737,10 +2293,41 @@ start(int argc, char *argv[])
 			g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
 		}
 	}
+
+#endif
 	exit(0);
 }
 
 void
+detect(int argc, char *argv[])
+{
+	if (g_dev != nullptr)
+		errx(0, "already loaded");
+
+	/* allocate the interface */
+	device::Device *interface = get_interface();
+
+	/* create the driver - it will set g_dev */
+	(void)new PX4IO(interface);
+
+	if (g_dev == nullptr)
+		errx(1, "driver alloc failed");
+
+	int ret = g_dev->detect();
+
+	delete g_dev;
+	g_dev = nullptr;
+
+	if (ret) {
+		/* nonzero, error */
+		exit(1);
+
+	} else {
+		exit(0);
+	}
+}
+
+void
 bind(int argc, char *argv[])
 {
 	int pulses;
@@ -1748,21 +2335,33 @@ bind(int argc, char *argv[])
 	if (g_dev == nullptr)
 		errx(1, "px4io must be started first");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 	if (!g_dev->get_dsm_vcc_ctl())
 		errx(1, "DSM bind feature not enabled");
 
+#endif
+
 	if (argc < 3)
 		errx(0, "needs argument, use dsm2 or dsmx");
 
 	if (!strcmp(argv[2], "dsm2"))
-		pulses = 3;
+		pulses = DSM2_BIND_PULSES;
 	else if (!strcmp(argv[2], "dsmx"))
-		pulses = 7;
+		pulses = DSMX_BIND_PULSES;
+	else if (!strcmp(argv[2], "dsmx8"))
+		pulses = DSMX8_BIND_PULSES;
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+	// Test for custom pulse parameter
+	if (argc > 3)
+		pulses = atoi(argv[3]);
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+		errx(1, "system must not be armed");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-
+#endif
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
 	exit(0);
@@ -1786,13 +2385,18 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
 		err(1, "failed to get servo count");
 
+	/* tell IO that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
 
 	warnx("Press CTRL-C or 'c' to abort.");
 
@@ -1800,22 +2404,27 @@ test(void)
 
 		/* sweep all servos between 1000..2000 */
 		servo_position_t servos[servo_count];
+
 		for (unsigned i = 0; i < servo_count; i++)
 			servos[i] = pwm_value;
 
 		ret = write(fd, servos, sizeof(servos));
+
 		if (ret != (int)sizeof(servos))
 			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
 
 		if (direction > 0) {
 			if (pwm_value < 2000) {
 				pwm_value++;
+
 			} else {
 				direction = -1;
 			}
+
 		} else {
 			if (pwm_value > 1000) {
 				pwm_value--;
+
 			} else {
 				direction = 1;
 			}
@@ -1827,16 +2436,21 @@ test(void)
 
 			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
 				err(1, "error reading PWM servo %d", i);
+
 			if (value != servos[i])
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
 		}
 
 		/* Check if user wants to quit */
 		char c;
-		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
+		ret = poll(&fds, 1, 0);
+
+		if (ret > 0) {
+
+			read(0, &c, 1);
+
+			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
-				close(console);
 				exit(0);
 			}
 		}
@@ -1846,33 +2460,54 @@ test(void)
 void
 monitor(void)
 {
+	/* clear screen */
+	printf("\033[2J");
+
 	unsigned cancels = 3;
-	printf("Hit <enter> three times to exit monitor mode\n");
 
 	for (;;) {
 		pollfd fds[1];
 
 		fds[0].fd = 0;
 		fds[0].events = POLLIN;
-		poll(fds, 1, 500);
+		poll(fds, 1, 2000);
 
 		if (fds[0].revents == POLLIN) {
 			int c;
 			read(0, &c, 1);
 
-			if (cancels-- == 0)
+			if (cancels-- == 0) {
+				printf("\033[H"); /* move cursor home and clear screen */
 				exit(0);
+			}
 		}
 
-#warning implement this
+		if (g_dev != nullptr) {
+
+			printf("\033[H"); /* move cursor home and clear screen */
+			(void)g_dev->print_status();
+			(void)g_dev->print_debug();
+			printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
-//		if (g_dev != nullptr)
-//			g_dev->dump_one = true;
+		} else {
+			errx(1, "driver not loaded, exiting");
+		}
 	}
 }
 
+void
+if_test(unsigned mode)
+{
+	device::Device *interface = get_interface();
+
+	int result = interface->ioctl(1, mode); /* XXX magic numbers */
+	delete interface;
+
+	errx(0, "test returned %d", result);
 }
 
+} /* namespace */
+
 int
 px4io_main(int argc, char *argv[])
 {
@@ -1883,102 +2518,131 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start"))
 		start(argc - 1, argv + 1);
 
-	if (!strcmp(argv[1], "limit")) {
+	if (!strcmp(argv[1], "detect"))
+		detect(argc - 1, argv + 1);
+
+	if (!strcmp(argv[1], "update")) {
 
 		if (g_dev != nullptr) {
+			printf("[px4io] loaded, detaching first\n");
+			/* stop the driver */
+			delete g_dev;
+		}
 
-			if ((argc > 2)) {
-				g_dev->set_update_rate(atoi(argv[2]));
-			} else {
-				errx(1, "missing argument (50 - 200 Hz)");
-				return 1;
-			}
+		PX4IO_Uploader *up;
+		const char *fn[3];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+
+		} else {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+			fn[0] = "/etc/extras/px4io-v1_default.bin";
+			fn[1] =	"/fs/microsd/px4io1.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	nullptr;
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+			fn[0] = "/etc/extras/px4io-v2_default.bin";
+			fn[1] =	"/fs/microsd/px4io2.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	nullptr;
+#else
+#error "unknown board"
+#endif
 		}
-		exit(0);
-	}
 
-	if (!strcmp(argv[1], "current")) {
-		if (g_dev != nullptr) {
-			if ((argc > 3)) {
-				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
-			} else {
-				errx(1, "missing argument (apm_per_volt, amp_offset)");
-				return 1;
-			}
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+		default:
+			errx(1, "unexpected error %d", ret);
 		}
-		exit(0);
+
+		return ret;
 	}
 
-	if (!strcmp(argv[1], "failsafe")) {
+	if (!strcmp(argv[1], "iftest")) {
+		if (g_dev != nullptr)
+			errx(1, "can't iftest when started");
 
-		if (argc < 3) {
-			errx(1, "failsafe command needs at least one channel value (ppm)");
-		}
+		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
+	}
 
-		if (g_dev != nullptr) {
+	/* commands below here require a started driver */
 
-			/* set values for first 8 channels, fill unassigned channels with 1500. */
-			uint16_t failsafe[8];
-
-			for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
-			{
-				/* set channel to commanline argument or to 900 for non-provided channels */
-				if (argc > i + 2) {
-					failsafe[i] = atoi(argv[i+2]);
-					if (failsafe[i] < 800 || failsafe[i] > 2200) {
-						errx(1, "value out of range of 800 < value < 2200. Aborting.");
-					}
-				} else {
-					/* a zero value will result in stopping to output any pulse */
-					failsafe[i] = 0;
-				}
-			}
+	if (g_dev == nullptr)
+		errx(1, "not started");
 
-			int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+	if (!strcmp(argv[1], "limit")) {
+
+		if ((argc > 2)) {
+			g_dev->set_update_rate(atoi(argv[2]));
 
-			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
 		} else {
-			errx(1, "not loaded");
+			errx(1, "missing argument (50 - 500 Hz)");
+			return 1;
 		}
+
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "recovery")) {
+	if (!strcmp(argv[1], "current")) {
+		if ((argc > 3)) {
+			g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
 
-		if (g_dev != nullptr) {
-			/*
-			 * Enable in-air restart support.
-			 * We can cheat and call the driver directly, as it
-		 	 * doesn't reference filp in ioctl()
-			 */
-			g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
 		} else {
-			errx(1, "not loaded");
+			errx(1, "missing argument (apm_per_volt, amp_offset)");
+			return 1;
 		}
+
+		exit(0);
+	}
+
+
+
+	if (!strcmp(argv[1], "recovery")) {
+
+		/*
+		 * Enable in-air restart support.
+		 * We can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 
-		if (g_dev != nullptr) {
-			/* stop the driver */
-			delete g_dev;
-		} else {
-			errx(1, "not loaded");
-		}
+		/* stop the driver */
+		delete g_dev;
 		exit(0);
 	}
 
 
 	if (!strcmp(argv[1], "status")) {
 
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded\n");
-			g_dev->print_status();
-		} else {
-			printf("[px4io] not loaded\n");
-		}
+		printf("[px4io] loaded\n");
+		g_dev->print_status();
 
 		exit(0);
 	}
@@ -1988,75 +2652,27 @@ px4io_main(int argc, char *argv[])
 			printf("usage: px4io debug LEVEL\n");
 			exit(1);
 		}
+
 		if (g_dev == nullptr) {
 			printf("px4io is not started\n");
 			exit(1);
 		}
+
 		uint8_t level = atoi(argv[2]);
 		/* we can cheat and call the driver directly, as it
 		 * doesn't reference filp in ioctl()
 		 */
 		int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level);
+
 		if (ret != 0) {
 			printf("SET_DEBUG failed - %d\n", ret);
 			exit(1);
 		}
+
 		printf("SET_DEBUG %u OK\n", (unsigned)level);
 		exit(0);
 	}
 
-	/* note, stop not currently implemented */
-
-	if (!strcmp(argv[1], "update")) {
-
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded, detaching first\n");
-			/* stop the driver */
-			delete g_dev;
-		}
-
-		PX4IO_Uploader *up;
-		const char *fn[3];
-
-		/* work out what we're uploading... */
-		if (argc > 2) {
-			fn[0] = argv[2];
-			fn[1] = nullptr;
-
-		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
-			fn[1] =	"/etc/px4io.bin";
-			fn[2] =	nullptr;
-		}
-
-		up = new PX4IO_Uploader;
-		int ret = up->upload(&fn[0]);
-		delete up;
-
-		switch (ret) {
-		case OK:
-			break;
-
-		case -ENOENT:
-			errx(1, "PX4IO firmware file not found");
-
-		case -EEXIST:
-		case -EIO:
-			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
-
-		case -EINVAL:
-			errx(1, "verify failed - retry the update");
-
-		case -ETIMEDOUT:
-			errx(1, "timed out waiting for bootloader - power-cycle and try again");
-
-		default:
-			errx(1, "unexpected error %d", ret);
-		}
-
-		return ret;
-	}
-
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||
@@ -2073,6 +2689,6 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "bind"))
 		bind(argc, argv);
 
-	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
+out:
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
 }
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
new file mode 100644
index 000000000..19776c40a
--- /dev/null
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file px4io_i2c.cpp
+  *
+  * I2C interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+
+#ifdef PX4_I2C_OBDEV_PX4IO
+
+device::Device	*PX4IO_i2c_interface();
+
+class PX4IO_I2C : public device::I2C
+{
+public:
+	PX4IO_I2C(int bus, uint8_t address);
+	virtual ~PX4IO_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count = 1);
+	virtual int	write(unsigned address, void *data, unsigned count = 1);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+
+};
+
+device::Device
+*PX4IO_i2c_interface()
+{
+	return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+}
+
+PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
+	I2C("PX4IO_i2c", nullptr, bus, address, 320000)
+{
+	_retries = 3;
+}
+
+PX4IO_I2C::~PX4IO_I2C()
+{
+}
+
+int
+PX4IO_I2C::init()
+{
+	int ret;
+
+	ret = I2C::init();
+	if (ret != OK)
+		goto out;
+
+	/* XXX really should do something more here */
+
+out:
+	return 0;
+}
+
+int
+PX4IO_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	return 0;
+}
+
+int
+PX4IO_I2C::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	/* set up the transfer */
+	uint8_t 	addr[2] = {
+		page,
+		offset
+	};
+
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+
+	msgv[1].flags = I2C_M_NORESTART;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = 2 * count;
+
+	int ret = transfer(msgv, 2);
+	if (ret == OK)
+		ret = count;
+	return ret;
+}
+
+int
+PX4IO_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	/* set up the transfer */
+	uint8_t		addr[2] = {
+		page,
+		offset
+	};
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+
+	msgv[1].flags = I2C_M_READ;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = 2 * count;
+
+	int ret = transfer(msgv, 2);
+	if (ret == OK)
+		ret = count;
+	return ret;
+}
+
+#endif /* PX4_I2C_OBDEV_PX4IO */
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
new file mode 100644
index 000000000..43318ca84
--- /dev/null
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -0,0 +1,679 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file px4io_serial.cpp
+  *
+  * Serial interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <nuttx/arch.h>
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <debug.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <modules/px4iofirmware/protocol.h>
+
+#ifdef PX4IO_SERIAL_BASE
+
+device::Device	*PX4IO_serial_interface();
+
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
+class PX4IO_serial : public device::Device
+{
+public:
+	PX4IO_serial();
+	virtual ~PX4IO_serial();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count = 1);
+	virtual int	write(unsigned address, void *data, unsigned count = 1);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+	/*
+	 * XXX tune this value
+	 *
+	 * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet.
+	 * Packet overhead is 26µs for the four-byte header.
+	 *
+	 * 32 registers = 451µs
+	 *
+	 * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common)
+	 * transfers? Could cause issues with any regs expecting to be written atomically...
+	 */
+	static IOPacket		_dma_buffer;		// XXX static to ensure DMA-able memory
+
+	DMA_HANDLE		_tx_dma;
+	DMA_HANDLE		_rx_dma;
+
+	/** saved DMA status */
+	static const unsigned	_dma_status_inactive = 0x80000000;	// low bits overlap DMA_STATUS_* values
+	static const unsigned	_dma_status_waiting  = 0x00000000;
+	volatile unsigned	_rx_dma_status;
+
+	/** bus-ownership lock */
+	sem_t			_bus_semaphore;
+
+	/** client-waiting lock/signal */
+	sem_t			_completion_semaphore;
+
+	/**
+	 * Start the transaction with IO and wait for it to complete.
+	 */
+	int			_wait_complete();
+
+	/**
+	 * DMA completion handler.
+	 */
+	static void		_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+	void			_do_rx_dma_callback(unsigned status);
+
+	/**
+	 * Serial interrupt handler.
+	 */
+	static int		_interrupt(int vector, void *context);
+	void			_do_interrupt();
+
+	/**
+	 * Cancel any DMA in progress with an error.
+	 */
+	void			_abort_dma();
+
+	/**
+	 * Performance counters.
+	 */
+	perf_counter_t		_pc_txns;
+	perf_counter_t		_pc_dmasetup;
+	perf_counter_t		_pc_retries;
+	perf_counter_t		_pc_timeouts;
+	perf_counter_t		_pc_crcerrs;
+	perf_counter_t		_pc_dmaerrs;
+	perf_counter_t		_pc_protoerrs;
+	perf_counter_t		_pc_uerrs;
+	perf_counter_t		_pc_idle;
+	perf_counter_t		_pc_badidle;
+
+};
+
+IOPacket PX4IO_serial::_dma_buffer;
+static PX4IO_serial *g_interface;
+
+device::Device
+*PX4IO_serial_interface()
+{
+	return new PX4IO_serial();
+}
+
+PX4IO_serial::PX4IO_serial() :
+	Device("PX4IO_serial"),
+	_tx_dma(nullptr),
+	_rx_dma(nullptr),
+	_rx_dma_status(_dma_status_inactive),
+	_pc_txns(perf_alloc(PC_ELAPSED,		"io_txns     ")),
+	_pc_dmasetup(perf_alloc(PC_ELAPSED,	"io_dmasetup ")),
+	_pc_retries(perf_alloc(PC_COUNT,	"io_retries  ")),
+	_pc_timeouts(perf_alloc(PC_COUNT,	"io_timeouts ")),
+	_pc_crcerrs(perf_alloc(PC_COUNT,	"io_crcerrs  ")),
+	_pc_dmaerrs(perf_alloc(PC_COUNT,	"io_dmaerrs  ")),
+	_pc_protoerrs(perf_alloc(PC_COUNT,	"io_protoerrs")),
+	_pc_uerrs(perf_alloc(PC_COUNT,		"io_uarterrs ")),
+	_pc_idle(perf_alloc(PC_COUNT,		"io_idle     ")),
+	_pc_badidle(perf_alloc(PC_COUNT,	"io_badidle  "))
+{
+	g_interface = this;
+}
+
+PX4IO_serial::~PX4IO_serial()
+{
+	if (_tx_dma != nullptr) {
+		stm32_dmastop(_tx_dma);
+		stm32_dmafree(_tx_dma);
+	}
+	if (_rx_dma != nullptr) {
+		stm32_dmastop(_rx_dma);
+		stm32_dmafree(_rx_dma);
+	}
+
+	/* reset the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* detach our interrupt handler */
+	up_disable_irq(PX4IO_SERIAL_VECTOR);
+	irq_detach(PX4IO_SERIAL_VECTOR);
+
+	/* restore the GPIOs */
+	stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
+	stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
+
+	/* and kill our semaphores */
+	sem_destroy(&_completion_semaphore);
+	sem_destroy(&_bus_semaphore);
+
+	perf_free(_pc_txns);
+	perf_free(_pc_dmasetup);
+	perf_free(_pc_retries);
+	perf_free(_pc_timeouts);
+	perf_free(_pc_crcerrs);
+	perf_free(_pc_dmaerrs);
+	perf_free(_pc_protoerrs);
+	perf_free(_pc_uerrs);
+	perf_free(_pc_idle);
+	perf_free(_pc_badidle);
+
+	if (g_interface == this)
+		g_interface = nullptr;
+}
+
+int
+PX4IO_serial::init()
+{
+	/* allocate DMA */
+	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
+	_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
+	if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
+		return -1;
+
+	/* configure pins for serial use */
+	stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
+	stm32_configgpio(PX4IO_SERIAL_RX_GPIO);
+
+	/* reset & configure the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* eat any existing interrupt status */
+	(void)rSR;
+	(void)rDR;
+
+	/* configure line speed */
+	uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2);
+	uint32_t mantissa = usartdiv32 >> 5;
+	uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+	rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+	/* attach serial interrupt handler */
+	irq_attach(PX4IO_SERIAL_VECTOR, _interrupt);
+	up_enable_irq(PX4IO_SERIAL_VECTOR);
+
+	/* enable UART in DMA mode, enable error and line idle interrupts */
+	rCR3 = USART_CR3_EIE;
+
+	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+	/* create semaphores */
+	sem_init(&_completion_semaphore, 0, 0);
+	sem_init(&_bus_semaphore, 0, 1);
+
+
+	/* XXX this could try talking to IO */
+
+	return 0;
+}
+
+int
+PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
+{
+
+	switch (operation) {
+
+	case 1:		/* XXX magic number - test operation */
+		switch (arg) {
+		case 0:
+			lowsyslog("test 0\n");
+
+			/* kill DMA, this is a PIO test */
+			stm32_dmastop(_tx_dma);
+			stm32_dmastop(_rx_dma);
+			rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
+
+			for (;;) {
+				while (!(rSR & USART_SR_TXE))
+					;
+				rDR = 0x55;
+			}
+			return 0;
+
+		case 1:
+			{
+				unsigned fails = 0;
+				for (unsigned count = 0;; count++) {
+					uint16_t value = count & 0xffff;
+
+					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
+						fails++;
+						
+					if (count >= 5000) {
+						lowsyslog("==== test 1 : %u failures ====\n", fails);
+						perf_print_counter(_pc_txns);
+						perf_print_counter(_pc_dmasetup);
+						perf_print_counter(_pc_retries);
+						perf_print_counter(_pc_timeouts);
+						perf_print_counter(_pc_crcerrs);
+						perf_print_counter(_pc_dmaerrs);
+						perf_print_counter(_pc_protoerrs);
+						perf_print_counter(_pc_uerrs);
+						perf_print_counter(_pc_idle);
+						perf_print_counter(_pc_badidle);
+						count = 0;
+					}
+				}
+				return 0;
+			}
+		case 2:
+			lowsyslog("test 2\n");
+			return 0;
+		}
+	default:
+		break;
+	}
+
+	return -1;
+}
+
+int
+PX4IO_serial::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	if (count > PKT_MAX_REGS)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	int result;
+	for (unsigned retries = 0; retries < 3; retries++) {
+
+		_dma_buffer.count_code = count | PKT_CODE_WRITE;
+		_dma_buffer.page = page;
+		_dma_buffer.offset = offset;
+		memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
+		for (unsigned i = count; i < PKT_MAX_REGS; i++)
+			_dma_buffer.regs[i] = 0x55aa;
+
+		/* XXX implement check byte */
+
+		/* start the transaction and wait for it to complete */
+		result = _wait_complete();
+
+		/* successful transaction? */
+		if (result == OK) {
+
+			/* check result in packet */
+			if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+				/* IO didn't like it - no point retrying */
+				result = -EINVAL;
+				perf_count(_pc_protoerrs);
+			}
+
+			break;
+		}
+		perf_count(_pc_retries);
+	}
+
+	sem_post(&_bus_semaphore);
+
+	if (result == OK)
+		result = count;
+	return result;
+}
+
+int
+PX4IO_serial::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	uint16_t *values = reinterpret_cast<uint16_t *>(data);
+
+	if (count > PKT_MAX_REGS)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	int result;
+	for (unsigned retries = 0; retries < 3; retries++) {
+
+		_dma_buffer.count_code = count | PKT_CODE_READ;
+		_dma_buffer.page = page;
+		_dma_buffer.offset = offset;
+
+		/* start the transaction and wait for it to complete */
+		result = _wait_complete();
+
+		/* successful transaction? */
+		if (result == OK) {
+
+			/* check result in packet */
+			if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+				/* IO didn't like it - no point retrying */
+				result = -EINVAL;
+				perf_count(_pc_protoerrs);
+
+			/* compare the received count with the expected count */
+			} else if (PKT_COUNT(_dma_buffer) != count) {
+
+				/* IO returned the wrong number of registers - no point retrying */
+				result = -EIO;
+				perf_count(_pc_protoerrs);
+
+			/* successful read */				
+			} else {
+
+				/* copy back the result */
+				memcpy(values, &_dma_buffer.regs[0], (2 * count));
+			}
+
+			break;
+		}
+		perf_count(_pc_retries);
+	}
+
+	sem_post(&_bus_semaphore);
+
+	if (result == OK)
+		result = count;
+	return result;
+}
+
+int
+PX4IO_serial::_wait_complete()
+{
+	/* clear any lingering error status */
+	(void)rSR;
+	(void)rDR;
+
+	/* start RX DMA */
+	perf_begin(_pc_txns);
+	perf_begin(_pc_dmasetup);
+
+	/* DMA setup time ~3µs */
+	_rx_dma_status = _dma_status_waiting;
+
+	/*
+	 * Note that we enable circular buffer mode as a workaround for
+	 * there being no API to disable the DMA FIFO. We need direct mode
+	 * because otherwise when the line idle interrupt fires there
+	 * will be packet bytes still in the DMA FIFO, and we will assume
+	 * that the idle was spurious.
+	 *
+	 * XXX this should be fixed with a NuttX change.
+	 */
+	stm32_dmasetup(
+		_rx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		sizeof(_dma_buffer),
+		DMA_SCR_CIRC		|	/* XXX see note above */
+		DMA_SCR_DIR_P2M		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_rx_dma, _dma_callback, this, false);
+	rCR3 |= USART_CR3_DMAR;
+
+	/* start TX DMA - no callback if we also expect a reply */
+	/* DMA setup time ~3µs */
+	_dma_buffer.crc = 0;
+	_dma_buffer.crc = crc_packet(&_dma_buffer);
+	stm32_dmasetup(
+		_tx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		PKT_SIZE(_dma_buffer),
+		DMA_SCR_DIR_M2P		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+	//rCR1 &= ~USART_CR1_TE;
+	//rCR1 |= USART_CR1_TE;
+	rCR3 |= USART_CR3_DMAT;
+
+	perf_end(_pc_dmasetup);
+
+	/* compute the deadline for a 10ms timeout */
+	struct timespec abstime;
+	clock_gettime(CLOCK_REALTIME, &abstime);
+	abstime.tv_nsec += 10*1000*1000;
+	if (abstime.tv_nsec >= 1000*1000*1000) {
+		abstime.tv_sec++;
+		abstime.tv_nsec -= 1000*1000*1000;
+	}
+
+	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
+	int ret;
+	for (;;) {
+		ret = sem_timedwait(&_completion_semaphore, &abstime);
+
+		if (ret == OK) {
+			/* check for DMA errors */
+			if (_rx_dma_status & DMA_STATUS_TEIF) {
+				perf_count(_pc_dmaerrs);
+				ret = -EIO;
+				break;
+			}
+
+			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
+			uint8_t crc = _dma_buffer.crc;
+			_dma_buffer.crc = 0;
+			if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
+				perf_count(_pc_crcerrs);
+				ret = -EIO;
+				break;
+			}
+
+			/* successful txn (may still be reporting an error) */
+			break;
+		}
+
+		if (errno == ETIMEDOUT) {
+			/* something has broken - clear out any partial DMA state and reconfigure */
+			_abort_dma();
+			perf_count(_pc_timeouts);
+			perf_cancel(_pc_txns);		/* don't count this as a transaction */
+			break;
+		}
+
+		/* we might? see this for EINTR */
+		lowsyslog("unexpected ret %d/%d\n", ret, errno);
+	}
+
+	/* reset DMA status */
+	_rx_dma_status = _dma_status_inactive;
+
+	/* update counters */
+	perf_end(_pc_txns);
+
+	return ret;
+}
+
+void
+PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	if (arg != nullptr) {
+		PX4IO_serial *ps = reinterpret_cast<PX4IO_serial *>(arg);
+
+		ps->_do_rx_dma_callback(status);
+	}
+}
+
+void
+PX4IO_serial::_do_rx_dma_callback(unsigned status)
+{
+	/* on completion of a reply, wake the waiter */
+	if (_rx_dma_status == _dma_status_waiting) {
+
+		/* check for packet overrun - this will occur after DMA completes */
+		uint32_t sr = rSR;
+		if (sr & (USART_SR_ORE | USART_SR_RXNE)) {
+			(void)rDR;
+			status = DMA_STATUS_TEIF;
+		}
+
+		/* save RX status */
+		_rx_dma_status = status;
+
+		/* disable UART DMA */
+		rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+		/* complete now */
+		sem_post(&_completion_semaphore);
+	}
+}
+
+int
+PX4IO_serial::_interrupt(int irq, void *context)
+{
+	if (g_interface != nullptr)
+		g_interface->_do_interrupt();
+	return 0;
+}
+
+void
+PX4IO_serial::_do_interrupt()
+{
+	uint32_t sr = rSR;	/* get UART status register */
+	(void)rDR;		/* read DR to clear status */
+
+	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
+		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+		
+		/* 
+		 * If we are in the process of listening for something, these are all fatal;
+		 * abort the DMA with an error.
+		 */
+		if (_rx_dma_status == _dma_status_waiting) {
+			_abort_dma();
+
+			perf_count(_pc_uerrs);
+			/* complete DMA as though in error */
+			_do_rx_dma_callback(DMA_STATUS_TEIF);
+
+			return;
+		}
+
+		/* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */
+
+		/* don't attempt to handle IDLE if it's set - things went bad */
+		return;
+	}
+
+	if (sr & USART_SR_IDLE) {
+
+		/* if there is DMA reception going on, this is a short packet */
+		if (_rx_dma_status == _dma_status_waiting) {
+
+			/* verify that the received packet is complete */
+			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
+				perf_count(_pc_badidle);
+
+				/* stop the receive DMA */
+				stm32_dmastop(_rx_dma);
+
+				/* complete the short reception */
+				_do_rx_dma_callback(DMA_STATUS_TEIF);
+				return;
+			}
+
+			perf_count(_pc_idle);
+
+			/* stop the receive DMA */
+			stm32_dmastop(_rx_dma);
+
+			/* complete the short reception */
+			_do_rx_dma_callback(DMA_STATUS_TCIF);
+		}
+	}
+}
+
+void
+PX4IO_serial::_abort_dma()
+{
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+	(void)rSR;
+	(void)rDR;
+	(void)rDR;
+
+	/* stop DMA */
+	stm32_dmastop(_tx_dma);
+	stm32_dmastop(_rx_dma);
+}
+
+#endif /* PX4IO_SERIAL_BASE */
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 9e3f041e3..d01dedb0d 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -52,51 +52,14 @@
 #include <termios.h>
 #include <sys/stat.h>
 
+#include <crc32.h>
+
 #include "uploader.h"
 
-static const uint32_t crctab[] =
-{
-	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
-	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
-	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
-	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
-	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
-	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
-	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
-	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
-	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
-	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
-	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
-	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
-	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
-	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
-	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
-	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
-	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
-	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
-	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
-	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
-	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
-	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
-	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
-	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
-	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
-	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
-	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
-	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
-	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
-	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
-	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
-	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
-};
-
-static uint32_t
-crc32(const uint8_t *src, unsigned len, unsigned state)
-{
-	for (unsigned i = 0; i < len; i++)
-		state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
-	return state;
-}
+#include <board_config.h>
+
+// define for comms logging
+//#define UDEBUG
 
 PX4IO_Uploader::PX4IO_Uploader() :
 	_io_fd(-1),
@@ -115,13 +78,42 @@ PX4IO_Uploader::upload(const char *filenames[])
 	const char *filename = NULL;
 	size_t fw_size;
 
-	_io_fd = open("/dev/ttyS2", O_RDWR);
+#ifndef PX4IO_SERIAL_DEVICE
+#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
+#endif
+
+	/* allow an early abort and look for file first */
+	for (unsigned i = 0; filenames[i] != nullptr; i++) {
+		_fw_fd = open(filenames[i], O_RDONLY);
+
+		if (_fw_fd < 0) {
+			log("failed to open %s", filenames[i]);
+			continue;
+		}
+
+		log("using firmware from %s", filenames[i]);
+		filename = filenames[i];
+		break;
+	}
+
+	if (filename == NULL) {
+		log("no firmware found");
+		close(_io_fd);
+		_io_fd = -1;
+		return -ENOENT;
+	}
+
+	_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
 
 	if (_io_fd < 0) {
 		log("could not open interface");
 		return -errno;
 	}
 
+	/* save initial uart configuration to reset after the update */
+	struct termios t_original;
+	tcgetattr(_io_fd, &t_original);
+
 	/* adjust line speed to match bootloader */
 	struct termios t;
 	tcgetattr(_io_fd, &t);
@@ -134,34 +126,16 @@ PX4IO_Uploader::upload(const char *filenames[])
 	if (ret != OK) {
 		/* this is immediately fatal */
 		log("bootloader not responding");
+		tcsetattr(_io_fd, TCSANOW, &t_original);
 		close(_io_fd);
 		_io_fd = -1;
 		return -EIO;
 	}
 
-	for (unsigned i = 0; filenames[i] != nullptr; i++) {
-		_fw_fd = open(filenames[i], O_RDONLY);
-
-		if (_fw_fd < 0) {
-			log("failed to open %s", filenames[i]);
-			continue;
-		}
-
-		log("using firmware from %s", filenames[i]);
-		filename = filenames[i];
-		break;
-	}
-
-	if (filename == NULL) {
-		log("no firmware found");
-		close(_io_fd);
-		_io_fd = -1;
-		return -ENOENT;
-	}
-
 	struct stat st;
 	if (stat(filename, &st) != 0) {
 		log("Failed to stat %s - %d\n", filename, (int)errno);
+		tcsetattr(_io_fd, TCSANOW, &t_original);
 		close(_io_fd);
 		_io_fd = -1;
 		return -errno;
@@ -169,6 +143,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 	fw_size = st.st_size;
 
 	if (_fw_fd == -1) {
+		tcsetattr(_io_fd, TCSANOW, &t_original);
 		close(_io_fd);
 		_io_fd = -1;
 		return -ENOENT;
@@ -183,6 +158,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 			if (ret != OK) {
 				/* this is immediately fatal */
 				log("bootloader not responding");
+				tcsetattr(_io_fd, TCSANOW, &t_original);
 				close(_io_fd);
 				_io_fd = -1;
 				return -EIO;
@@ -196,6 +172,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 				log("found bootloader revision: %d", bl_rev);
 			} else {
 				log("found unsupported bootloader revision %d, exiting", bl_rev);
+				tcsetattr(_io_fd, TCSANOW, &t_original);
 				close(_io_fd);
 				_io_fd = -1;
 				return OK;
@@ -231,6 +208,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 		if (ret != OK) {
 			log("reboot failed");
+			tcsetattr(_io_fd, TCSANOW, &t_original);
+			close(_io_fd);
+			_io_fd = -1;
 			return ret;
 		}
 
@@ -240,6 +220,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 		break;
 	}
 
+	/* reset uart to previous/default baudrate */
+	tcsetattr(_io_fd, TCSANOW, &t_original);
+
 	close(_fw_fd);
 	close(_io_fd);
 	_io_fd = -1;
@@ -254,16 +237,20 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	fds[0].fd = _io_fd;
 	fds[0].events = POLLIN;
 
-	/* wait 100 ms for a character */
+	/* wait <timout> ms for a character */
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {
-		//log("poll timeout %d", ret);
+#ifdef UDEBUG
+		log("poll timeout %d", ret);
+#endif
 		return -ETIMEDOUT;
 	}
 
 	read(_io_fd, &c, 1);
-	//log("recv 0x%02x", c);
+#ifdef UDEBUG
+	log("recv 0x%02x", c);
+#endif
 	return OK;
 }
 
@@ -289,16 +276,20 @@ PX4IO_Uploader::drain()
 	do {
 		ret = recv(c, 1000);
 
+#ifdef UDEBUG
 		if (ret == OK) {
-			//log("discard 0x%02x", c);
+			log("discard 0x%02x", c);
 		}
+#endif
 	} while (ret == OK);
 }
 
 int
 PX4IO_Uploader::send(uint8_t c)
 {
-	//log("send 0x%02x", c);
+#ifdef UDEBUG
+	log("send 0x%02x", c);
+#endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
 
@@ -572,14 +563,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 			return -errno;
 
 		/* calculate crc32 sum */
-		sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+		sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
 
 		bytes_read += count;
 	}
 
 	/* fill the rest with 0xff */
 	while (bytes_read < fw_size_remote) {
-		sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+		sum = crc32part(&fill_blank, sizeof(fill_blank), sum);
 		bytes_read += sizeof(fill_blank);
 	}
 
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index 135202dd1..22387a3e2 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -69,7 +69,7 @@ private:
 		PROTO_REBOOT		= 0x30,
 
 		INFO_BL_REV		= 1,		/**< bootloader protocol revision */
-		BL_REV			= 3,		/**< supported bootloader protocol  */
+		BL_REV			= 4,		/**< supported bootloader protocol  */
 		INFO_BOARD_ID		= 2,		/**< board type */
 		INFO_BOARD_REV		= 3,		/**< board revision */
 		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
new file mode 100644
index 000000000..39b53ec9e
--- /dev/null
+++ b/src/drivers/rgbled/module.mk
@@ -0,0 +1,6 @@
+#
+# TCA62724FMG driver for RGB LED
+#
+
+MODULE_COMMAND	 = rgbled
+SRCS		 = rgbled.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
new file mode 100644
index 000000000..727c86e02
--- /dev/null
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -0,0 +1,707 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_rgbled.h>
+
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+#define ADDR			PX4_I2C_OBDEV_LED	/**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
+#define SETTING_ENABLE   	0x02	/**< on */
+
+
+class RGBLED : public device::I2C
+{
+public:
+	RGBLED(int bus, int rgbled);
+	virtual ~RGBLED();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+	work_s			_work;
+
+	rgbled_mode_t		_mode;
+	rgbled_pattern_t	_pattern;
+
+	uint8_t			_r;
+	uint8_t			_g;
+	uint8_t			_b;
+	float			_brightness;
+
+	bool			_running;
+	int			_led_interval;
+	bool			_should_run;
+	int			_counter;
+
+	void 			set_color(rgbled_color_t ledcolor);
+	void			set_mode(rgbled_mode_t mode);
+	void			set_pattern(rgbled_pattern_t *pattern);
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			send_led_enable(bool enable);
+	int			send_led_rgb();
+	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+RGBLED *g_rgbled;
+}
+
+void rgbled_usage();
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
+	_mode(RGBLED_MODE_OFF),
+	_r(0),
+	_g(0),
+	_b(0),
+	_brightness(1.0f),
+	_running(false),
+	_led_interval(0),
+	_should_run(false),
+	_counter(0)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(&_pattern, 0, sizeof(_pattern));
+}
+
+RGBLED::~RGBLED()
+{
+}
+
+int
+RGBLED::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
+
+	return OK;
+}
+
+int
+RGBLED::probe()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	/**
+	   this may look strange, but is needed. There is a serial
+	   EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
+	   a bunch of I2C addresses, including the 0x55 used by this
+	   LED device. So we need to do enough operations to be sure
+	   we are talking to the right device. These 3 operations seem
+	   to be enough, as the 3rd one consistently fails if no
+	   RGBLED is on the bus.
+	 */
+	if ((ret=get(on, powersave, r, g, b)) != OK ||
+	    (ret=send_led_enable(false) != OK) ||
+	    (ret=send_led_enable(false) != OK)) {
+		return ret;
+	}
+
+	return ret;
+}
+
+int
+RGBLED::info()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		log("state: %s", on ? "ON" : "OFF");
+		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
+	} else {
+		warnx("failed to read led");
+	}
+
+	return ret;
+}
+
+int
+RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case RGBLED_SET_RGB:
+		/* set the specified color */
+		_r = ((rgbled_rgbset_t *) arg)->red;
+		_g = ((rgbled_rgbset_t *) arg)->green;
+		_b = ((rgbled_rgbset_t *) arg)->blue;
+		send_led_rgb();
+		return OK;
+
+	case RGBLED_SET_COLOR:
+		/* set the specified color name */
+		set_color((rgbled_color_t)arg);
+		send_led_rgb();
+		return OK;
+
+	case RGBLED_SET_MODE:
+		/* set the specified mode */
+		set_mode((rgbled_mode_t)arg);
+		return OK;
+
+	case RGBLED_SET_PATTERN:
+		/* set a special pattern */
+		set_pattern((rgbled_pattern_t *)arg);
+		return OK;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+	RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+RGBLED::led()
+{
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
+	switch (_mode) {
+	case RGBLED_MODE_BLINK_SLOW:
+	case RGBLED_MODE_BLINK_NORMAL:
+	case RGBLED_MODE_BLINK_FAST:
+		if (_counter >= 2)
+			_counter = 0;
+
+		send_led_enable(_counter == 0);
+
+		break;
+
+	case RGBLED_MODE_BREATHE:
+
+		if (_counter >= 62)
+			_counter = 0;
+
+		int n;
+
+		if (_counter < 32) {
+			n = _counter;
+
+		} else {
+			n = 62 - _counter;
+		}
+
+		_brightness = n * n / (31.0f * 31.0f);
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_PATTERN:
+
+		/* don't run out of the pattern array and stop if the next frame is 0 */
+		if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+			_counter = 0;
+
+		set_color(_pattern.color[_counter]);
+		send_led_rgb();
+		_led_interval = _pattern.duration[_counter];
+		break;
+
+	default:
+		break;
+	}
+
+	_counter++;
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
+}
+
+/**
+ * Parse color constant and set _r _g _b values
+ */
+void
+RGBLED::set_color(rgbled_color_t color)
+{
+	switch (color) {
+	case RGBLED_COLOR_OFF:
+		_r = 0;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_RED:
+		_r = 255;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_YELLOW:
+		_r = 255;
+		_g = 200;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_PURPLE:
+		_r = 255;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_GREEN:
+		_r = 0;
+		_g = 255;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_WHITE:
+		_r = 255;
+		_g = 255;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_AMBER:
+		_r = 255;
+		_g = 80;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_RED:
+		_r = 90;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_YELLOW:
+		_r = 80;
+		_g = 30;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_PURPLE:
+		_r = 45;
+		_g = 0;
+		_b = 45;
+		break;
+
+	case RGBLED_COLOR_DIM_GREEN:
+		_r = 0;
+		_g = 90;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 90;
+		break;
+
+	case RGBLED_COLOR_DIM_WHITE:
+		_r = 30;
+		_g = 30;
+		_b = 30;
+		break;
+
+	case RGBLED_COLOR_DIM_AMBER:
+		_r = 80;
+		_g = 20;
+		_b = 0;
+		break;
+
+	default:
+		warnx("color unknown");
+		break;
+	}
+}
+
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
+void
+RGBLED::set_mode(rgbled_mode_t mode)
+{
+	if (mode != _mode) {
+		_mode = mode;
+
+		switch (mode) {
+		case RGBLED_MODE_OFF:
+			_should_run = false;
+			send_led_enable(false);
+			break;
+
+		case RGBLED_MODE_ON:
+			_brightness = 1.0f;
+			send_led_rgb();
+			send_led_enable(true);
+			break;
+
+		case RGBLED_MODE_BLINK_SLOW:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 2000;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BLINK_NORMAL:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 500;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BLINK_FAST:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 100;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BREATHE:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 25;
+			send_led_enable(true);
+			break;
+
+		case RGBLED_MODE_PATTERN:
+			_should_run = true;
+			_counter = 0;
+			_brightness = 1.0f;
+			send_led_enable(true);
+			break;
+
+		default:
+			warnx("mode unknown");
+			break;
+		}
+
+		/* if it should run now, start the workq */
+		if (_should_run && !_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+		}
+
+	}
+}
+
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
+void
+RGBLED::set_pattern(rgbled_pattern_t *pattern)
+{
+	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
+}
+
+/**
+ * Sent ENABLE flag to LED driver
+ */
+int
+RGBLED::send_led_enable(bool enable)
+{
+	uint8_t settings_byte = 0;
+
+	if (enable)
+		settings_byte |= SETTING_ENABLE;
+
+	settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
+int
+RGBLED::send_led_rgb()
+{
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
+		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+	};
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	uint8_t result[2];
+	int ret;
+
+	ret = transfer(nullptr, 0, &result[0], 2);
+
+	if (ret == OK) {
+		on = result[0] & SETTING_ENABLE;
+		powersave = !(result[0] & SETTING_NOT_POWERSAVE);
+		/* XXX check, looks wrong */
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
+	}
+
+	return ret;
+}
+
+void
+rgbled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	warnx("    -a addr (0x%x)", ADDR);
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int rgbledadr = ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			rgbledadr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			rgbled_usage();
+			exit(0);
+		}
+	}
+
+        if (optind >= argc) {
+            rgbled_usage();
+            exit(1);
+        }
+
+	const char *verb = argv[optind];
+
+	int fd;
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr)
+			errx(1, "already started");
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
+			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+			}
+
+			if (g_rgbled == nullptr) {
+				// fall back to default bus
+				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+					errx(1, "init failed");
+				}
+				i2cdevice = PX4_I2C_BUS_LED;
+			}
+		}
+
+		if (g_rgbled == nullptr) {
+			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
+			if (g_rgbled == nullptr)
+				errx(1, "new failed");
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+		warnx("not started");
+		rgbled_usage();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "test")) {
+		fd = open(RGBLED_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
+			{500, 500, 500, 500, 1000, 0 }	// "0" indicates end of pattern
+		};
+
+		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
+
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_rgbled->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "off")) {
+		fd = open(RGBLED_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+		}
+
+		fd = open(RGBLED_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		rgbled_rgbset_t v;
+		v.red   = strtol(argv[2], NULL, 0);
+		v.green = strtol(argv[3], NULL, 0);
+		v.blue  = strtol(argv[4], NULL, 0);
+		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
+		close(fd);
+		exit(ret);
+	}
+
+	rgbled_usage();
+	exit(0);
+}
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
new file mode 100644
index 000000000..d65a9be36
--- /dev/null
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoboClaw.cpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#include "RoboClaw.hpp"
+#include <poll.h>
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <systemlib/err.h>
+#include <arch/board/board.h>
+#include <mavlink/mavlink_log.h>
+
+#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
+
+uint8_t RoboClaw::checksum_mask = 0x7f;
+
+RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
+		uint16_t pulsesPerRev):
+	_address(address),
+	_pulsesPerRev(pulsesPerRev),
+	_uart(0),
+	_controlPoll(),
+	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_motor1Position(0),
+	_motor1Speed(0),
+	_motor1Overflow(0),
+	_motor2Position(0),
+	_motor2Speed(0),
+	_motor2Overflow(0)
+{
+	// setup control polling
+	_controlPoll.fd = _actuators.getHandle();
+	_controlPoll.events = POLLIN;
+
+	// start serial port
+	_uart = open(deviceName, O_RDWR | O_NOCTTY);
+	if (_uart < 0) err(1, "could not open %s", deviceName);
+	int ret = 0;
+	struct termios uart_config;
+	ret = tcgetattr(_uart, &uart_config);
+	if (ret < 0) err (1, "failed to get attr");
+	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
+	ret = cfsetispeed(&uart_config, B38400);
+	if (ret < 0) err (1, "failed to set input speed");
+	ret = cfsetospeed(&uart_config, B38400);
+	if (ret < 0) err (1, "failed to set output speed");
+	ret = tcsetattr(_uart, TCSANOW, &uart_config);
+	if (ret < 0) err (1, "failed to set attr");
+
+	// setup default settings, reset encoders
+	resetEncoders();
+}
+
+RoboClaw::~RoboClaw()
+{
+	setMotorDutyCycle(MOTOR_1, 0.0);
+	setMotorDutyCycle(MOTOR_2, 0.0);
+	close(_uart);
+}
+
+int RoboClaw::readEncoder(e_motor motor)
+{
+	uint16_t sum = 0;
+	if (motor == MOTOR_1) {
+		_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
+	} else if (motor == MOTOR_2) {
+		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
+	}
+	uint8_t rbuf[50];
+	usleep(5000);
+	int nread = read(_uart, rbuf, 50);
+	if (nread < 6) { 
+		printf("failed to read\n");
+		return -1;
+	}
+	//printf("received: \n");
+	//for (int i=0;i<nread;i++) {
+		//printf("%d\t", rbuf[i]);
+	//}
+	//printf("\n");
+	uint32_t count = 0;
+	uint8_t * countBytes = (uint8_t *)(&count);
+	countBytes[3] = rbuf[0];
+	countBytes[2] = rbuf[1];
+	countBytes[1] = rbuf[2];
+	countBytes[0] = rbuf[3];
+	uint8_t status = rbuf[4];
+	uint8_t checksum = rbuf[5];
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+		checksum_mask;
+	// check if checksum is valid
+	if (checksum != checksum_computed) {
+		printf("checksum failed: expected %d got %d\n",
+				checksum, checksum_computed);
+		return -1;
+	}
+	int overFlow = 0;
+
+	if (status & STATUS_REVERSE) {
+		//printf("roboclaw: reverse\n");
+	}
+
+	if (status & STATUS_UNDERFLOW) {
+		//printf("roboclaw: underflow\n");
+		overFlow = -1;
+	} else if (status & STATUS_OVERFLOW) {
+		//printf("roboclaw: overflow\n");
+		overFlow = +1;
+	}
+
+	static int64_t overflowAmount = 0x100000000LL;
+	if (motor == MOTOR_1) {
+		_motor1Overflow += overFlow;
+		_motor1Position = float(int64_t(count) + 
+			_motor1Overflow*overflowAmount)/_pulsesPerRev;
+	} else if (motor == MOTOR_2) {
+		_motor2Overflow += overFlow;
+		_motor2Position = float(int64_t(count) + 
+			_motor2Overflow*overflowAmount)/_pulsesPerRev;
+	}
+	return 0;
+}
+
+void RoboClaw::printStatus(char *string, size_t n)
+{
+	snprintf(string, n,
+		 "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
+		 double(getMotorPosition(MOTOR_1)),
+		 double(getMotorSpeed(MOTOR_1)),
+		 double(getMotorPosition(MOTOR_2)),
+		 double(getMotorSpeed(MOTOR_2)));
+}
+
+float RoboClaw::getMotorPosition(e_motor motor)
+{
+	if (motor == MOTOR_1) {
+		return _motor1Position;
+	} else if (motor == MOTOR_2) {
+		return _motor2Position;
+	}
+}
+
+float RoboClaw::getMotorSpeed(e_motor motor)
+{
+	if (motor == MOTOR_1) {
+		return _motor1Speed;
+	} else if (motor == MOTOR_2) {
+		return _motor2Speed;
+	}
+}
+
+int RoboClaw::setMotorSpeed(e_motor motor, float value)
+{
+	uint16_t sum = 0;
+	// bound
+	if (value > 1) value = 1;
+	if (value < -1) value = -1;
+	uint8_t speed = fabs(value)*127;
+	// send command
+	if (motor == MOTOR_1) {
+		if (value > 0) {
+			return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
+		} else {
+			return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
+		}
+	} else if (motor == MOTOR_2) {
+		if (value > 0) {
+			return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
+		} else {
+			return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
+		}
+	}
+	return -1;
+}
+
+int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
+{
+	uint16_t sum = 0;
+	// bound
+	if (value > 1) value = 1;
+	if (value < -1) value = -1;
+	int16_t duty = 1500*value;
+	// send command
+	if (motor == MOTOR_1) {
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
+				(uint8_t *)(&duty), 2, sum);
+	} else if (motor == MOTOR_2) {
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
+				(uint8_t *)(&duty), 2, sum);
+	}
+	return -1;
+}
+
+int RoboClaw::resetEncoders() 
+{
+	uint16_t sum = 0;
+	return _sendCommand(CMD_RESET_ENCODERS,
+			nullptr, 0, sum);
+}
+
+int RoboClaw::update()
+{
+	// wait for an actuator publication,
+	// check for exit condition every second
+	// note "::poll" is required to distinguish global
+	// poll from member function for driver
+	if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
+
+	// if new data, send to motors
+	if (_actuators.updated()) {
+		_actuators.update();
+		setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
+		setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
+	}
+	return 0;
+}
+
+uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
+{
+	uint16_t sum = 0;
+	//printf("sum\n");
+	for (size_t i=0;i<n;i++) {
+		sum += buf[i];
+		//printf("%d\t", buf[i]);
+	}
+	//printf("total sum %d\n", sum);
+	return sum;
+}
+
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+		size_t n_data, uint16_t & prev_sum)
+{
+	tcflush(_uart, TCIOFLUSH); // flush  buffers
+	uint8_t buf[n_data + 3];
+	buf[0] = _address;
+	buf[1] = cmd;
+	for (size_t i=0;i<n_data;i++) {
+		buf[i+2] = data[n_data - i - 1]; // MSB
+	}
+	uint16_t sum = _sumBytes(buf, n_data + 2);
+	prev_sum += sum;
+	buf[n_data + 2] = sum & checksum_mask;
+	//printf("\nmessage:\n");
+	//for (size_t i=0;i<n_data+3;i++) {
+		//printf("%d\t", buf[i]);
+	//}
+	//printf("\n");
+	return write(_uart, buf, n_data + 3);
+}
+
+int roboclawTest(const char *deviceName, uint8_t address, 
+		uint16_t pulsesPerRev)
+{
+	printf("roboclaw test: starting\n");
+
+	// setup
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
+	char buf[200];
+	for (int i=0; i<10; i++) {
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
+	}
+
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
+	for (int i=0; i<10; i++) {
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
+	}
+
+	printf("Test complete\n");
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
new file mode 100644
index 000000000..e9f35cf95
--- /dev/null
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoboClas.hpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <stdio.h>
+#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <drivers/device/i2c.h>
+
+/**
+ * This is a driver for the RoboClaw motor controller
+ */
+class RoboClaw
+{
+public:
+
+	/** control channels */
+	enum e_channel {
+		CH_VOLTAGE_LEFT = 0,
+		CH_VOLTAGE_RIGHT
+	};
+	
+	/**  motors */
+	enum e_motor {
+		MOTOR_1 = 0,
+		MOTOR_2
+	};
+
+	/**
+	 * constructor
+	 * @param deviceName the name of the 
+	 * 	serial port e.g. "/dev/ttyS2"
+	 * @param address the adddress  of the motor
+	 * 	(selectable on roboclaw)
+	 * @param pulsesPerRev # of encoder
+	 *  pulses per revolution of wheel
+	 */
+	RoboClaw(const char *deviceName, uint16_t address, 
+			uint16_t pulsesPerRev);
+
+	/**
+	 * deconstructor
+	 */
+	virtual ~RoboClaw();
+
+	/**
+	 * @return position of a motor, rev
+	 */
+	float getMotorPosition(e_motor motor);
+
+	/**
+	 * @return speed of a motor, rev/sec
+	 */
+	float getMotorSpeed(e_motor motor);
+
+	/**
+	 * set the speed of a motor, rev/sec
+	 */
+	int setMotorSpeed(e_motor motor, float value);
+
+	/**
+	 * set the duty cycle of a motor, rev/sec
+	 */
+	int setMotorDutyCycle(e_motor motor, float value);
+
+	/**
+	 * reset the encoders
+	 * @return status
+	 */
+	int resetEncoders();
+
+	/**
+	 * main update loop that updates RoboClaw motor
+	 * dutycycle based on actuator publication
+	 */
+	int update();
+
+	/**
+	 * read data from serial
+	 */
+	int readEncoder(e_motor motor);
+
+	/**
+	 * print status
+	 */
+	void printStatus(char *string, size_t n);
+
+private:
+
+	// Quadrature status flags
+	enum e_quadrature_status_flags {
+		STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/
+		STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/
+		STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/
+	};
+
+	// commands
+	// We just list the commands we want from the manual here.
+	enum e_command {
+
+		// simple
+		CMD_DRIVE_FWD_1 = 0,
+		CMD_DRIVE_REV_1 = 1,
+		CMD_DRIVE_FWD_2 = 4,
+		CMD_DRIVE_REV_2 = 5,
+
+		// encoder commands
+		CMD_READ_ENCODER_1 = 16,
+		CMD_READ_ENCODER_2 = 17,
+		CMD_RESET_ENCODERS = 20,
+
+		// advanced motor control
+		CMD_READ_SPEED_HIRES_1 = 30,
+		CMD_READ_SPEED_HIRES_2 = 31, 
+		CMD_SIGNED_DUTYCYCLE_1 = 32,
+		CMD_SIGNED_DUTYCYCLE_2 = 33,
+	};
+
+	static uint8_t checksum_mask;
+
+	uint16_t _address;
+	uint16_t _pulsesPerRev;
+
+	int _uart;
+
+	/** poll structure for control packets */
+	struct pollfd _controlPoll;
+
+	/** actuator controls subscription */
+	control::UOrbSubscription<actuator_controls_s> _actuators;
+
+	// private data
+	float _motor1Position;
+	float _motor1Speed;
+	int16_t _motor1Overflow;
+
+	float _motor2Position;
+	float _motor2Speed;
+	int16_t _motor2Overflow;
+
+	// private methods
+	uint16_t _sumBytes(uint8_t * buf, size_t n);
+	int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum);
+};
+
+// unit testing
+int roboclawTest(const char *deviceName, uint8_t address,
+		uint16_t pulsesPerRev);
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk
new file mode 100644
index 000000000..1abecf198
--- /dev/null
+++ b/src/drivers/roboclaw/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# RoboClaw Motor Controller
+#
+
+MODULE_COMMAND		= roboclaw
+
+SRCS			= roboclaw_main.cpp \
+			  RoboClaw.cpp
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
new file mode 100644
index 000000000..44ed03254
--- /dev/null
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+
+/**
+ * @ file roboclaw_main.cpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+
+#include <arch/board/board.h>
+#include "RoboClaw.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int roboclaw_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage();
+
+static void usage()
+{
+	fprintf(stderr, "usage: roboclaw "
+			"{start|stop|status|test}\n\n");
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int roboclaw_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("roboclaw already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd("roboclaw",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 2048,
+					 roboclaw_thread_main,
+					 (const char **)argv);
+		exit(0);
+
+	} else if (!strcmp(argv[1], "test")) {
+
+		const char * deviceName = "/dev/ttyS2";
+		uint8_t address = 128;
+		uint16_t pulsesPerRev = 1200;
+
+		if (argc == 2) {
+			printf("testing with default settings\n");
+		} else if (argc != 4) {
+			printf("usage: roboclaw test device address pulses_per_rev\n");
+			exit(-1);
+		} else {
+			deviceName = argv[2];
+			address = strtoul(argv[3], nullptr, 0);
+			pulsesPerRev = strtoul(argv[4], nullptr, 0);
+		}
+
+		printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
+
+		roboclawTest(deviceName, address, pulsesPerRev);
+		thread_should_exit = true;
+		exit(0);
+
+	} else if (!strcmp(argv[1], "stop")) {
+
+		thread_should_exit = true;
+		exit(0);
+
+	} else if (!strcmp(argv[1], "status")) {
+
+		if (thread_running) {
+			printf("\troboclaw app is running\n");
+
+		} else {
+			printf("\troboclaw app not started\n");
+		}
+		exit(0);
+	}
+
+	usage();
+	exit(1);
+}
+
+int roboclaw_thread_main(int argc, char *argv[])
+{
+	printf("[roboclaw] starting\n");
+
+	// skip parent process args
+	argc -=2;
+	argv +=2;
+
+	if (argc < 3) {
+		printf("usage: roboclaw start device address\n");
+		return -1;
+	}
+
+	const char *deviceName = argv[1];
+	uint8_t address = strtoul(argv[2], nullptr, 0);
+	uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
+
+	printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
+
+	// start
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+
+	thread_running = true;
+
+	// loop
+	while (!thread_should_exit) {
+		roboclaw.update();
+	}
+
+	// exit
+	printf("[roboclaw] exiting.\n");
+	thread_running = false;
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 1020eb946..00e46d6b8 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -227,7 +227,6 @@ ADC::init()
 		if ((hrt_absolute_time() - now) > 500) {
 			log("sample timeout");
 			return -1;
-			return 0xffff;
 		}
 	}
 
@@ -282,7 +281,7 @@ ADC::close_last(struct file *filp)
 void
 ADC::_tick_trampoline(void *arg)
 {
-	((ADC *)arg)->_tick();
+	(reinterpret_cast<ADC *>(arg))->_tick();
 }
 
 void
@@ -342,7 +341,7 @@ test(void)
 		err(1, "can't open ADC device");
 
 	for (unsigned i = 0; i < 50; i++) {
-		adc_msg_s data[8];
+		adc_msg_s data[10];
 		ssize_t count = read(fd, data, sizeof(data));
 
 		if (count < 0)
@@ -366,8 +365,15 @@ int
 adc_main(int argc, char *argv[])
 {
 	if (g_adc == nullptr) {
-		/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		/* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */
 		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		/* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */
+		g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | 
+			(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
+#endif
 
 		if (g_adc == nullptr)
 			errx(1, "couldn't allocate the ADC driver");
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 83a1a1abb..e79d7e10a 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -59,7 +59,7 @@
 #include <errno.h>
 #include <string.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include "chip.h"
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 2284be84d..f36f2091e 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -104,7 +104,7 @@
 #include <math.h>
 #include <ctype.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
@@ -230,13 +230,18 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
+	inline const char	*name(int tune) {
+		return _tune_names[tune];
+	}
 
 private:
-	static const unsigned	_tune_max = 1024; // be reasonable about user tunes
-	static const char	* const _default_tunes[];
-	static const unsigned	_default_ntunes;
+	static const unsigned	_tune_max = 1024 * 8; // be reasonable about user tunes
+	const char		* _default_tunes[TONE_NUMBER_OF_TUNES];
+	const char		* _tune_names[TONE_NUMBER_OF_TUNES];
 	static const uint8_t	_note_tab[];
 
+	unsigned		_default_tune_number; // number of currently playing default tune (0 for none)
+
 	const char		*_user_tune;
 
 	const char		*_tune;		// current tune string
@@ -302,160 +307,6 @@ private:
 
 };
 
-// predefined tune array
-const char * const ToneAlarm::_default_tunes[] = {
-	"MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc",		// startup tune
-	"MBT200a8a8a8PaaaP",					// ERROR tone
-	"MFT200e8a8a",						// NotifyPositive tone
-	"MFT200e8e",						// NotifyNeutral tone
-	"MFT200e8c8e8c8e8c8",					// NotifyNegative tone
-	"MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge!
-	"MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16",	// dixie
-	"MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8",		// cucuracha
-	"MNT150L8O2GGABGBADGGABL4GL8F+",			// yankee
-	"MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy
-	"T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8"	// william tell
-	"O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16"
-	"O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16"
-	"O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8"
-	"P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8"
-	"O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16"
-	"O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4"
-	"O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8"
-	"O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16"
-	"O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8"
-	"O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16"
-	"O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8"
-	"O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8"
-	"O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
-	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
-	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
-	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
-	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16"
-	"O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
-	"O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8"
-	"O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16"
-	"O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16"
-	"O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16"
-	"O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16"
-	"O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8"
-	"O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8"
-	"O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16"
-	"O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64"
-	"O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16"
-	"O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16"
-	"O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16"
-	"O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16"
-	"O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16"
-	"O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
-	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
-	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
-	"O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16"
-	"O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16"
-	"O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16"
-	"O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16"
-	"O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16"
-	"O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16"
-	"O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16"
-	"O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16"
-	"O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16"
-	"O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16"
-	"O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16"
-	"O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16"
-	"O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16"
-	"O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16"
-	"O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16"
-	"O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16"
-	"O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16"
-	"O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16"
-	"O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16"
-	"O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16"
-	"O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16"
-	"O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16"
-	"O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16"
-	"O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16"
-	"O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
-	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
-	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
-	"O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16"
-	"O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4."
-	"O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16"
-	"O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16"
-	"O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16"
-	"O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16"
-	"O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8"
-	"O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16"
-	"O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
-	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
-	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
-	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
-	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8"
-	"O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
-	"O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8"
-	"O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8"
-	"O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16"
-	"O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16"
-	"O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8"
-	"O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8"
-	"O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16"
-	"O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16"
-	"O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16"
-	"O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16"
-	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
-	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16"
-	"O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16"
-	"O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16"
-	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
-	"O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16"
-	"O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16"
-	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
-	"P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16"
-	"O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8"
-	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16"
-	"O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16"
-	"O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16"
-	"O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16"
-	"O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16"
-	"P16O4E16P16D+16P16C+16P16O3B16P16A+16P16"
-	"O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16"
-	"O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16"
-	"O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16"
-	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-	"O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16"
-	"O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16"
-	"O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
-	"P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16"
-	"O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16"
-	"O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16"
-	"O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16"
-	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8"
-	"O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16"
-	"O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16"
-	"O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8"
-	"O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16"
-	"O4E16P16D+16P16C+16P16O3B16P16A+16P16A16"
-	"P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16"
-	"O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8"
-	"O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8"
-	"O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16"
-	"O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16"
-	"O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16"
-	"O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16"
-	"O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16"
-	"O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8"
-	"O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16"
-	"O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16"
-	"O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8"
-	"O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8"
-	"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
-	"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
-	"O2E2P64",
-};
-
-const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
-
 // semitone offsets from C for the characters 'A'-'G'
 const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7};
 
@@ -466,13 +317,33 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
 
 
 ToneAlarm::ToneAlarm() :
-	CDev("tone_alarm", "/dev/tone_alarm"),
+	CDev("tone_alarm", TONEALARM_DEVICE_PATH),
+	_default_tune_number(0),
 	_user_tune(nullptr),
 	_tune(nullptr),
 	_next(nullptr)
 {
 	// enable debug() calls
 	//_debug_enabled = true;
+	_default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc";		// startup tune
+	_default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP";						// ERROR tone
+	_default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a";					// Notify Positive tone
+	_default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e";						// Notify Neutral tone
+	_default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8";				// Notify Negative tone
+	_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G";					//arming warning
+	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
+	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
+	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
+
+	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
+	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
+	_tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive";		// Notify Positive tone
+	_tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral";		// Notify Neutral tone
+	_tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative";		// Notify Negative tone
+	_tune_names[TONE_ARMING_WARNING_TUNE] = "arming";		// arming warning
+	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
+	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
+	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
 }
 
 ToneAlarm::~ToneAlarm()
@@ -799,8 +670,12 @@ tune_error:
 	// stop (and potentially restart) the tune
 tune_end:
 	stop_note();
-	if (_repeat)
+	if (_repeat) {
 		start_tune(_tune);
+	} else {
+		_tune = nullptr;
+		_default_tune_number = 0;
+	}
 	return;
 }
 
@@ -863,14 +738,20 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 	case TONE_SET_ALARM:
 		debug("TONE_SET_ALARM %u", arg);
 
-		if (arg <= _default_ntunes) {
-			if (arg == 0) {
+		if (arg < TONE_NUMBER_OF_TUNES) {
+			if (arg == TONE_STOP_TUNE) {
 				// stop the tune
 				_tune = nullptr;
 				_next = nullptr;
+				_repeat = false;
+				_default_tune_number = 0;
 			} else {
-				// play the selected tune
-				start_tune(_default_tunes[arg - 1]);
+				/* always interrupt alarms, unless they are repeating and already playing */
+				if (!(_repeat && _default_tune_number == arg)) {
+					/* play the selected tune */
+					_default_tune_number = arg;
+					start_tune(_default_tunes[arg]);
+				}
 			}
 		} else {
 			result = -EINVAL;
@@ -941,10 +822,10 @@ play_tune(unsigned tune)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", 0);
+	fd = open(TONEALARM_DEVICE_PATH, 0);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = ioctl(fd, TONE_SET_ALARM, tune);
 	close(fd);
@@ -956,18 +837,21 @@ play_tune(unsigned tune)
 }
 
 int
-play_string(const char *str)
+play_string(const char *str, bool free_buffer)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = write(fd, str, strlen(str) + 1);
 	close(fd);
 
+	if (free_buffer)
+		free((void *)str);
+
 	if (ret < 0)
 		err(1, "play tune");
 	exit(0);
@@ -993,22 +877,50 @@ tone_alarm_main(int argc, char *argv[])
 		}
 	}
 
-	if ((argc > 1) && !strcmp(argv[1], "start"))
-		play_tune(1);
-
-	if ((argc > 1) && !strcmp(argv[1], "stop"))
-		play_tune(0);
-
-	if ((tune = strtol(argv[1], nullptr, 10)) != 0)
-		play_tune(tune);
+	if (argc > 1) {
+		const char *argv1 = argv[1];
+
+		if (!strcmp(argv1, "start"))
+			play_tune(TONE_STARTUP_TUNE);
+
+		if (!strcmp(argv1, "stop"))
+			play_tune(TONE_STOP_TUNE);
+
+		if ((tune = strtol(argv1, nullptr, 10)) != 0)
+			play_tune(tune);
+
+		/* It might be a tune name */
+		for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++)
+			if (!strcmp(g_dev->name(tune), argv1))
+				play_tune(tune);
+
+		/* If it is a file name then load and play it as a string */
+		if (*argv1 == '/') {
+			FILE *fd = fopen(argv1, "r");
+			int sz;
+			char *buffer;
+			if (fd == nullptr)
+				errx(1, "couldn't open '%s'", argv1);
+			fseek(fd, 0, SEEK_END);
+			sz = ftell(fd);
+			fseek(fd, 0, SEEK_SET);
+			buffer = (char *)malloc(sz + 1);
+			if (buffer == nullptr)
+				errx(1, "not enough memory memory");
+			fread(buffer, sz, 1, fd);
+			/* terminate the string */
+			buffer[sz] = 0;
+			play_string(buffer, true);
+		}
 
-	/* if it looks like a PLAY string... */
-	if (strlen(argv[1]) > 2) {
-		const char *str = argv[1];
-		if (str[0] == 'M') {
-			play_string(str);
+		/* if it looks like a PLAY string... */
+		if (strlen(argv1) > 2) {
+			if (*argv1 == 'M') {
+				play_string(argv1, false);
+			}
 		}
+
 	}
 
-	errx(1, "unrecognised command, try 'start', 'stop' or an alarm number");
+	errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'");
 }