Moved the bulk of sensor drivers to the new world

22 files changed, 0 insertions, 9983 deletions

diff --git a/apps/drivers/blinkm/Makefile b/apps/drivers/blinkm/Makefile
deleted file mode 100644
index 5a623693d..000000000
--- a/apps/drivers/blinkm/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# BlinkM I2C LED driver
-#
-
-APPNAME		 = blinkm
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
deleted file mode 100644
index 3fabfd9a5..000000000
--- a/apps/drivers/blinkm/blinkm.cpp
+++ /dev/null
@@ -1,919 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file blinkm.cpp
- *
- * Driver for the BlinkM LED controller connected via I2C.
- *
- * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
- * blinkm start
- *
- * To start the system monitor put in the next line after the blinkm start:
- * blinkm systemmonitor
- *
- *
- * Description:
- * After startup, the Application checked how many lipo cells are connected to the System.
- * The recognized number off cells, will be blinked 5 times in purple color.
- * 2 Cells = 2 blinks
- * ...
- * 5 Cells = 5 blinks
- * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
- *
- * System disarmed:
- * The BlinkM should lit solid red.
- *
- * System armed:
- * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
- *
- * X-X-X-X-_-_-_-_-_-_-
- * -------------------------
- * G G G M
- * P P P O
- * S S S D
- *       E
- *
- * (X = on, _=off)
- *
- * The first 3 blinks indicates the status of the GPS-Signal (red):
- * 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
- *   5 satellites = X-X-_-X-_-_-_-_-_-_-
- *   6 satellites = X-_-_-X-_-_-_-_-_-_-
- * >=7 satellites = _-_-_-X-_-_-_-_-_-_-
- * If no GPS is found the first 3 blinks are white
- *
- * The fourth Blink indicates the Flightmode:
- * MANUAL     : amber
- * STABILIZED : yellow
- * HOLD		  : blue
- * AUTO       : green
- *
- * Battery Warning (low Battery Level):
- * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X
- *
- * Battery Alert (critical Battery Level)
- * Continuously blinking in red X-X-X-X-X-X-X-X-X-X
- *
- * General Error (no uOrb Data)
- * Continuously blinking in white X-X-X-X-X-X-X-X-X-X
- *
- */
-
-#include <nuttx/config.h>
-
-#include <arch/board/board.h>
-#include <drivers/device/i2c.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <string.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <ctype.h>
-
-#include <drivers/drv_blinkm.h>
-
-#include <nuttx/wqueue.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#include <systemlib/systemlib.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_gps_position.h>
-
-static const float MAX_CELL_VOLTAGE	= 4.3f;
-static const int LED_ONTIME = 120;
-static const int LED_OFFTIME = 120;
-static const int LED_BLINK = 1;
-static const int LED_NOBLINK = 0;
-
-class BlinkM : public device::I2C
-{
-public:
-	BlinkM(int bus, int blinkm);
-	virtual ~BlinkM();
-
-
-	virtual int		init();
-	virtual int		probe();
-	virtual int		setMode(int mode);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	static const char	*const script_names[];
-
-private:
-	enum ScriptID {
-		USER		= 0,
-		RGB,
-		WHITE_FLASH,
-		RED_FLASH,
-		GREEN_FLASH,
-		BLUE_FLASH,
-		CYAN_FLASH,
-		MAGENTA_FLASH,
-		YELLOW_FLASH,
-		BLACK,
-		HUE_CYCLE,
-		MOOD_LIGHT,
-		VIRTUAL_CANDLE,
-		WATER_REFLECTIONS,
-		OLD_NEON,
-		THE_SEASONS,
-		THUNDERSTORM,
-		STOP_LIGHT,
-		MORSE_CODE
-	};
-
-	enum ledColors {
-		LED_OFF,
-		LED_RED,
-		LED_YELLOW,
-		LED_PURPLE,
-		LED_GREEN,
-		LED_BLUE,
-		LED_WHITE,
-		LED_AMBER
-	};
-
-	work_s			_work;
-
-	int led_color_1;
-	int led_color_2;
-	int led_color_3;
-	int led_color_4;
-	int led_color_5;
-	int led_color_6;
-	int led_color_7;
-	int led_color_8;
-	int led_blink;
-
-	bool systemstate_run;
-
-	void 			setLEDColor(int ledcolor);
-	static void		led_trampoline(void *arg);
-	void			led();
-
-	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
-
-	int			fade_rgb(uint8_t r, uint8_t g, uint8_t b);
-	int			fade_hsb(uint8_t h, uint8_t s, uint8_t b);
-
-	int			fade_rgb_random(uint8_t r, uint8_t g, uint8_t b);
-	int			fade_hsb_random(uint8_t h, uint8_t s, uint8_t b);
-
-	int			set_fade_speed(uint8_t s);
-
-	int			play_script(uint8_t script_id);
-	int			play_script(const char *script_name);
-	int			stop_script();
-
-	int			write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3);
-	int			read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]);
-	int			set_script(uint8_t length, uint8_t repeats);
-
-	int			get_rgb(uint8_t &r, uint8_t &g, uint8_t &b);
-
-	int			get_firmware_version(uint8_t version[2]);
-};
-
-/* for now, we only support one BlinkM */
-namespace
-{
-	BlinkM *g_blinkm;
-}
-
-/* list of script names, must match script ID numbers */
-const char *const BlinkM::script_names[] = {
-	"USER",
-	"RGB",
-	"WHITE_FLASH",
-	"RED_FLASH",
-	"GREEN_FLASH",
-	"BLUE_FLASH",
-	"CYAN_FLASH",
-	"MAGENTA_FLASH",
-	"YELLOW_FLASH",
-	"BLACK",
-	"HUE_CYCLE",
-	"MOOD_LIGHT",
-	"VIRTUAL_CANDLE",
-	"WATER_REFLECTIONS",
-	"OLD_NEON",
-	"THE_SEASONS",
-	"THUNDERSTORM",
-	"STOP_LIGHT",
-	"MORSE_CODE",
-	nullptr
-};
-
-
-extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
-
-BlinkM::BlinkM(int bus, int blinkm) :
-	I2C("blinkm", BLINKM_DEVICE_PATH, bus, blinkm, 100000),
-	led_color_1(LED_OFF),
-	led_color_2(LED_OFF),
-	led_color_3(LED_OFF),
-	led_color_4(LED_OFF),
-	led_color_5(LED_OFF),
-	led_color_6(LED_OFF),
-	led_color_7(LED_OFF),
-	led_color_8(LED_OFF),
-	led_blink(LED_NOBLINK),
-	systemstate_run(false)
-{
-	memset(&_work, 0, sizeof(_work));
-}
-
-BlinkM::~BlinkM()
-{
-}
-
-int
-BlinkM::init()
-{
-	int ret;
-	ret = I2C::init();
-
-	if (ret != OK) {
-		warnx("I2C init failed");
-		return ret;
-	}
-
-	stop_script();
-	set_rgb(0,0,0);
-
-	return OK;
-}
-
-int
-BlinkM::setMode(int mode)
-{
-	if(mode == 1) {
-		if(systemstate_run == false) {
-			stop_script();
-			set_rgb(0,0,0);
-			systemstate_run = true;
-			work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
-		}
-	} else {
-		systemstate_run = false;
-	}
-
-	return OK;
-}
-
-int
-BlinkM::probe()
-{
-	uint8_t version[2];
-	int ret;
-
-	ret = get_firmware_version(version);
-
-	if (ret == OK)
-		log("found BlinkM firmware version %c%c", version[1], version[0]);
-
-	return ret;
-}
-
-int
-BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int ret = ENOTTY;
-
-	switch (cmd) {
-	case BLINKM_PLAY_SCRIPT_NAMED:
-		if (arg == 0) {
-			ret = EINVAL;
-			break;
-		}
-		ret = play_script((const char *)arg);
-		break;
-
-	case BLINKM_PLAY_SCRIPT:
-		ret = play_script(arg);
-		break;
-
-	case BLINKM_SET_USER_SCRIPT: {
-		if (arg == 0) {
-			ret = EINVAL;
-			break;
-		}
-
-		unsigned lines = 0;
-		const uint8_t *script = (const uint8_t *)arg;
-
-		while ((lines < 50) && (script[1] != 0)) {
-			ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
-			if (ret != OK)
-				break;
-			script += 5;
-		}
-		if (ret == OK)
-			ret = set_script(lines, 0);
-		break;
-	}
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
-
-void
-BlinkM::led_trampoline(void *arg)
-{
-	BlinkM *bm = (BlinkM *)arg;
-
-	bm->led();
-}
-
-
-
-void
-BlinkM::led()
-{
-
-	static int vehicle_status_sub_fd;
-	static int vehicle_gps_position_sub_fd;
-
-	static int num_of_cells = 0;
-	static int detected_cells_runcount = 0;
-
-	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
-	static int t_led_blink = 0;
-	static int led_thread_runcount=0;
-	static int led_interval = 1000;
-
-	static int no_data_vehicle_status = 0;
-	static int no_data_vehicle_gps_position = 0;
-
-	static bool topic_initialized = false;
-	static bool detected_cells_blinked = false;
-	static bool led_thread_ready = true;
-
-	int num_of_used_sats = 0;
-
-	if(!topic_initialized) {
-		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
-		orb_set_interval(vehicle_status_sub_fd, 1000);
-
-		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
-		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
-
-		topic_initialized = true;
-	}
-
-	if(led_thread_ready == true) {
-		if(!detected_cells_blinked) {
-			if(num_of_cells > 0) {
-				t_led_color[0] = LED_PURPLE;
-			}
-			if(num_of_cells > 1) {
-				t_led_color[1] = LED_PURPLE;
-			}
-			if(num_of_cells > 2) {
-				t_led_color[2] = LED_PURPLE;
-			}
-			if(num_of_cells > 3) {
-				t_led_color[3] = LED_PURPLE;
-			}
-			if(num_of_cells > 4) {
-				t_led_color[4] = LED_PURPLE;
-			}
-			t_led_color[5] = LED_OFF;
-			t_led_color[6] = LED_OFF;
-			t_led_color[7] = LED_OFF;
-			t_led_blink = LED_BLINK;
-		} else {
-			t_led_color[0] = led_color_1;
-			t_led_color[1] = led_color_2;
-			t_led_color[2] = led_color_3;
-			t_led_color[3] = led_color_4;
-			t_led_color[4] = led_color_5;
-			t_led_color[5] = led_color_6;
-			t_led_color[6] = led_color_7;
-			t_led_color[7] = led_color_8;
-			t_led_blink = led_blink;
-		}
-		led_thread_ready = false;
-	}
-
-	if (led_thread_runcount & 1) {
-		if (t_led_blink)
-			setLEDColor(LED_OFF);
-		led_interval = LED_OFFTIME;
-	} else {
-		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
-		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
-		led_interval = LED_ONTIME;
-	}
-
-	if (led_thread_runcount == 15) {
-		/* obtained data for the first file descriptor */
-		struct vehicle_status_s vehicle_status_raw;
-		struct vehicle_gps_position_s vehicle_gps_position_raw;
-
-		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
-		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
-
-		bool new_data_vehicle_status;
-		bool new_data_vehicle_gps_position;
-
-		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
-
-		if (new_data_vehicle_status) {
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
-			no_data_vehicle_status = 0;
-		} else {
-			no_data_vehicle_status++;
-			if(no_data_vehicle_status >= 3)
-				no_data_vehicle_status = 3;
-		}
-
-		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
-
-		if (new_data_vehicle_gps_position) {
-			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
-			no_data_vehicle_gps_position = 0;
-		} else {
-			no_data_vehicle_gps_position++;
-			if(no_data_vehicle_gps_position >= 3)
-				no_data_vehicle_gps_position = 3;
-		}
-
-
-
-		/* get number of used satellites in navigation */
-		num_of_used_sats = 0;
-		//for(int satloop=0; satloop<20; satloop++) {
-		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
-			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
-				num_of_used_sats++;
-			}
-		}
-
-		if(new_data_vehicle_status || no_data_vehicle_status < 3){
-			if(num_of_cells == 0) {
-				/* looking for lipo cells that are connected */
-				printf("<blinkm> checking cells\n");
-				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
-				}
-				printf("<blinkm> cells found:%d\n", num_of_cells);
-
-			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-					/* LED Pattern for battery low warning */
-					led_color_1 = LED_YELLOW;
-					led_color_2 = LED_YELLOW;
-					led_color_3 = LED_YELLOW;
-					led_color_4 = LED_YELLOW;
-					led_color_5 = LED_YELLOW;
-					led_color_6 = LED_YELLOW;
-					led_color_7 = LED_YELLOW;
-					led_color_8 = LED_YELLOW;
-					led_blink = LED_BLINK;
-
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-					/* LED Pattern for battery critical alerting */
-					led_color_1 = LED_RED;
-					led_color_2 = LED_RED;
-					led_color_3 = LED_RED;
-					led_color_4 = LED_RED;
-					led_color_5 = LED_RED;
-					led_color_6 = LED_RED;
-					led_color_7 = LED_RED;
-					led_color_8 = LED_RED;
-					led_blink = LED_BLINK;
-
-				} else {
-					/* no battery warnings here */
-
-					if(vehicle_status_raw.flag_system_armed == false) {
-						/* system not armed */
-						led_color_1 = LED_RED;
-						led_color_2 = LED_RED;
-						led_color_3 = LED_RED;
-						led_color_4 = LED_RED;
-						led_color_5 = LED_RED;
-						led_color_6 = LED_RED;
-						led_color_7 = LED_RED;
-						led_color_8 = LED_RED;
-						led_blink = LED_NOBLINK;
-
-					} else {
-						/* armed system - initial led pattern */
-						led_color_1 = LED_RED;
-						led_color_2 = LED_RED;
-						led_color_3 = LED_RED;
-						led_color_4 = LED_OFF;
-						led_color_5 = LED_OFF;
-						led_color_6 = LED_OFF;
-						led_color_7 = LED_OFF;
-						led_color_8 = LED_OFF;
-						led_blink = LED_BLINK;
-
-						/* handle 4th led - flightmode indicator */
-						switch((int)vehicle_status_raw.flight_mode) {
-							case VEHICLE_FLIGHT_MODE_MANUAL:
-								led_color_4 = LED_AMBER;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_STAB:
-								led_color_4 = LED_YELLOW;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_HOLD:
-								led_color_4 = LED_BLUE;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_AUTO:
-								led_color_4 = LED_GREEN;
-								break;
-						}
-
-						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used satīs */
-							if(num_of_used_sats >= 7) {
-								led_color_1 = LED_OFF;
-								led_color_2 = LED_OFF;
-								led_color_3 = LED_OFF;
-							} else if(num_of_used_sats == 6) {
-								led_color_2 = LED_OFF;
-								led_color_3 = LED_OFF;
-							} else if(num_of_used_sats == 5) {
-								led_color_3 = LED_OFF;
-							}
-
-						} else {
-							/* no vehicle_gps_position data */
-							led_color_1 = LED_WHITE;
-							led_color_2 = LED_WHITE;
-							led_color_3 = LED_WHITE;
-
-						}
-
-					}
-				}
-			}
-		} else {
-			/* LED Pattern for general Error - no vehicle_status can retrieved */
-			led_color_1 = LED_WHITE;
-			led_color_2 = LED_WHITE;
-			led_color_3 = LED_WHITE;
-			led_color_4 = LED_WHITE;
-			led_color_5 = LED_WHITE;
-			led_color_6 = LED_WHITE;
-			led_color_7 = LED_WHITE;
-			led_color_8 = LED_WHITE;
-			led_blink = LED_BLINK;
-
-		}
-
-		/*
-		printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
-		vehicle_status_raw.voltage_battery,
-		vehicle_status_raw.flag_system_armed,
-		vehicle_status_raw.flight_mode,
-		num_of_cells,
-		no_data_vehicle_status,
-		no_data_vehicle_gps_position,
-		num_of_used_sats,
-		vehicle_gps_position_raw.fix_type,
-		vehicle_gps_position_raw.satellites_visible);
-		*/
-
-		led_thread_runcount=0;
-		led_thread_ready = true;
-		led_interval = LED_OFFTIME;
-
-		if(detected_cells_runcount < 4){
-			detected_cells_runcount++;
-		} else {
-			detected_cells_blinked = true;
-		}
-
-	} else {
-		led_thread_runcount++;
-	}
-
-	if(systemstate_run == true) {
-		/* re-queue ourselves to run again later */
-		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
-	} else {
-		stop_script();
-		set_rgb(0,0,0);
-	}
-}
-
-void BlinkM::setLEDColor(int ledcolor) {
-	switch (ledcolor) {
-		case LED_OFF:	// off
-			set_rgb(0,0,0);
-			break;
-		case LED_RED:	// red
-			set_rgb(255,0,0);
-			break;
-		case LED_YELLOW:	// yellow
-			set_rgb(255,70,0);
-			break;
-		case LED_PURPLE:	// purple
-			set_rgb(255,0,255);
-			break;
-		case LED_GREEN:	// green
-			set_rgb(0,255,0);
-			break;
-		case LED_BLUE:	// blue
-			set_rgb(0,0,255);
-			break;
-		case LED_WHITE:	// white
-			set_rgb(255,255,255);
-			break;
-		case LED_AMBER:	// amber
-			set_rgb(255,20,0);
-			break;
-	}
-}
-
-int
-BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b)
-{
-	const uint8_t msg[4] = { 'n', r, g, b };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b)
-{
-	const uint8_t msg[4] = { 'c', r, g, b };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b)
-{
-	const uint8_t msg[4] = { 'h', h, s, b };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b)
-{
-	const uint8_t msg[4] = { 'C', r, g, b };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b)
-{
-	const uint8_t msg[4] = { 'H', h, s, b };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::set_fade_speed(uint8_t s)
-{
-	const uint8_t msg[2] = { 'f', s };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::play_script(uint8_t script_id)
-{
-	const uint8_t msg[4] = { 'p', script_id, 0, 0 };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::play_script(const char *script_name)
-{
-	/* handle HTML colour encoding */
-	if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) {
-		char code[3];
-		uint8_t r, g, b;
-
-		code[2] = '\0';
-
-		code[0] = script_name[1];
-		code[1] = script_name[2];
-		r = strtol(code, 0, 16);
-		code[0] = script_name[3];
-		code[1] = script_name[4];
-		g = strtol(code, 0, 16);
-		code[0] = script_name[5];
-		code[1] = script_name[6];
-		b = strtol(code, 0, 16);
-
-		stop_script();
-		return set_rgb(r, g, b);
-	}
-
-	for (unsigned i = 0; script_names[i] != nullptr; i++)
-		if (!strcasecmp(script_name, script_names[i]))
-			return play_script(i);
-
-	return -1;
-}
-
-int
-BlinkM::stop_script()
-{
-	const uint8_t msg[1] = { 'o' };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3)
-{
-	const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4])
-{
-	const uint8_t msg[3] = { 'R', 0, line };
-	uint8_t result[5];
-
-	int ret = transfer(msg, sizeof(msg), result, sizeof(result));
-
-	if (ret == OK) {
-		ticks = result[0];
-		cmd[0] = result[1];
-		cmd[1] = result[2];
-		cmd[2] = result[3];
-		cmd[3] = result[4];
-	}
-
-	return ret;
-}
-
-int
-BlinkM::set_script(uint8_t len, uint8_t repeats)
-{
-	const uint8_t msg[4] = { 'L', 0, len, repeats };
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
-int
-BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
-{
-	const uint8_t msg = 'g';
-	uint8_t result[3];
-
-	int ret = transfer(&msg, sizeof(msg), result, sizeof(result));
-
-	if (ret == OK) {
-		r = result[0];
-		g = result[1];
-		b = result[2];
-	}
-
-	return ret;
-}
-
-int
-BlinkM::get_firmware_version(uint8_t version[2])
-{
-	const uint8_t msg = 'Z';
-
-	return transfer(&msg, sizeof(msg), version, 2);
-}
-
-void blinkm_usage() {
-	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
-	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
-}
-
-int
-blinkm_main(int argc, char *argv[])
-{
-
-	int i2cdevice = PX4_I2C_BUS_EXPANSION;
-	int blinkmadr = 9;
-
-	int x;
-
-	for (x = 1; x < argc; x++) {
-		if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
-			if (argc > x + 1) {
-				i2cdevice = atoi(argv[x + 1]);
-			}
-		}
-
-		if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) {
-			if (argc > x + 1) {
-				blinkmadr = atoi(argv[x + 1]);
-			}
-		}
-
-	}
-
-	if (!strcmp(argv[1], "start")) {
-		if (g_blinkm != nullptr)
-			errx(1, "already started");
-
-		g_blinkm = new BlinkM(i2cdevice, blinkmadr);
-
-		if (g_blinkm == nullptr)
-			errx(1, "new failed");
-
-		if (OK != g_blinkm->init()) {
-			delete g_blinkm;
-			g_blinkm = nullptr;
-			errx(1, "init failed");
-		}
-
-		exit(0);
-	}
-
-
-	if (g_blinkm == nullptr) {
-		fprintf(stderr, "not started\n");
-		blinkm_usage();
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "systemstate")) {
-		g_blinkm->setMode(1);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "ledoff")) {
-		g_blinkm->setMode(0);
-		exit(0);
-	}
-
-
-	if (!strcmp(argv[1], "list")) {
-		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
-			fprintf(stderr, "    %s\n", BlinkM::script_names[i]);
-		fprintf(stderr, "    <html color number>\n");
-		exit(0);
-	}
-
-	/* things that require access to the device */
-	int fd = open(BLINKM_DEVICE_PATH, 0);
-	if (fd < 0)
-		err(1, "can't open BlinkM device");
-
-	g_blinkm->setMode(0);
-	if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
-		exit(0);
-
-	blinkm_usage();
-	exit(0);
-}
diff --git a/apps/drivers/bma180/Makefile b/apps/drivers/bma180/Makefile
deleted file mode 100644
index cc01b629e..000000000
--- a/apps/drivers/bma180/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Makefile to build the BMA180 driver.
-#
-
-APPNAME		 = bma180
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp
deleted file mode 100644
index 4409a8a9c..000000000
--- a/apps/drivers/bma180/bma180.cpp
+++ /dev/null
@@ -1,929 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file bma180.cpp
- * Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
- */
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
-
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
-#include <drivers/device/spi.h>
-#include <drivers/drv_accel.h>
-
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-#define DIR_READ			(1<<7)
-#define DIR_WRITE			(0<<7)
-
-#define ADDR_CHIP_ID			0x00
-#define CHIP_ID				0x03
-
-#define ADDR_ACC_X_LSB			0x02
-#define ADDR_ACC_Y_LSB			0x04
-#define ADDR_ACC_Z_LSB			0x06
-#define ADDR_TEMPERATURE		0x08
-
-#define ADDR_CTRL_REG0			0x0D
-#define REG0_WRITE_ENABLE		0x10
-
-#define ADDR_RESET			0x10
-#define SOFT_RESET				0xB6
-
-#define ADDR_BW_TCS			0x20
-#define BW_TCS_BW_MASK				(0xf<<4)
-#define BW_TCS_BW_10HZ				(0<<4)
-#define BW_TCS_BW_20HZ				(1<<4)
-#define BW_TCS_BW_40HZ				(2<<4)
-#define BW_TCS_BW_75HZ				(3<<4)
-#define BW_TCS_BW_150HZ				(4<<4)
-#define BW_TCS_BW_300HZ				(5<<4)
-#define BW_TCS_BW_600HZ				(6<<4)
-#define BW_TCS_BW_1200HZ			(7<<4)
-
-#define ADDR_HIGH_DUR			0x27
-#define HIGH_DUR_DIS_I2C			(1<<0)
-
-#define ADDR_TCO_Z			0x30
-#define TCO_Z_MODE_MASK				0x3
-
-#define ADDR_GAIN_Y			0x33
-#define GAIN_Y_SHADOW_DIS			(1<<0)
-
-#define ADDR_OFFSET_LSB1		0x35
-#define OFFSET_LSB1_RANGE_MASK			(7<<1)
-#define OFFSET_LSB1_RANGE_1G			(0<<1)
-#define OFFSET_LSB1_RANGE_2G			(2<<1)
-#define OFFSET_LSB1_RANGE_3G			(3<<1)
-#define OFFSET_LSB1_RANGE_4G			(4<<1)
-#define OFFSET_LSB1_RANGE_8G			(5<<1)
-#define OFFSET_LSB1_RANGE_16G			(6<<1)
-
-#define ADDR_OFFSET_T			0x37
-#define OFFSET_T_READOUT_12BIT			(1<<0)
-
-extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); }
-
-class BMA180 : public device::SPI
-{
-public:
-	BMA180(int bus, spi_dev_e device);
-	virtual ~BMA180();
-
-	virtual int		init();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void			print_info();
-
-protected:
-	virtual int		probe();
-
-private:
-
-	struct hrt_call		_call;
-	unsigned		_call_interval;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct accel_report	*_reports;
-
-	struct accel_scale	_accel_scale;
-	float			_accel_range_scale;
-	float			_accel_range_m_s2;
-	orb_advert_t		_accel_topic;
-
-	unsigned		_current_lowpass;
-	unsigned		_current_range;
-
-	perf_counter_t		_sample_perf;
-
-	/**
-	 * Start automatic measurement.
-	 */
-	void			start();
-
-	/**
-	 * Stop automatic measurement.
-	 */
-	void			stop();
-
-	/**
-	 * Static trampoline from the hrt_call context; because we don't have a
-	 * generic hrt wrapper yet.
-	 *
-	 * Called by the HRT in interrupt context at the specified rate if
-	 * automatic polling is enabled.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		measure_trampoline(void *arg);
-
-	/**
-	 * Fetch measurements from the sensor and update the report ring.
-	 */
-	void			measure();
-
-	/**
-	 * Read a register from the BMA180
-	 *
-	 * @param		The register to read.
-	 * @return		The value that was read.
-	 */
-	uint8_t			read_reg(unsigned reg);
-
-	/**
-	 * Write a register in the BMA180
-	 *
-	 * @param reg		The register to write.
-	 * @param value		The new value to write.
-	 */
-	void			write_reg(unsigned reg, uint8_t value);
-
-	/**
-	 * Modify a register in the BMA180
-	 *
-	 * Bits are cleared before bits are set.
-	 *
-	 * @param reg		The register to modify.
-	 * @param clearbits	Bits in the register to clear.
-	 * @param setbits	Bits in the register to set.
-	 */
-	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
-
-	/**
-	 * Set the BMA180 measurement range.
-	 *
-	 * @param max_g		The maximum G value the range must support.
-	 * @return		OK if the value can be supported, -ERANGE otherwise.
-	 */
-	int			set_range(unsigned max_g);
-
-	/**
-	 * Set the BMA180 internal lowpass filter frequency.
-	 *
-	 * @param frequency	The internal lowpass filter frequency is set to a value
-	 *			equal or greater to this.
-	 *			Zero selects the highest frequency supported.
-	 * @return		OK if the value can be supported.
-	 */
-	int			set_lowpass(unsigned frequency);
-};
-
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-
-BMA180::BMA180(int bus, spi_dev_e device) :
-	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
-	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_accel_range_scale(0.0f),
-	_accel_range_m_s2(0.0f),
-	_accel_topic(-1),
-	_current_lowpass(0),
-	_current_range(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
-{
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// default scale factors
-	_accel_scale.x_offset = 0;
-	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
-	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
-	_accel_scale.z_scale  = 1.0f;
-}
-
-BMA180::~BMA180()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-
-	/* delete the perf counter */
-	perf_free(_sample_perf);
-}
-
-int
-BMA180::init()
-{
-	int ret = ERROR;
-
-	/* do SPI init (and probe) first */
-	if (SPI::init() != OK)
-		goto out;
-
-	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct accel_report[_num_reports];
-
-	if (_reports == nullptr)
-		goto out;
-
-	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
-
-	/* perform soft reset (p48) */
-	write_reg(ADDR_RESET, SOFT_RESET);
-
-	/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
-	usleep(10000);
-
-	/* enable writing to chip config */
-	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
-
-	/* disable I2C interface */
-	modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
-
-	/* switch to low-noise mode */
-	modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
-
-	/* disable 12-bit mode */
-	modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
-
-	/* disable shadow-disable mode */
-	modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
-
-	/* disable writing to chip config */
-	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
-
-	if (set_range(4)) warnx("Failed setting range");
-
-	if (set_lowpass(75)) warnx("Failed setting lowpass");
-
-	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
-		ret = OK;
-
-	} else {
-		ret = ERROR;
-	}
-
-out:
-	return ret;
-}
-
-int
-BMA180::probe()
-{
-	/* dummy read to ensure SPI state machine is sane */
-	read_reg(ADDR_CHIP_ID);
-
-	if (read_reg(ADDR_CHIP_ID) == CHIP_ID)
-		return OK;
-
-	return -EIO;
-}
-
-ssize_t
-BMA180::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct accel_report);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1)
-		return -ENOSPC;
-
-	/* if automatic measurement is enabled */
-	if (_call_interval > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the measurement code while we are doing this;
-		 * we are careful to avoid racing with it.
-		 */
-		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement */
-	_oldest_report = _next_report = 0;
-	measure();
-
-	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
-
-	return ret;
-}
-
-int
-BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_call_interval = 0;
-				return OK;
-
-				/* external signalling not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_call_interval == 0);
-
-					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
-
-					/* check against maximum sane rate */
-					if (ticks < 1000)
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					/* XXX this is a bit shady, but no other way to adjust... */
-					_call.period = _call_interval = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_call_interval == 0)
-			return SENSOR_POLLRATE_MANUAL;
-
-		return 1000000 / _call_interval;
-
-	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct accel_report *buf = new struct accel_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
-
-	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
-
-	case ACCELIOCSSAMPLERATE:	/* sensor sample rate is not (really) adjustable */
-		return -EINVAL;
-
-	case ACCELIOCGSAMPLERATE:
-		return 1200;		/* always operating in low-noise mode */
-
-	case ACCELIOCSLOWPASS:
-		return set_lowpass(arg);
-
-	case ACCELIOCGLOWPASS:
-		return _current_lowpass;
-
-	case ACCELIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
-		return OK;
-
-	case ACCELIOCGSCALE:
-		/* copy scale out */
-		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
-		return OK;
-
-	case ACCELIOCSRANGE:
-		return set_range(arg);
-
-	case ACCELIOCGRANGE:
-		return _current_range;
-
-	default:
-		/* give it to the superclass */
-		return SPI::ioctl(filp, cmd, arg);
-	}
-}
-
-uint8_t
-BMA180::read_reg(unsigned reg)
-{
-	uint8_t cmd[2];
-
-	cmd[0] = reg | DIR_READ;
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return cmd[1];
-}
-
-void
-BMA180::write_reg(unsigned reg, uint8_t value)
-{
-	uint8_t	cmd[2];
-
-	cmd[0] = reg | DIR_WRITE;
-	cmd[1] = value;
-
-	transfer(cmd, nullptr, sizeof(cmd));
-}
-
-void
-BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
-{
-	uint8_t	val;
-
-	val = read_reg(reg);
-	val &= ~clearbits;
-	val |= setbits;
-	write_reg(reg, val);
-}
-
-int
-BMA180::set_range(unsigned max_g)
-{
-	uint8_t rangebits;
-
-	if (max_g == 0)
-		max_g = 16;
-
-	if (max_g > 16)
-		return -ERANGE;
-
-	if (max_g <= 2) {
-		_current_range = 2;
-		rangebits = OFFSET_LSB1_RANGE_2G;
-
-	} else if (max_g <= 3) {
-		_current_range = 3;
-		rangebits = OFFSET_LSB1_RANGE_3G;
-
-	} else if (max_g <= 4) {
-		_current_range = 4;
-		rangebits = OFFSET_LSB1_RANGE_4G;
-
-	} else if (max_g <= 8) {
-		_current_range = 8;
-		rangebits = OFFSET_LSB1_RANGE_8G;
-
-	} else if (max_g <= 16) {
-		_current_range = 16;
-		rangebits = OFFSET_LSB1_RANGE_16G;
-
-	} else {
-		return -EINVAL;
-	}
-
-	/* set new range scaling factor */
-	_accel_range_m_s2 = _current_range * 9.80665f;
-	_accel_range_scale = _accel_range_m_s2 / 8192.0f;
-
-	/* enable writing to chip config */
-	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
-
-	/* adjust sensor configuration */
-	modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
-
-	/* block writing to chip config */
-	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
-
-	/* check if wanted value is now in register */
-	return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
-		 (OFFSET_LSB1_RANGE_MASK & rangebits));
-}
-
-int
-BMA180::set_lowpass(unsigned frequency)
-{
-	uint8_t	bwbits;
-
-	if (frequency > 1200) {
-		return -ERANGE;
-
-	} else if (frequency > 600) {
-		bwbits = BW_TCS_BW_1200HZ;
-
-	} else if (frequency > 300) {
-		bwbits = BW_TCS_BW_600HZ;
-
-	} else if (frequency > 150) {
-		bwbits = BW_TCS_BW_300HZ;
-
-	} else if (frequency > 75) {
-		bwbits = BW_TCS_BW_150HZ;
-
-	} else if (frequency > 40) {
-		bwbits = BW_TCS_BW_75HZ;
-
-	} else if (frequency > 20) {
-		bwbits = BW_TCS_BW_40HZ;
-
-	} else if (frequency > 10) {
-		bwbits = BW_TCS_BW_20HZ;
-
-	} else {
-		bwbits = BW_TCS_BW_10HZ;
-	}
-
-	/* enable writing to chip config */
-	modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
-
-	/* adjust sensor configuration */
-	modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
-
-	/* block writing to chip config */
-	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
-
-	/* check if wanted value is now in register */
-	return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
-		 (BW_TCS_BW_MASK & bwbits));
-}
-
-void
-BMA180::start()
-{
-	/* make sure we are stopped first */
-	stop();
-
-	/* reset the report ring */
-	_oldest_report = _next_report = 0;
-
-	/* start polling at the specified rate */
-	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
-}
-
-void
-BMA180::stop()
-{
-	hrt_cancel(&_call);
-}
-
-void
-BMA180::measure_trampoline(void *arg)
-{
-	BMA180 *dev = (BMA180 *)arg;
-
-	/* make another measurement */
-	dev->measure();
-}
-
-void
-BMA180::measure()
-{
-	/* BMA180 measurement registers */
-// #pragma pack(push, 1)
-// 	struct {
-// 		uint8_t		cmd;
-// 		int16_t	x;
-// 		int16_t	y;
-// 		int16_t	z;
-// 	} raw_report;
-// #pragma pack(pop)
-
-	accel_report		*report = &_reports[_next_report];
-
-	/* start the performance counter */
-	perf_begin(_sample_perf);
-
-	/*
-	 * Fetch the full set of measurements from the BMA180 in one pass;
-	 * starting from the X LSB.
-	 */
-	//raw_report.cmd = ADDR_ACC_X_LSB;
-	// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
-
-	/*
-	 * Adjust and scale results to SI units.
-	 *
-	 * Note that we ignore the "new data" bits.  At any time we read, each
-	 * of the axis measurements are the "most recent", even if we've seen
-	 * them before.  There is no good way to synchronise with the internal
-	 * measurement flow without using the external interrupt.
-	 */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	/*
-	 * y of board is x of sensor and x of board is -y of sensor
-	 * perform only the axis assignment here.
-	 * Two non-value bits are discarded directly
-	 */
-	report->y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
-	report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
-	report->x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
-	report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
-	report->z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
-	report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
-
-	/* discard two non-value bits in the 16 bit measurement */
-	report->x_raw = (report->x_raw / 4);
-	report->y_raw = (report->y_raw / 4);
-	report->z_raw = (report->z_raw / 4);
-
-	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
-	report->y_raw = -report->y_raw;
-
-	report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	report->scaling = _accel_range_scale;
-	report->range_m_s2 = _accel_range_m_s2;
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
-
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
-
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-
-	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
-
-	/* stop the perf counter */
-	perf_end(_sample_perf);
-}
-
-void
-BMA180::print_info()
-{
-	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace bma180
-{
-
-BMA180	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-
-/**
- * Start the driver.
- */
-void
-start()
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
-
-	exit(0);
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-	int fd = -1;
-	struct accel_report a_report;
-	ssize_t sz;
-
-	/* get the driver */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
-		    ACCEL_DEVICE_PATH);
-
-	/* reset to manual polling */
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
-		err(1, "reset to manual polling");
-
-	/* do a simple demand read */
-	sz = read(fd, &a_report, sizeof(a_report));
-
-	if (sz != sizeof(a_report))
-		err(1, "immediate acc read failed");
-
-	warnx("single read");
-	warnx("time:     %lld", a_report.timestamp);
-	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
-	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
-	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
-	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
-	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
-	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
-	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / 9.81f));
-
-	/* XXX add poll-rate tests here too */
-
-	reset();
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
-
-	exit(0);
-}
-
-/**
- * Print a little info about the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "BMA180: driver not running");
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	exit(0);
-}
-
-
-} // namespace
-
-int
-bma180_main(int argc, char *argv[])
-{
-	/*
-	 * Start/load the driver.
-
-	 */
-	if (!strcmp(argv[1], "start"))
-		bma180::start();
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		bma180::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		bma180::reset();
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info"))
-		bma180::info();
-
-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
-}
diff --git a/apps/drivers/gps/Makefile b/apps/drivers/gps/Makefile
deleted file mode 100644
index 3859a88a5..000000000
--- a/apps/drivers/gps/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# GPS driver
-#
-
-APPNAME		 = gps
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
deleted file mode 100644
index e35bdb944..000000000
--- a/apps/drivers/gps/gps.cpp
+++ /dev/null
@@ -1,536 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file gps.cpp
- * Driver for the GPS on a serial port
- */
-
-#include <nuttx/clock.h>
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <nuttx/config.h>
-#include <nuttx/arch.h>
-#include <arch/board/board.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/device/i2c.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/scheduling_priorities.h>
-#include <systemlib/err.h>
-#include <drivers/drv_gps.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_gps_position.h>
-
-#include "ubx.h"
-#include "mtk.h"
-
-
-#define TIMEOUT_5HZ 500
-#define RATE_MEASUREMENT_PERIOD 5000000
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-
-
-class GPS : public device::CDev
-{
-public:
-	GPS(const char* uart_path);
-	virtual ~GPS();
-
-	virtual int			init();
-
-	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void				print_info();
-
-private:
-
-	bool				_task_should_exit;				///< flag to make the main worker task exit
-	int				_serial_fd;					///< serial interface to GPS
-	unsigned			_baudrate;					///< current baudrate
-	char				_port[20];					///< device / serial port path
-	volatile int			_task;						//< worker task
-	bool				_healthy;					///< flag to signal if the GPS is ok
-	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
-	bool				_mode_changed;					///< flag that the GPS mode has changed
-	gps_driver_mode_t		_mode;						///< current mode
-	GPS_Helper			*_Helper;					///< instance of GPS parser
-	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
-	orb_advert_t			_report_pub;					///< uORB pub for gps position
-	float				_rate;						///< position update rate
-
-
-	/**
-	 * Try to configure the GPS, handle outgoing communication to the GPS
-	 */
-	void				config();
-
-	/**
-	 * Trampoline to the worker task
-	 */
-	static void			task_main_trampoline(void *arg);
-
-
-	/**
-	 * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
-	 */
-	void				task_main(void);
-
-	/**
-	 * Set the baudrate of the UART to the GPS
-	 */
-	int				set_baudrate(unsigned baud);
-
-	/**
-	 * Send a reset command to the GPS
-	 */
-	void				cmd_reset();
-
-};
-
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int gps_main(int argc, char *argv[]);
-
-namespace
-{
-
-GPS	*g_dev;
-
-}
-
-
-GPS::GPS(const char* uart_path) :
-	CDev("gps", GPS_DEVICE_PATH),
-	_task_should_exit(false),
-	_healthy(false),
-	_mode_changed(false),
-	_mode(GPS_DRIVER_MODE_UBX),
-	_Helper(nullptr),
-	_report_pub(-1),
-	_rate(0.0f)
-{
-	/* store port name */
-	strncpy(_port, uart_path, sizeof(_port));
-	/* enforce null termination */
-	_port[sizeof(_port) - 1] = '\0';
-
-	/* we need this potentially before it could be set in task_main */
-	g_dev = this;
-	memset(&_report, 0, sizeof(_report));
-
-	_debug_enabled = true;
-}
-
-GPS::~GPS()
-{
-	/* tell the task we want it to go away */
-	_task_should_exit = true;
-
-	/* spin waiting for the task to stop */
-	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
-		/* give it another 100ms */
-		usleep(100000);
-	}
-
-	/* well, kill it anyway, though this will probably crash */
-	if (_task != -1)
-		task_delete(_task);
-	g_dev = nullptr;
-
-}
-
-int
-GPS::init()
-{
-	int ret = ERROR;
-
-	/* do regular cdev init */
-	if (CDev::init() != OK)
-		goto out;
-
-	/* start the GPS driver worker task */
-	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
-
-	if (_task < 0) {
-		warnx("task start failed: %d", errno);
-		return -errno;
-	}
-
-	ret = OK;
-out:
-	return ret;
-}
-
-int
-GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	lock();
-
-	int ret = OK;
-
-	switch (cmd) {
-	case SENSORIOCRESET:
-		cmd_reset();
-		break;
-	}
-
-	unlock();
-
-	return ret;
-}
-
-void
-GPS::task_main_trampoline(void *arg)
-{
-	g_dev->task_main();
-}
-
-void
-GPS::task_main()
-{
-	log("starting");
-
-	/* open the serial port */
-	_serial_fd = ::open(_port, O_RDWR);
-
-	if (_serial_fd < 0) {
-		log("failed to open serial port: %s err: %d", _port, errno);
-		/* tell the dtor that we are exiting, set error code */
-		_task = -1;
-		_exit(1);
-	}
-
-	uint64_t last_rate_measurement = hrt_absolute_time();
-	unsigned last_rate_count = 0;
-
-	/* loop handling received serial bytes and also configuring in between */
-	while (!_task_should_exit) {
-
-		if (_Helper != nullptr) {
-			delete(_Helper);
-			/* set to zero to ensure parser is not used while not instantiated */
-			_Helper = nullptr;
-		}
-
-		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
-				break;
-			case GPS_DRIVER_MODE_MTK:
-				_Helper = new MTK(_serial_fd, &_report);
-				break;
-			case GPS_DRIVER_MODE_NMEA:
-				//_Helper = new NMEA(); //TODO: add NMEA
-				break;
-			default:
-				break;
-		}
-		unlock();
-		if (_Helper->configure(_baudrate) == 0) {
-			unlock();
-			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
-//				lock();
-				/* opportunistic publishing - else invalid data would end up on the bus */
-				if (_report_pub > 0) {
-					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
-				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
-				}
-
-				last_rate_count++;
-
-				/* measure update rate every 5 seconds */
-				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
-					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
-					last_rate_measurement = hrt_absolute_time();
-					last_rate_count = 0;
-				}
-
-				if (!_healthy) {
-					warnx("module found");
-					_healthy = true;
-				}
-			}
-			if (_healthy) {
-				warnx("module lost");
-				_healthy = false;
-				_rate = 0.0f;
-			}
-
-			lock();
-		}
-		lock();
-
-		/* select next mode */
-		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_mode = GPS_DRIVER_MODE_MTK;
-				break;
-			case GPS_DRIVER_MODE_MTK:
-				_mode = GPS_DRIVER_MODE_UBX;
-				break;
-		//				case GPS_DRIVER_MODE_NMEA:
-		//					_mode = GPS_DRIVER_MODE_UBX;
-		//					break;
-			default:
-				break;
-		}
-
-	}
-	debug("exiting");
-
-	::close(_serial_fd);
-
-	/* tell the dtor that we are exiting */
-	_task = -1;
-	_exit(0);
-}
-
-
-
-void
-GPS::cmd_reset()
-{
-	//XXX add reset?
-}
-
-void
-GPS::print_info()
-{
-	switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			warnx("protocol: UBX");
-			break;
-		case GPS_DRIVER_MODE_MTK:
-			warnx("protocol: MTK");
-			break;
-		case GPS_DRIVER_MODE_NMEA:
-			warnx("protocol: NMEA");
-			break;
-		default:
-			break;
-	}
-	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
-	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
-			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
-		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("update rate: %6.2f Hz", (double)_rate);
-	}
-
-	usleep(100000);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace gps
-{
-
-GPS	*g_dev;
-
-void	start(const char *uart_path);
-void	stop();
-void	test();
-void	reset();
-void	info();
-
-/**
- * Start the driver.
- */
-void
-start(const char *uart_path)
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new GPS(uart_path);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(GPS_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
-		goto fail;
-	}
-	exit(0);
-
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Stop the driver.
- */
-void
-stop()
-{
-	delete g_dev;
-	g_dev = nullptr;
-
-	exit(0);
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(GPS_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	exit(0);
-}
-
-/**
- * Print the status of the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running");
-
-	g_dev->print_info();
-
-	exit(0);
-}
-
-} // namespace
-
-
-int
-gps_main(int argc, char *argv[])
-{
-
-	/* set to default */
-	char* device_name = GPS_DEFAULT_UART_PORT;
-
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(argv[1], "start")) {
-		/* work around getopt unreliability */
-		if (argc > 3) {
-			if (!strcmp(argv[2], "-d")) {
-				device_name = argv[3];
-			} else {
-				goto out;
-			}
-		}
-		gps::start(device_name);
-	}
-
-	if (!strcmp(argv[1], "stop"))
-		gps::stop();
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		gps::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		gps::reset();
-
-	/*
-	 * Print driver status.
-	 */
-	if (!strcmp(argv[1], "status"))
-		gps::info();
-
-out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
-}
diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp
deleted file mode 100644
index 9c1fad569..000000000
--- a/apps/drivers/gps/gps_helper.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <termios.h>
-#include <errno.h>
-#include <systemlib/err.h>
-#include "gps_helper.h"
-
-/* @file gps_helper.cpp */
-
-int
-GPS_Helper::set_baudrate(const int &fd, unsigned baud)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	warnx("try baudrate: %d\n", speed);
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n", baud);
-		return -EINVAL;
-	}
-	struct termios uart_config;
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	tcgetattr(fd, &uart_config);
-
-	/* clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-	/* no parity, one stop bit */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
-		return -1;
-	}
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
-		return -1;
-	}
-	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate (tcsetattr)\n");
-		return -1;
-	}
-	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
-	return 0;
-}
diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h
deleted file mode 100644
index f3d3bc40b..000000000
--- a/apps/drivers/gps/gps_helper.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* @file gps_helper.h */
-
-#ifndef GPS_HELPER_H
-#define GPS_HELPER_H
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_gps_position.h>
-
-class GPS_Helper
-{
-public:
-	virtual int				configure(unsigned &baud) = 0;
-	virtual int 			receive(unsigned timeout) = 0;
-	int 					set_baudrate(const int &fd, unsigned baud);
-};
-
-#endif /* GPS_HELPER_H */
diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp
deleted file mode 100644
index 4762bd503..000000000
--- a/apps/drivers/gps/mtk.cpp
+++ /dev/null
@@ -1,274 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* @file mkt.cpp */
-
-#include <unistd.h>
-#include <stdio.h>
-#include <poll.h>
-#include <math.h>
-#include <string.h>
-#include <assert.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-
-#include "mtk.h"
-
-
-MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_mtk_revision(0)
-{
-	decode_init();
-}
-
-MTK::~MTK()
-{
-}
-
-int
-MTK::configure(unsigned &baudrate)
-{
-	/* set baudrate first */
-	if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
-		return -1;
-
-	baudrate = MTK_BAUDRATE;
-
-	/* Write config messages, don't wait for an answer */
-	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
-		warnx("mtk: config write failed");
-		return -1;
-	}
-	usleep(10000);
-
-	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
-		warnx("mtk: config write failed");
-		return -1;
-	}
-	usleep(10000);
-
-	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
-		warnx("mtk: config write failed");
-		return -1;
-	}
-	usleep(10000);
-
-	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
-		warnx("mtk: config write failed");
-		return -1;
-	}
-	usleep(10000);
-
-	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
-		warnx("mtk: config write failed");
-		return -1;
-	}
-
-	return 0;
-}
-
-int
-MTK::receive(unsigned timeout)
-{
-	/* poll descriptor */
-	pollfd fds[1];
-	fds[0].fd = _fd;
-	fds[0].events = POLLIN;
-
-	uint8_t buf[32];
-	gps_mtk_packet_t packet;
-
-	/* timeout additional to poll */
-	uint64_t time_started = hrt_absolute_time();
-
-	int j = 0;
-	ssize_t count = 0;
-
-	while (true) {
-
-		/* first read whatever is left */
-		if (j < count) {
-			/* pass received bytes to the packet decoder */
-			while (j < count) {
-				if (parse_char(buf[j], packet) > 0) {
-					handle_message(packet);
-					return 1;
-				}
-				/* in case we keep trying but only get crap from GPS */
-				if (time_started + timeout*1000 < hrt_absolute_time() ) {
-					return -1;
-				}
-				j++;
-			}
-			/* everything is read */
-			j = count = 0;
-		}
-
-		/* then poll for new data */
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
-
-		if (ret < 0) {
-			/* something went wrong when polling */
-			return -1;
-
-		} else if (ret == 0) {
-			/* Timeout */
-			return -1;
-
-		} else if (ret > 0) {
-			/* if we have new data from GPS, go handle it */
-			if (fds[0].revents & POLLIN) {
-				/*
-				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device.  If more bytes are
-				 * available, we'll go back to poll() again...
-				 */
-				count = ::read(_fd, buf, sizeof(buf));
-			}
-		}
-	}
-}
-
-void
-MTK::decode_init(void)
-{
-	_rx_ck_a = 0;
-	_rx_ck_b = 0;
-	_rx_count = 0;
-	_decode_state = MTK_DECODE_UNINIT;
-}
-int
-MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
-{
-	int ret = 0;
-
-	if (_decode_state == MTK_DECODE_UNINIT) {
-
-		if (b == MTK_SYNC1_V16) {
-			_decode_state = MTK_DECODE_GOT_CK_A;
-			_mtk_revision = 16;
-		} else if (b == MTK_SYNC1_V19) {
-			_decode_state = MTK_DECODE_GOT_CK_A;
-			_mtk_revision = 19;
-		}
-
-	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
-		if (b == MTK_SYNC2) {
-			_decode_state = MTK_DECODE_GOT_CK_B;
-
-		} else {
-			// Second start symbol was wrong, reset state machine
-			decode_init();
-		}
-
-	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
-		// Add to checksum
-		if (_rx_count < 33)
-			add_byte_to_checksum(b);
-
-		// Fill packet buffer
-		((uint8_t*)(&packet))[_rx_count] = b;
-		_rx_count++;
-
-		/* Packet size minus checksum, XXX ? */
-		if (_rx_count >= sizeof(packet)) {
-			/* Compare checksum */
-			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
-				ret = 1;
-			} else {
-				warnx("MTK Checksum invalid");
-				ret = -1;
-			}
-			// Reset state machine to decode next packet
-			decode_init();
-		}
-	}
-	return ret;
-}
-
-void
-MTK::handle_message(gps_mtk_packet_t &packet)
-{
-	if (_mtk_revision == 16) {
-		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
-		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
-	} else if (_mtk_revision == 19) {
-		_gps_position->lat = packet.latitude; // both degrees*1e7
-		_gps_position->lon = packet.longitude; // both degrees*1e7
-	} else {
-		warnx("mtk: unknown revision");
-		_gps_position->lat = 0;
-		_gps_position->lon = 0;
-	}
-	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
-	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
-	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
-	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
-	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
-	_gps_position->satellites_visible = packet.satellites;
-
-	/* convert time and date information to unix timestamp */
-	struct tm timeinfo; //TODO: test this conversion
-	uint32_t timeinfo_conversion_temp;
-
-	timeinfo.tm_mday = packet.date * 1e-4;
-	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
-	timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
-	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
-
-	timeinfo.tm_hour = packet.utc_time * 1e-7;
-	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
-	timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
-	timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
-	timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
-	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
-	time_t epoch = mktime(&timeinfo);
-
-	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
-	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
-	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
-
-	return;
-}
-
-void
-MTK::add_byte_to_checksum(uint8_t b)
-{
-	_rx_ck_a = _rx_ck_a + b;
-	_rx_ck_b = _rx_ck_b + _rx_ck_a;
-}
diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h
deleted file mode 100644
index d4e390b01..000000000
--- a/apps/drivers/gps/mtk.h
+++ /dev/null
@@ -1,124 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* @file mtk.h */
-
-#ifndef MTK_H_
-#define MTK_H_
-
-#include "gps_helper.h"
-
-#define MTK_SYNC1_V16 0xd0
-#define MTK_SYNC1_V19 0xd1
-#define MTK_SYNC2 0xdd
-
-#define MTK_OUTPUT_5HZ		"$PMTK220,200*2C\r\n"
-#define MTK_SET_BINARY		"$PGCMD,16,0,0,0,0,0*6A\r\n"
-#define SBAS_ON	        	"$PMTK313,1*2E\r\n"
-#define WAAS_ON				"$PMTK301,2*2E\r\n"
-#define MTK_NAVTHRES_OFF 	"$PMTK397,0*23\r\n"
-
-#define MTK_TIMEOUT_5HZ 400
-#define MTK_BAUDRATE 38400
-
-typedef enum {
-	MTK_DECODE_UNINIT = 0,
-	MTK_DECODE_GOT_CK_A = 1,
-	MTK_DECODE_GOT_CK_B = 2
-} mtk_decode_state_t;
-
-/** the structures of the binary packets */
-#pragma pack(push, 1)
-
-typedef struct {
-	uint8_t payload; ///< Number of payload bytes
-	int32_t latitude;  ///< Latitude in degrees * 10^7
-	int32_t longitude; ///< Longitude in degrees * 10^7
-	uint32_t msl_altitude;  ///< MSL altitude in meters * 10^2
-	uint32_t ground_speed; ///< velocity in m/s
-	int32_t heading; ///< heading in degrees * 10^2
-	uint8_t satellites; ///< number of sattelites used
-	uint8_t fix_type;  ///< fix type: XXX correct for that
-	uint32_t date;
-	uint32_t utc_time;
-	uint16_t hdop; ///< horizontal dilution of position (without unit)
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_mtk_packet_t;
-
-#pragma pack(pop)
-
-#define MTK_RECV_BUFFER_SIZE 40
-
-class MTK : public GPS_Helper
-{
-public:
-	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
-	~MTK();
-	int					receive(unsigned timeout);
-	int					configure(unsigned &baudrate);
-
-private:
-	/**
-	 * Parse the binary MTK packet
-	 */
-	int					parse_char(uint8_t b, gps_mtk_packet_t &packet);
-
-	/**
-	 * Handle the package once it has arrived
-	 */
-	void				handle_message(gps_mtk_packet_t &packet);
-
-	/**
-	 * Reset the parse state machine for a fresh start
-	 */
-	void				decode_init(void);
-
-	/**
-	 * While parsing add every byte (except the sync bytes) to the checksum
-	 */
-	void				add_byte_to_checksum(uint8_t);
-
-	int					_fd;
-	struct vehicle_gps_position_s *_gps_position;
-	mtk_decode_state_t	_decode_state;
-	uint8_t				_mtk_revision;
-	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];
-	unsigned			_rx_count;
-	uint8_t 			_rx_ck_a;
-	uint8_t				_rx_ck_b;
-};
-
-#endif /* MTK_H_ */
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
deleted file mode 100644
index c150f5271..000000000
--- a/apps/drivers/gps/ubx.cpp
+++ /dev/null
@@ -1,755 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ubx.cpp
- *
- * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
- * including Prototol Specification.
- *
- * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
- */
-
-#include <unistd.h>
-#include <stdio.h>
-#include <poll.h>
-#include <math.h>
-#include <string.h>
-#include <assert.h>
-#include <systemlib/err.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <drivers/drv_hrt.h>
-
-#include "ubx.h"
-
-#define UBX_CONFIG_TIMEOUT 100
-
-UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_waiting_for_ack(false)
-{
-	decode_init();
-}
-
-UBX::~UBX()
-{
-}
-
-int
-UBX::configure(unsigned &baudrate)
-{
-	_waiting_for_ack = true;
-
-	/* try different baudrates */
-	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
-
-	for (int baud_i = 0; baud_i < 5; baud_i++) {
-		baudrate = baudrates_to_try[baud_i];
-		set_baudrate(_fd, baudrate);
-
-		/* Send a CFG-PRT message to set the UBX protocol for in and out
-		 * and leave the baudrate as it is, we just want an ACK-ACK from this
-		 */
-		type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
-		/* Set everything else of the packet to 0, otherwise the module wont accept it */
-		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
-
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_PRT;
-
-		/* Define the package contents, don't change the baudrate */
-		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
-		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
-		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
-		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
-		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
-		cfg_prt_packet.baudRate		= baudrate;
-		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
-		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		/* Send a CFG-PRT message again, this time change the baudrate */
-
-		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
-		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
-		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
-		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
-		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
-		cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
-		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
-		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-		
-		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-		receive(UBX_CONFIG_TIMEOUT);
-		
-		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
-			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
-			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
-		}
-
-		/* send a CFT-RATE message to define update rate */
-		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
-		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
-
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_RATE;
-
-		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
-		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
-		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
-		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
-		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
-		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		/* send a NAV5 message to set the options for the internal filter */
-		type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
-		memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
-
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
-
-		cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
-		cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
-		cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
-		cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
-		cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
-		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
-		memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
-
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_MSG;
-
-		cfg_msg_packet.clsID        = UBX_CLASS_CFG;
-		cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
-		cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
-		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
-		cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
-		/* For satelites info 1Hz is enough */
-		cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-//		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
-
-//		cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
-//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
-
-		_waiting_for_ack = false;
-		return 0;
-	}
-	return -1;
-}
-
-int
-UBX::receive(unsigned timeout)
-{
-	/* poll descriptor */
-	pollfd fds[1];
-	fds[0].fd = _fd;
-	fds[0].events = POLLIN;
-
-	uint8_t buf[32];
-
-	/* timeout additional to poll */
-	uint64_t time_started = hrt_absolute_time();
-
-	int j = 0;
-	ssize_t count = 0;
-
-	while (true) {
-
-		/* pass received bytes to the packet decoder */
-		while (j < count) {
-			if (parse_char(buf[j]) > 0) {
-				/* return to configure during configuration or to the gps driver during normal work
-				 * if a packet has arrived */
-				 if (handle_message() > 0)
-					return 1;
-			}
-			/* in case we keep trying but only get crap from GPS */
-			if (time_started + timeout*1000 < hrt_absolute_time() ) {
-				return -1;
-			}
-			j++;
-		}
-
-		/* everything is read */
-		j = count = 0;
-
-		/* then poll for new data */
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
-
-		if (ret < 0) {
-			/* something went wrong when polling */
-			return -1;
-
-		} else if (ret == 0) {
-			/* Timeout */
-			return -1;
-
-		} else if (ret > 0) {
-			/* if we have new data from GPS, go handle it */
-			if (fds[0].revents & POLLIN) {
-				/*
-				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device.  If more bytes are
-				 * available, we'll go back to poll() again...
-				 */
-				count = ::read(_fd, buf, sizeof(buf));
-			}
-		}
-	}
-}
-
-int
-UBX::parse_char(uint8_t b)
-{
-	switch (_decode_state) {
-		/* First, look for sync1 */
-		case UBX_DECODE_UNINIT:
-			if (b == UBX_SYNC1) {
-				_decode_state = UBX_DECODE_GOT_SYNC1;
-			}
-			break;
-		/* Second, look for sync2 */
-		case UBX_DECODE_GOT_SYNC1:
-			if (b == UBX_SYNC2) {
-				_decode_state = UBX_DECODE_GOT_SYNC2;
-			} else {
-				/* Second start symbol was wrong, reset state machine */
-				decode_init();
-			}
-			break;
-		/* Now look for class */
-		case UBX_DECODE_GOT_SYNC2:
-			/* everything except sync1 and sync2 needs to be added to the checksum */
-			add_byte_to_checksum(b);
-			/* check for known class */
-			switch (b) {
-				case UBX_CLASS_ACK:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = ACK;
-					break;
-
-				case UBX_CLASS_NAV:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = NAV;
-					break;
-
-//				case UBX_CLASS_RXM:
-//					_decode_state = UBX_DECODE_GOT_CLASS;
-//					_message_class = RXM;
-//					break;
-
-				case UBX_CLASS_CFG:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = CFG;
-					break;
-				default: //unknown class: reset state machine
-					decode_init();
-					break;
-			}
-			break;
-		case UBX_DECODE_GOT_CLASS:
-			add_byte_to_checksum(b);
-			switch (_message_class) {
-				case NAV:
-					switch (b) {
-					case UBX_MESSAGE_NAV_POSLLH:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_POSLLH;
-						break;
-
-					case UBX_MESSAGE_NAV_SOL:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_SOL;
-						break;
-
-					case UBX_MESSAGE_NAV_TIMEUTC:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_TIMEUTC;
-						break;
-
-//					case UBX_MESSAGE_NAV_DOP:
-//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-//						_message_id = NAV_DOP;
-//						break;
-
-					case UBX_MESSAGE_NAV_SVINFO:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_SVINFO;
-						break;
-
-					case UBX_MESSAGE_NAV_VELNED:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_VELNED;
-						break;
-
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-//				case RXM:
-//					switch (b) {
-//					case UBX_MESSAGE_RXM_SVSI:
-//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-//						_message_id = RXM_SVSI;
-//						break;
-//
-//					default: //unknown class: reset state machine, should not happen
-//						decode_init();
-//						break;
-//					}
-//					break;
-
-				case CFG:
-					switch (b) {
-					case UBX_MESSAGE_CFG_NAV5:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = CFG_NAV5;
-						break;
-
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-
-				case ACK:
-					switch (b) {
-					case UBX_MESSAGE_ACK_ACK:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = ACK_ACK;
-						break;
-					case UBX_MESSAGE_ACK_NAK:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = ACK_NAK;
-						break;
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-				default: //should not happen because we set the class
-					warnx("UBX Error, we set a class that we don't know");
-					decode_init();
-//					config_needed = true;
-					break;
-				}
-				break;
-			case UBX_DECODE_GOT_MESSAGEID:
-				add_byte_to_checksum(b);
-				_payload_size = b; //this is the first length byte
-				_decode_state = UBX_DECODE_GOT_LENGTH1;
-				break;
-			case UBX_DECODE_GOT_LENGTH1:
-				add_byte_to_checksum(b);
-				_payload_size += b << 8; // here comes the second byte of length
-				_decode_state = UBX_DECODE_GOT_LENGTH2;
-				break;
-			case UBX_DECODE_GOT_LENGTH2:
-				/* Add to checksum if not yet at checksum byte */
-				if (_rx_count < _payload_size)
-					add_byte_to_checksum(b);
-					_rx_buffer[_rx_count] = b;
-					/* once the payload has arrived, we can process the information */
-					if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
-
-						/* compare checksum */
-						if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
-							return 1;
-						} else {
-							decode_init();
-							return -1;
-							warnx("ubx: Checksum wrong");
-						}
-
-						return 1;
-					} else if (_rx_count < RECV_BUFFER_SIZE) {
-						_rx_count++;
-					} else {
-						warnx("ubx: buffer full");
-						decode_init();
-						return -1;
-					}
-				break;
-		default:
-			break;
-	}
-	return 0; //XXX ?
-}
-
-
-int
-UBX::handle_message()
-{
-	int ret = 0;
-
-	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
-		case NAV_POSLLH: {
-//			printf("GOT NAV_POSLLH MESSAGE\n");
-			if (!_waiting_for_ack) {
-				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
-
-				_gps_position->lat = packet->lat;
-				_gps_position->lon = packet->lon;
-				_gps_position->alt = packet->height_msl;
-
-				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
-				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
-
-				/* Add timestamp to finish the report */
-				_gps_position->timestamp_position = hrt_absolute_time();
-				/* only return 1 when new position is available */
-				ret = 1;
-			}
-			break;
-		}
-
-		case NAV_SOL: {
-//			printf("GOT NAV_SOL MESSAGE\n");
-			if (!_waiting_for_ack) {
-				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
-
-				_gps_position->fix_type = packet->gpsFix;
-				_gps_position->s_variance_m_s = packet->sAcc;
-				_gps_position->p_variance_m = packet->pAcc;
-
-				_gps_position->timestamp_variance = hrt_absolute_time();
-			}
-			break;
-		}
-
-//		case NAV_DOP: {
-////		printf("GOT NAV_DOP MESSAGE\n");
-//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
-//
-//			_gps_position->eph_m =  packet->hDOP;
-//			_gps_position->epv =  packet->vDOP;
-//
-//			_gps_position->timestamp_posdilution = hrt_absolute_time();
-//
-//			_new_nav_dop = true;
-//
-//			break;
-//		}
-
-		case NAV_TIMEUTC: {
-//			printf("GOT NAV_TIMEUTC MESSAGE\n");
-
-			if (!_waiting_for_ack) {
-				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
-
-				//convert to unix timestamp
-				struct tm timeinfo;
-				timeinfo.tm_year = packet->year - 1900;
-				timeinfo.tm_mon = packet->month - 1;
-				timeinfo.tm_mday = packet->day;
-				timeinfo.tm_hour = packet->hour;
-				timeinfo.tm_min = packet->min;
-				timeinfo.tm_sec = packet->sec;
-
-				time_t epoch = mktime(&timeinfo);
-
-				_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
-
-				_gps_position->timestamp_time = hrt_absolute_time();
-			}
-			break;
-		}
-
-		case NAV_SVINFO: {
-//			printf("GOT NAV_SVINFO MESSAGE\n");
-
-			if (!_waiting_for_ack) {
-				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
-				const int length_part1 = 8;
-				char _rx_buffer_part1[length_part1];
-				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
-
-				//read checksum
-				const int length_part3 = 2;
-				char _rx_buffer_part3[length_part3];
-				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
-
-				//definitions needed to read numCh elements from the buffer:
-				const int length_part2 = 12;
-				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-				char _rx_buffer_part2[length_part2]; //for temporal storage
-
-				uint8_t satellites_used = 0;
-				int i;
-
-				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
-
-					/* Get satellite information from the buffer */
-					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
-					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
-
-
-					/* Write satellite information in the global storage */
-					_gps_position->satellite_prn[i] = packet_part2->svid;
-
-					//if satellite information is healthy store the data
-					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
-
-					if (!unhealthy) {
-						if ((packet_part2->flags) & 1) { //flags is a bitfield
-							_gps_position->satellite_used[i] = 1;
-							satellites_used++;
-
-						} else {
-							_gps_position->satellite_used[i] = 0;
-						}
-
-						_gps_position->satellite_snr[i] = packet_part2->cno;
-						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-
-					} else {
-						_gps_position->satellite_used[i] = 0;
-						_gps_position->satellite_snr[i] = 0;
-						_gps_position->satellite_elevation[i] = 0;
-						_gps_position->satellite_azimuth[i] = 0;
-					}
-
-				}
-
-				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
-					/* Unused channels have to be set to zero for e.g. MAVLink */
-					_gps_position->satellite_prn[i] = 0;
-					_gps_position->satellite_used[i] = 0;
-					_gps_position->satellite_snr[i] = 0;
-					_gps_position->satellite_elevation[i] = 0;
-					_gps_position->satellite_azimuth[i] = 0;
-				}
-				_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
-
-				/* set timestamp if any sat info is available */
-				if (packet_part1->numCh > 0) {
-					_gps_position->satellite_info_available = true;
-				} else {
-					_gps_position->satellite_info_available = false;
-				}
-				_gps_position->timestamp_satellites = hrt_absolute_time();
-			}
-
-			break;
-		}
-
-		case NAV_VELNED: {
-
-			if (!_waiting_for_ack) {
-				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
-				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
-
-				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
-				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
-				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
-				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
-				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
-				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
-				_gps_position->vel_ned_valid = true;
-				_gps_position->timestamp_velocity = hrt_absolute_time();
-			}
-
-			break;
-		}
-
-//		case RXM_SVSI: {
-//				printf("GOT RXM_SVSI MESSAGE\n");
-//				const int length_part1 = 7;
-//				char _rx_buffer_part1[length_part1];
-//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
-//
-//				_gps_position->satellites_visible = packet->numVis;
-//				_gps_position->counter++;
-//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
-//
-//			break;
-//		}
-		case ACK_ACK: {
-//			printf("GOT ACK_ACK\n");
-			gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
-
-			if (_waiting_for_ack) {
-				if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
-					ret = 1;
-				}
-			}
-		}
-		break;
-
-		case ACK_NAK: {
-//			printf("GOT ACK_NAK\n");
-			warnx("UBX: Received: Not Acknowledged");
-			/* configuration obviously not successful */
-			ret = -1;
-			break;
-		}
-
-		default: //we don't know the message
-			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
-			ret = -1;
-			break;
-		}
-	// end if _rx_count high enough
-	decode_init();
-	return ret; //XXX?
-}
-
-void
-UBX::decode_init(void)
-{
-	_rx_ck_a = 0;
-	_rx_ck_b = 0;
-	_rx_count = 0;
-	_decode_state = UBX_DECODE_UNINIT;
-	_message_class = CLASS_UNKNOWN;
-	_message_id = ID_UNKNOWN;
-	_payload_size = 0;
-}
-
-void
-UBX::add_byte_to_checksum(uint8_t b)
-{
-	_rx_ck_a = _rx_ck_a + b;
-	_rx_ck_b = _rx_ck_b + _rx_ck_a;
-}
-
-void
-UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
-{
-	uint8_t ck_a = 0;
-	uint8_t ck_b = 0;
-	unsigned i;
-
-	for (i = 0; i < length-2; i++) {
-		ck_a = ck_a + message[i];
-		ck_b = ck_b + ck_a;
-	}
-	/* The checksum is written to the last to bytes of a message */
-	message[length-2] = ck_a;
-	message[length-1] = ck_b;
-}
-
-void
-UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
-{
-	ssize_t ret = 0;
-
-	/* Calculate the checksum now */
-	add_checksum_to_message(packet, length);
-
-	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
-
-	/* Start with the two sync bytes */
-	ret += write(fd, sync_bytes, sizeof(sync_bytes));
-	ret += write(fd, packet, length);
-
-	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: config write fail");
-}
diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h
deleted file mode 100644
index e3dd1c7ea..000000000
--- a/apps/drivers/gps/ubx.h
+++ /dev/null
@@ -1,395 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* @file U-Blox protocol definitions */
-
-#ifndef UBX_H_
-#define UBX_H_
-
-#include "gps_helper.h"
-
-#define UBX_SYNC1 0xB5
-#define UBX_SYNC2 0x62
-
-/* ClassIDs (the ones that are used) */
-#define UBX_CLASS_NAV 0x01
-//#define UBX_CLASS_RXM 0x02
-#define UBX_CLASS_ACK 0x05
-#define UBX_CLASS_CFG 0x06
-
-/* MessageIDs (the ones that are used) */
-#define UBX_MESSAGE_NAV_POSLLH 0x02
-#define UBX_MESSAGE_NAV_SOL 0x06
-#define UBX_MESSAGE_NAV_TIMEUTC 0x21
-//#define UBX_MESSAGE_NAV_DOP 0x04
-#define UBX_MESSAGE_NAV_SVINFO 0x30
-#define UBX_MESSAGE_NAV_VELNED 0x12
-//#define UBX_MESSAGE_RXM_SVSI 0x20
-#define UBX_MESSAGE_ACK_ACK 0x01
-#define UBX_MESSAGE_ACK_NAK 0x00
-#define UBX_MESSAGE_CFG_PRT 0x00
-#define UBX_MESSAGE_CFG_NAV5 0x24
-#define UBX_MESSAGE_CFG_MSG 0x01
-#define UBX_MESSAGE_CFG_RATE 0x08
-
-#define UBX_CFG_PRT_LENGTH 20
-#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01         /**< UART1 */
-#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0     /**< 0b0000100011010000: 8N1 */
-#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400      /**< always choose 38400 as GPS baudrate */
-#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01    /**< UBX in */
-#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01   /**< UBX out */
-
-#define UBX_CFG_RATE_LENGTH 6
-#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200		/**< 200ms for 5Hz */
-#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
-#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
-
-
-#define UBX_CFG_NAV5_LENGTH 36
-#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001		/**< only update dynamic model and fix mode */
-#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
-#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2          /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
-
-#define UBX_CFG_MSG_LENGTH 8
-#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
-#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
-
-#define UBX_MAX_PAYLOAD_LENGTH 500
-
-// ************
-/** the structures of the binary packets */
-#pragma pack(push, 1)
-
-typedef struct {
-	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
-	int32_t lon;					/**<  Longitude * 1e-7, deg */
-	int32_t lat;					/**<  Latitude * 1e-7, deg */
-	int32_t height;					/**<  Height above Ellipsoid, mm */
-	int32_t height_msl;				/**<  Height above mean sea level, mm */
-	uint32_t hAcc;  				/**< Horizontal Accuracy Estimate, mm */
-	uint32_t vAcc;  				/**< Vertical Accuracy Estimate, mm */
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_posllh_packet_t;
-
-typedef struct {
-	uint32_t time_milliseconds; 	/**< GPS Millisecond Time of Week */
-	int32_t time_nanoseconds;		/**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
-	int16_t week;					/**< GPS week (GPS time) */
-	uint8_t gpsFix;					/**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
-	uint8_t flags;
-	int32_t ecefX;
-	int32_t ecefY;
-	int32_t ecefZ;
-	uint32_t pAcc;
-	int32_t ecefVX;
-	int32_t ecefVY;
-	int32_t ecefVZ;
-	uint32_t sAcc;
-	uint16_t pDOP;
-	uint8_t reserved1;
-	uint8_t numSV;
-	uint32_t reserved2;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_sol_packet_t;
-
-typedef struct {
-	uint32_t time_milliseconds;		/**< GPS Millisecond Time of Week */
-	uint32_t time_accuracy; 		/**< Time Accuracy Estimate, ns */
-	int32_t time_nanoseconds; 		/**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
-	uint16_t year; 					/**< Year, range 1999..2099 (UTC) */
-	uint8_t month; 					/**< Month, range 1..12 (UTC) */
-	uint8_t day; 					/**< Day of Month, range 1..31 (UTC) */
-	uint8_t hour; 					/**< Hour of Day, range 0..23 (UTC) */
-	uint8_t min; 					/**< Minute of Hour, range 0..59 (UTC) */
-	uint8_t sec; 					/**< Seconds of Minute, range 0..59 (UTC) */
-	uint8_t valid_flag; 			/**< Validity Flags (see ubx documentation) */
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_timeutc_packet_t;
-
-//typedef struct {
-//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
-//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
-//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
-//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
-//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
-//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
-//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
-//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
-//	uint8_t ck_a;
-//	uint8_t ck_b;
-//} gps_bin_nav_dop_packet_t;
-
-typedef struct {
-	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
-	uint8_t numCh; 					/**< Number of channels */
-	uint8_t globalFlags;
-	uint16_t reserved2;
-
-} gps_bin_nav_svinfo_part1_packet_t;
-
-typedef struct {
-	uint8_t chn; 					/**< Channel number, 255 for SVs not assigned to a channel */
-	uint8_t svid; 					/**< Satellite ID */
-	uint8_t flags;
-	uint8_t quality;
-	uint8_t cno; 					/**< Carrier to Noise Ratio (Signal Strength), dbHz */
-	int8_t elev; 					/**< Elevation in integer degrees */
-	int16_t azim; 					/**< Azimuth in integer degrees */
-	int32_t prRes; 					/**< Pseudo range residual in centimetres */
-
-} gps_bin_nav_svinfo_part2_packet_t;
-
-typedef struct {
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_svinfo_part3_packet_t;
-
-typedef struct {
-	uint32_t time_milliseconds; // GPS Millisecond Time of Week
-	int32_t velN; //NED north velocity, cm/s
-	int32_t velE; //NED east velocity, cm/s
-	int32_t velD; //NED down velocity, cm/s
-	uint32_t speed; //Speed (3-D), cm/s
-	uint32_t gSpeed; //Ground Speed (2-D), cm/s
-	int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
-	uint32_t sAcc; //Speed Accuracy Estimate, cm/s
-	uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_velned_packet_t;
-
-//typedef struct {
-//	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
-//	int16_t week; 					/**< Measurement GPS week number */
-//	uint8_t numVis;					/**< Number of visible satellites */
-//
-//	//... rest of package is not used in this implementation
-//
-//} gps_bin_rxm_svsi_packet_t;
-
-typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_ack_ack_packet_t;
-
-typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_ack_nak_packet_t;
-
-typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint8_t portID;
-	uint8_t res0;
-	uint16_t res1;
-	uint32_t mode;
-	uint32_t baudRate;
-	uint16_t inProtoMask;
-	uint16_t outProtoMask;
-	uint16_t flags;
-	uint16_t pad;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_prt_packet_t;
-
-typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint16_t measRate;
-	uint16_t navRate;
-	uint16_t timeRef;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_rate_packet_t;
-
-typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint16_t mask;
-	uint8_t dynModel;
-	uint8_t fixMode;
-	int32_t fixedAlt;
-	uint32_t fixedAltVar;
-	int8_t minElev;
-	uint8_t drLimit;
-	uint16_t pDop;
-	uint16_t tDop;
-	uint16_t pAcc;
-	uint16_t tAcc;
-	uint8_t staticHoldThresh;
-	uint8_t dgpsTimeOut;
-	uint32_t reserved2;
-	uint32_t reserved3;
-	uint32_t reserved4;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_nav5_packet_t;
-
-typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint8_t msgClass_payload;
-	uint8_t msgID_payload;
-	uint8_t rate[6];
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_msg_packet_t;
-
-
-// END the structures of the binary packets
-// ************
-
-typedef enum {
-	UBX_CONFIG_STATE_PRT = 0,
-	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
-	UBX_CONFIG_STATE_RATE,
-	UBX_CONFIG_STATE_NAV5,
-	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
-	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
-	UBX_CONFIG_STATE_MSG_NAV_DOP,
-	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
-	UBX_CONFIG_STATE_MSG_NAV_SOL,
-	UBX_CONFIG_STATE_MSG_NAV_VELNED,
-//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
-	UBX_CONFIG_STATE_CONFIGURED
-} ubx_config_state_t;
-
-typedef enum {
-	CLASS_UNKNOWN = 0,
-	NAV = 1,
-	RXM = 2,
-	ACK = 3,
-	CFG = 4
-} ubx_message_class_t;
-
-typedef enum {
-	//these numbers do NOT correspond to the message id numbers of the ubx protocol
-	ID_UNKNOWN = 0,
-	NAV_POSLLH,
-	NAV_SOL,
-	NAV_TIMEUTC,
-//	NAV_DOP,
-	NAV_SVINFO,
-	NAV_VELNED,
-//	RXM_SVSI,
-	CFG_NAV5,
-	ACK_ACK,
-	ACK_NAK,
-} ubx_message_id_t;
-
-typedef enum {
-	UBX_DECODE_UNINIT = 0,
-	UBX_DECODE_GOT_SYNC1,
-	UBX_DECODE_GOT_SYNC2,
-	UBX_DECODE_GOT_CLASS,
-	UBX_DECODE_GOT_MESSAGEID,
-	UBX_DECODE_GOT_LENGTH1,
-	UBX_DECODE_GOT_LENGTH2
-} ubx_decode_state_t;
-
-//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
-#pragma pack(pop)
-
-#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
-
-class UBX : public GPS_Helper
-{
-public:
-	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
-	~UBX();
-	int					receive(unsigned timeout);
-	int					configure(unsigned &baudrate);
-
-private:
-
-	/**
-	 * Parse the binary MTK packet
-	 */
-	int					parse_char(uint8_t b);
-
-	/**
-	 * Handle the package once it has arrived
-	 */
-	int					handle_message(void);
-
-	/**
-	 * Reset the parse state machine for a fresh start
-	 */
-	void				decode_init(void);
-
-	/**
-	 * While parsing add every byte (except the sync bytes) to the checksum
-	 */
-	void				add_byte_to_checksum(uint8_t);
-
-	/**
-	 * Add the two checksum bytes to an outgoing message
-	 */
-	void				add_checksum_to_message(uint8_t* message, const unsigned length);
-
-	/**
-	 * Helper to send a config packet
-	 */
-	void				send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
-
-	int 				_fd;
-	struct vehicle_gps_position_s *_gps_position;
-	ubx_config_state_t	_config_state;
-	bool 				_waiting_for_ack;
-	uint8_t				_clsID_needed;
-	uint8_t				_msgID_needed;
-	ubx_decode_state_t	_decode_state;
-	uint8_t				_rx_buffer[RECV_BUFFER_SIZE];
-	unsigned			_rx_count;
-	uint8_t 			_rx_ck_a;
-	uint8_t				_rx_ck_b;
-	ubx_message_class_t _message_class;
-	ubx_message_id_t	_message_id;
-	unsigned			_payload_size;
-};
-
-#endif /* UBX_H_ */
diff --git a/apps/drivers/hil/Makefile b/apps/drivers/hil/Makefile
deleted file mode 100644
index 1fb6e37bc..000000000
--- a/apps/drivers/hil/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Interface driver for the PX4FMU board
-#
-
-APPNAME		 = hil
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
deleted file mode 100644
index d9aa772d4..000000000
--- a/apps/drivers/hil/hil.cpp
+++ /dev/null
@@ -1,851 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file hil.cpp
- *
- * Driver/configurator for the virtual HIL port.
- *
- * This virtual driver emulates PWM / servo outputs for setups where
- * the connected hardware does not provide enough or no PWM outputs.
- *
- * Its only function is to take actuator_control uORB messages,
- * mix them with any loaded mixer and output the result to the
- * actuator_output uORB topic. HIL can also be performed with normal
- * PWM outputs, a special flag prevents the outputs to be operated
- * during HIL mode. If HIL is not performed with a standalone FMU,
- * but in a real system, it is NOT recommended to use this virtual
- * driver. Use instead the normal FMU or IO driver.
- */
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-
-#include <drivers/device/device.h>
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_mixer.h>
-
-#include <systemlib/systemlib.h>
-#include <systemlib/mixer/mixer.h>
-
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_outputs.h>
-
-#include <systemlib/err.h>
-
-class HIL : public device::CDev
-{
-public:
-	enum Mode {
-		MODE_2PWM,
-		MODE_4PWM,
-        MODE_8PWM,
-        MODE_12PWM,
-        MODE_16PWM,
-		MODE_NONE
-	};
-	HIL();
-	virtual ~HIL();
-
-	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
-
-	virtual int	init();
-
-	int		set_mode(Mode mode);
-	int		set_pwm_rate(unsigned rate);
-
-private:
-	static const unsigned _max_actuators = 4;
-
-	Mode		_mode;
-	int 		_update_rate;
-	int 		_current_update_rate;
-	int		_task;
-	int		_t_actuators;
-	int		_t_armed;
-	orb_advert_t	_t_outputs;
-	unsigned	_num_outputs;
-	bool		_primary_pwm_device;
-
-	volatile bool	_task_should_exit;
-	bool		_armed;
-
-	MixerGroup	*_mixers;
-
-	actuator_controls_s _controls;
-
-	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
-
-	static int	control_callback(uintptr_t handle,
-			uint8_t control_group,
-			uint8_t control_index,
-			float &input);
-
-	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
-
-	struct GPIOConfig {
-		uint32_t	input;
-		uint32_t	output;
-		uint32_t	alt;
-	};
-
-	static const GPIOConfig	_gpio_tab[];
-	static const unsigned	_ngpio;
-
-	void		gpio_reset(void);
-	void		gpio_set_function(uint32_t gpios, int function);
-	void		gpio_write(uint32_t gpios, int function);
-	uint32_t	gpio_read(void);
-	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
-
-};
-
-namespace
-{
-
-HIL	*g_hil;
-
-} // namespace
-
-HIL::HIL() :
-	CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
-	_mode(MODE_NONE),
-	_update_rate(50),
-	_current_update_rate(0),
-	_task(-1),
-	_t_actuators(-1),
-	_t_armed(-1),
-	_t_outputs(0),
-	_num_outputs(0),
-	_primary_pwm_device(false),
-	_task_should_exit(false),
-	_armed(false),
-	_mixers(nullptr)
-{
-	_debug_enabled = true;
-}
-
-HIL::~HIL()
-{
-	if (_task != -1) {
-		/* tell the task we want it to go away */
-		_task_should_exit = true;
-
-		unsigned i = 10;
-		do {
-			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(50000);
-
-			/* if we have given up, kill it */
-			if (--i == 0) {
-				task_delete(_task);
-				break;
-			}
-
-		} while (_task != -1);
-	}
-
-	/* clean up the alternate device node */
-	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
-
-	g_hil = nullptr;
-}
-
-int
-HIL::init()
-{
-	int ret;
-
-	ASSERT(_task == -1);
-
-	/* do regular cdev init */
-	ret = CDev::init();
-
-	if (ret != OK)
-		return ret;
-
-	// XXX already claimed with CDEV
-	///* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	//ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
-	if (ret == OK) {
-		log("default PWM output device");
-		_primary_pwm_device = true;
-	}
-
-	/* reset GPIOs */
-	// gpio_reset();
-
-	/* start the HIL interface task */
-	_task = task_spawn("fmuhil",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&HIL::task_main_trampoline,
-			   nullptr);
-
-	if (_task < 0) {
-		debug("task start failed: %d", errno);
-		return -errno;
-	}
-
-	return OK;
-}
-
-void
-HIL::task_main_trampoline(int argc, char *argv[])
-{
-	g_hil->task_main();
-}
-
-int
-HIL::set_mode(Mode mode)
-{
-	/* 
-	 * Configure for PWM output.
-	 *
-	 * Note that regardless of the configured mode, the task is always
-	 * listening and mixing; the mode just selects which of the channels
-	 * are presented on the output pins.
-	 */
-	switch (mode) {
-	case MODE_2PWM:
-		debug("MODE_2PWM");
-		/* multi-port with flow control lines as PWM */
-		_update_rate = 50;	/* default output rate */
-		break;
-
-	case MODE_4PWM:
-		debug("MODE_4PWM");
-		/* multi-port as 4 PWM outs */
-		_update_rate = 50;	/* default output rate */
-		break;
-            
-    	case MODE_8PWM:
-            debug("MODE_8PWM");
-            /* multi-port as 8 PWM outs */
-            _update_rate = 50;	/* default output rate */
-            break;
-            
-        case MODE_12PWM:
-            debug("MODE_12PWM");
-            /* multi-port as 12 PWM outs */
-            _update_rate = 50;	/* default output rate */
-            break;
-            
-        case MODE_16PWM:
-            debug("MODE_16PWM");
-            /* multi-port as 16 PWM outs */
-            _update_rate = 50;	/* default output rate */
-            break;
-
-	case MODE_NONE:
-		debug("MODE_NONE");
-		/* disable servo outputs and set a very low update rate */
-		_update_rate = 10;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-	_mode = mode;
-	return OK;
-}
-
-int
-HIL::set_pwm_rate(unsigned rate)
-{
-	if ((rate > 500) || (rate < 10))
-		return -EINVAL;
-
-	_update_rate = rate;
-	return OK;
-}
-
-void
-HIL::task_main()
-{
-	/*
-	 * Subscribe to the appropriate PWM output topic based on whether we are the
-	 * primary PWM output or not.
-	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
-	/* force a reset of the update rate */
-	_current_update_rate = 0;
-
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
-
-	/* advertise the mixed control outputs */
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
-	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &outputs);
-
-	pollfd fds[2];
-	fds[0].fd = _t_actuators;
-	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
-	fds[1].events = POLLIN;
-
-	unsigned num_outputs;
-
-	/* select the number of virtual outputs */
-	switch (_mode) {
-	case MODE_2PWM:
-		num_outputs = 2;
-		break;
-
-	case MODE_4PWM:
-		num_outputs = 4;
-		break;
-
-	case MODE_8PWM:
-	case MODE_12PWM:
-	case MODE_16PWM:
-		// XXX only support the lower 8 - trivial to extend
-		num_outputs = 8;
-		break;
-
-	case MODE_NONE:
-	default:
-		num_outputs = 0;
-		break;
-	}
-
-	log("starting");
-
-	/* loop until killed */
-	while (!_task_should_exit) {
-
-		/* handle update rate changes */
-		if (_current_update_rate != _update_rate) {
-			int update_rate_in_ms = int(1000 / _update_rate);
-			if (update_rate_in_ms < 2)
-				update_rate_in_ms = 2;
-			orb_set_interval(_t_actuators, update_rate_in_ms);
-			// up_pwm_servo_set_rate(_update_rate);
-			_current_update_rate = _update_rate;
-		}
-
-		/* sleep waiting for data, but no more than a second */
-		int ret = ::poll(&fds[0], 2, 1000);
-
-		/* this would be bad... */
-		if (ret < 0) {
-			log("poll error %d", errno);
-			continue;
-		}
-
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
-			/* can we mix? */
-			if (_mixers != nullptr) {
-
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-				outputs.timestamp = hrt_absolute_time();
-
-				/* iterate actuators */
-				for (unsigned i = 0; i < num_outputs; i++) {
-
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < (unsigned)outputs.noutputs &&
-						isfinite(outputs.output[i]) &&
-						outputs.output[i] >= -1.0f &&
-						outputs.output[i] <= 1.0f) {
-						/* scale for PWM output 900 - 2100us */
-						outputs.output[i] = 1500 + (600 * outputs.output[i]);
-					} else {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = 900;
-					}
-				}
-
-				/* and publish for anyone that cares to see */
-				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
-			}
-		}
-
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
-
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
-		}
-	}
-
-	::close(_t_actuators);
-	::close(_t_armed);
-
-	/* make sure servos are off */
-	// up_pwm_servo_deinit();
-
-	log("stopping");
-
-	/* note - someone else is responsible for restoring the GPIO config */
-
-	/* tell the dtor that we are exiting */
-	_task = -1;
-	_exit(0);
-}
-
-int
-HIL::control_callback(uintptr_t handle,
-				      uint8_t control_group,
-				      uint8_t control_index,
-				      float &input)
-{
-	const actuator_controls_s *controls = (actuator_controls_s *)handle;
-
-	input = controls->control[control_index];
-	return 0;
-}
-
-int
-HIL::ioctl(file *filp, int cmd, unsigned long arg)
-{
-	int ret;
-
-	debug("ioctl 0x%04x 0x%08x", cmd, arg);
-
-	// /* try it as a GPIO ioctl first */
-	// ret = HIL::gpio_ioctl(filp, cmd, arg);
-	// if (ret != -ENOTTY)
-	// 	return ret;
-
-	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
-	switch(_mode) {
-	case MODE_2PWM:
-	case MODE_4PWM:
-        case MODE_8PWM:
-        case MODE_12PWM:
-        case MODE_16PWM:
-		ret = HIL::pwm_ioctl(filp, cmd, arg);
-		break;
-	default:
-		ret = -ENOTTY;
-		debug("not in a PWM mode");
-		break;
-	}
-
-	/* if nobody wants it, let CDev have it */
-	if (ret == -ENOTTY)
-		ret = CDev::ioctl(filp, cmd, arg);
-
-	return ret;
-}
-
-int
-HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
-{
-	int ret = OK;
-	// int channel;
-
-	lock();
-
-	switch (cmd) {
-	case PWM_SERVO_ARM:
-		// up_pwm_servo_arm(true);
-		break;
-
-	case PWM_SERVO_DISARM:
-		// up_pwm_servo_arm(false);
-		break;
-
-	case PWM_SERVO_SET_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
-		g_hil->set_pwm_rate(arg);
-		break;
-
-	case PWM_SERVO_SELECT_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
-		break;
-
-	case PWM_SERVO_SET(2):
-	case PWM_SERVO_SET(3):
-		if (_mode != MODE_4PWM) {
-			ret = -EINVAL;
-			break;
-		}
-
-		/* FALLTHROUGH */
-	case PWM_SERVO_SET(0):
-	case PWM_SERVO_SET(1):
-		if (arg < 2100) {
-			// channel = cmd - PWM_SERVO_SET(0);
-//			up_pwm_servo_set(channel, arg); XXX
-
-		} else {
-			ret = -EINVAL;
-		}
-
-		break;
-
-	case PWM_SERVO_GET(2):
-	case PWM_SERVO_GET(3):
-		if (_mode != MODE_4PWM) {
-			ret = -EINVAL;
-			break;
-		}
-
-		/* FALLTHROUGH */
-	case PWM_SERVO_GET(0):
-	case PWM_SERVO_GET(1): {
-			// channel = cmd - PWM_SERVO_SET(0);
-			// *(servo_position_t *)arg = up_pwm_servo_get(channel);
-			break;
-		}
-
-	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
-		// no restrictions on output grouping
-		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
-
-		*(uint32_t *)arg = (1 << channel);
-		break;
-	}
-
-	case MIXERIOCGETOUTPUTCOUNT:
-		if (_mode == MODE_4PWM) {
-			*(unsigned *)arg = 4;
-
-		} else {
-			*(unsigned *)arg = 2;
-		}
-
-		break;
-
-	case MIXERIOCRESET:
-		if (_mixers != nullptr) {
-			delete _mixers;
-			_mixers = nullptr;
-		}
-
-		break;
-
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
-
-			if (mixer->check()) {
-				delete mixer;
-				ret = -EINVAL;
-
-			} else {
-				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
-
-				_mixers->add_mixer(mixer);
-			}
-
-			break;
-		}
-
-	case MIXERIOCLOADBUF: {
-			const char *buf = (const char *)arg;
-			unsigned buflen = strnlen(buf, 1024);
-
-			if (_mixers == nullptr)
-				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (_mixers == nullptr) {
-				ret = -ENOMEM;
-
-			} else {
-
-				ret = _mixers->load_from_buf(buf, buflen);
-
-				if (ret != 0) {
-					debug("mixer load failed with %d", ret);
-					delete _mixers;
-					_mixers = nullptr;
-					ret = -EINVAL;
-				}
-			}
-			break;
-		}
-
-
-	default:
-		ret = -ENOTTY;
-		break;
-	}
-
-	unlock();
-
-	return ret;
-}
-
-namespace
-{
-
-enum PortMode {
-	PORT_MODE_UNDEFINED = 0,
-	PORT1_MODE_UNSET,
-	PORT1_FULL_PWM,
-	PORT1_PWM_AND_SERIAL,
-	PORT1_PWM_AND_GPIO,
-	PORT2_MODE_UNSET,
-	PORT2_8PWM,
-	PORT2_12PWM,
-	PORT2_16PWM,
-};
-
-PortMode g_port_mode;
-
-int
-hil_new_mode(PortMode new_mode)
-{
-	// uint32_t gpio_bits;
-	
-
-//	/* reset to all-inputs */
-//	g_hil->ioctl(0, GPIO_RESET, 0);
-
-	// gpio_bits = 0;
-
-	HIL::Mode servo_mode = HIL::MODE_NONE;
-
-	switch (new_mode) {
-	case PORT_MODE_UNDEFINED:
-	case PORT1_MODE_UNSET:
-    	case PORT2_MODE_UNSET:
-            /* nothing more to do here */
-            break;
-
-	case PORT1_FULL_PWM:
-		/* select 4-pin PWM mode */
-		servo_mode = HIL::MODE_4PWM;
-		break;
-
-	case PORT1_PWM_AND_SERIAL:
-		/* select 2-pin PWM mode */
-		servo_mode = HIL::MODE_2PWM;
-//		/* set RX/TX multi-GPIOs to serial mode */
-//		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT1_PWM_AND_GPIO:
-		/* select 2-pin PWM mode */
-		servo_mode = HIL::MODE_2PWM;
-		break;
-            
-        case PORT2_8PWM:
-            /* select 8-pin PWM mode */
-            servo_mode = HIL::MODE_8PWM;
-            break;
-            
-        case PORT2_12PWM:
-            /* select 12-pin PWM mode */
-            servo_mode = HIL::MODE_12PWM;
-            break;
-            
-        case PORT2_16PWM:
-            /* select 16-pin PWM mode */
-            servo_mode = HIL::MODE_16PWM;
-            break;
-	}
-
-//	/* adjust GPIO config for serial mode(s) */
-//	if (gpio_bits != 0)
-//		g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
-
-	/* (re)set the PWM output mode */
-	g_hil->set_mode(servo_mode);
-
-	return OK;
-}
-
-int
-hil_start(void)
-{
-	int ret = OK;
-
-	if (g_hil == nullptr) {
-
-		g_hil = new HIL;
-
-		if (g_hil == nullptr) {
-			ret = -ENOMEM;
-
-		} else {
-			ret = g_hil->init();
-
-			if (ret != OK) {
-				delete g_hil;
-				g_hil = nullptr;
-			}
-		}
-	}
-
-	return ret;
-}
-
-void
-test(void)
-{
-	int	fd;
-
-	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
-
-	if (fd < 0) {
-		puts("open fail");
-		exit(1);
-	}
-
-	ioctl(fd, PWM_SERVO_ARM, 0);
-	ioctl(fd, PWM_SERVO_SET(0), 1000);
-
-	close(fd);
-
-	exit(0);
-}
-
-void
-fake(int argc, char *argv[])
-{
-	if (argc < 5) {
-		puts("hil fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
-		exit(1);
-	}
-
-	actuator_controls_s ac;
-
-	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
-
-	ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
-
-	ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
-
-	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
-
-	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
-
-	if (handle < 0) {
-		puts("advertise failed");
-		exit(1);
-	}
-
-	exit(0);
-}
-
-} // namespace
-
-extern "C" __EXPORT int hil_main(int argc, char *argv[]);
-
-int
-hil_main(int argc, char *argv[])
-{
-	PortMode new_mode = PORT_MODE_UNDEFINED;
-	const char *verb = argv[1];
-
-	if (hil_start() != OK)
-		errx(1, "failed to start the HIL driver");
-
-	/*
-	 * Mode switches.
-	 */
-
- 	// this was all cut-and-pasted from the FMU driver; it's junk
-	if (!strcmp(verb, "mode_pwm")) {
-		new_mode = PORT1_FULL_PWM;
-
-	} else if (!strcmp(verb, "mode_pwm_serial")) {
-		new_mode = PORT1_PWM_AND_SERIAL;
-
-	} else if (!strcmp(verb, "mode_pwm_gpio")) {
-		new_mode = PORT1_PWM_AND_GPIO;
-
-	} else if (!strcmp(verb, "mode_port2_pwm8")) {
-		new_mode = PORT2_8PWM;
-
-	} else if (!strcmp(verb, "mode_port2_pwm12")) {
-		new_mode = PORT2_8PWM;
-
-	} else if (!strcmp(verb, "mode_port2_pwm16")) {
-		new_mode = PORT2_8PWM;
-	}
-
-	/* was a new mode set? */
-	if (new_mode != PORT_MODE_UNDEFINED) {
-
-		/* yes but it's the same mode */
-		if (new_mode == g_port_mode)
-			return OK;
-
-		/* switch modes */
-		return hil_new_mode(new_mode);
-	}
-
-	if (!strcmp(verb, "test"))
-		test();
-
-	if (!strcmp(verb, "fake"))
-		fake(argc - 1, argv + 1);
-
-
-	fprintf(stderr, "HIL: unrecognized command, try:\n");
-	fprintf(stderr, "  mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
-	return -EINVAL;
-}
diff --git a/apps/drivers/hmc5883/Makefile b/apps/drivers/hmc5883/Makefile
deleted file mode 100644
index 4d7cb4e7b..000000000
--- a/apps/drivers/hmc5883/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# HMC5883 driver
-#
-
-APPNAME		 = hmc5883
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
deleted file mode 100644
index 8ab568282..000000000
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ /dev/null
@@ -1,1447 +0,0 @@
-/****************************************************************************
- *
- *   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 hmc5883.cpp
- *
- * Driver for the HMC5883 magnetometer connected via I2C.
- */
-
-#include <nuttx/config.h>
-
-#include <drivers/device/i2c.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
-
-#include <arch/board/board.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#include <drivers/drv_mag.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
-
-#include <float.h>
-
-/*
- * HMC5883 internal constants and data structures.
- */
-
-#define HMC5883L_BUS			PX4_I2C_BUS_ONBOARD
-#define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
-
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
-
-#define ADDR_CONF_A			0x00
-#define ADDR_CONF_B			0x01
-#define ADDR_MODE			0x02
-#define ADDR_DATA_OUT_X_MSB		0x03
-#define ADDR_DATA_OUT_X_LSB		0x04
-#define ADDR_DATA_OUT_Z_MSB		0x05
-#define ADDR_DATA_OUT_Z_LSB		0x06
-#define ADDR_DATA_OUT_Y_MSB		0x07
-#define ADDR_DATA_OUT_Y_LSB		0x08
-#define ADDR_STATUS			0x09
-#define ADDR_ID_A			0x0a
-#define ADDR_ID_B			0x0b
-#define ADDR_ID_C			0x0c
-
-/* modes not changeable outside of driver */
-#define HMC5883L_MODE_NORMAL		(0 << 0)  /* default */
-#define HMC5883L_MODE_POSITIVE_BIAS	(1 << 0)  /* positive bias */
-#define HMC5883L_MODE_NEGATIVE_BIAS	(1 << 1)  /* negative bias */
-
-#define HMC5883L_AVERAGING_1		(0 << 5) /* conf a register */
-#define HMC5883L_AVERAGING_2		(1 << 5)
-#define HMC5883L_AVERAGING_4		(2 << 5)
-#define HMC5883L_AVERAGING_8		(3 << 5)
-
-#define MODE_REG_CONTINOUS_MODE		(0 << 0)
-#define MODE_REG_SINGLE_MODE		(1 << 0) /* default */
-
-#define STATUS_REG_DATA_OUT_LOCK	(1 << 1) /* page 16: set if data is only partially read, read device to reset */
-#define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
-
-#define ID_A_WHO_AM_I			'H'
-#define ID_B_WHO_AM_I			'4'
-#define ID_C_WHO_AM_I			'3'
-
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-class HMC5883 : public device::I2C
-{
-public:
-	HMC5883(int bus);
-	virtual ~HMC5883();
-
-	virtual int		init();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void			print_info();
-
-protected:
-	virtual int		probe();
-
-private:
-	work_s			_work;
-	unsigned		_measure_ticks;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	mag_report		*_reports;
-	mag_scale		_scale;
-	float 			_range_scale;
-	float 			_range_ga;
-	bool			_collect_phase;
-
-	orb_advert_t		_mag_topic;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_comms_errors;
-	perf_counter_t		_buffer_overflows;
-
-	/* status reporting */
-	bool			_sensor_ok;		/**< sensor was found and reports ok */
-	bool			_calibrated;		/**< the calibration is valid */
-
-	/**
-	 * Test whether the device supported by the driver is present at a
-	 * specific address.
-	 *
-	 * @param address	The I2C bus address to probe.
-	 * @return		True if the device is present.
-	 */
-	int			probe_address(uint8_t address);
-
-	/**
-	 * Initialise the automatic measurement state machine and start it.
-	 *
-	 * @note This function is called at open and error time.  It might make sense
-	 *       to make it more aggressive about resetting the bus in case of errors.
-	 */
-	void			start();
-
-	/**
-	 * Stop the automatic measurement state machine.
-	 */
-	void			stop();
-
-	/**
-	 * Perform the on-sensor scale calibration routine.
-	 *
-	 * @note The sensor will continue to provide measurements, these
-	 *	 will however reflect the uncalibrated sensor state until
-	 *	 the calibration routine has been completed.
-	 *
-	 * @param enable set to 1 to enable self-test strap, 0 to disable
-	 */
-	int			calibrate(struct file *filp, unsigned enable);
-
-	/**
-	 * Perform the on-sensor scale calibration routine.
-	 *
-	 * @note The sensor will continue to provide measurements, these
-	 *	 will however reflect the uncalibrated sensor state until
-	 *	 the calibration routine has been completed.
-	 *
-	 * @param enable set to 1 to enable self-test positive strap, -1 to enable
-	 *        negative strap, 0 to set to normal mode
-	 */
-	int			set_excitement(unsigned enable);
-
-	/**
-	 * Set the sensor range.
-	 *
-	 * Sets the internal range to handle at least the argument in Gauss.
-	 */
-	int 			set_range(unsigned range);
-
-	/**
-	 * Perform a poll cycle; collect from the previous measurement
-	 * and start a new one.
-	 *
-	 * This is the heart of the measurement state machine.  This function
-	 * alternately starts a measurement, or collects the data from the
-	 * previous measurement.
-	 *
-	 * When the interval between measurements is greater than the minimum
-	 * measurement interval, a gap is inserted between collection
-	 * and measurement to provide the most recent measurement possible
-	 * at the next interval.
-	 */
-	void			cycle();
-
-	/**
-	 * Static trampoline from the workq context; because we don't have a
-	 * generic workq wrapper yet.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		cycle_trampoline(void *arg);
-
-	/**
-	 * Write a register.
-	 *
-	 * @param reg		The register to write.
-	 * @param val		The value to write.
-	 * @return		OK on write success.
-	 */
-	int			write_reg(uint8_t reg, uint8_t val);
-
-	/**
-	 * Read a register.
-	 *
-	 * @param reg		The register to read.
-	 * @param val		The value read.
-	 * @return		OK on read success.
-	 */
-	int			read_reg(uint8_t reg, uint8_t &val);
-
-	/**
-	 * Issue a measurement command.
-	 *
-	 * @return		OK if the measurement command was successful.
-	 */
-	int			measure();
-
-	/**
-	 * Collect the result of the most recent measurement.
-	 */
-	int			collect();
-
-	/**
-	 * Convert a big-endian signed 16-bit value to a float.
-	 *
-	 * @param in		A signed 16-bit big-endian value.
-	 * @return		The floating-point representation of the value.
-	 */
-	float			meas_to_float(uint8_t in[2]);
-
-	/**
-	 * Check the current calibration and update device status
-	 *
-	 * @return 0 if calibration is ok, 1 else
-	 */
-	 int 			check_calibration();
-
-	 /**
-	 * Check the current scale calibration
-	 *
-	 * @return 0 if scale calibration is ok, 1 else
-	 */
-	 int 			check_scale();
-
-	 /**
-	 * Check the current offset calibration
-	 *
-	 * @return 0 if offset calibration is ok, 1 else
-	 */
-	 int 			check_offset();
-
-};
-
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
-
-
-HMC5883::HMC5883(int bus) :
-	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
-	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_range_scale(0), /* default range scale from counts to gauss */
-	_range_ga(1.3f),
-	_mag_topic(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
-	_sensor_ok(false),
-	_calibrated(false)
-{
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// default scaling
-	_scale.x_offset = 0;
-	_scale.x_scale = 1.0f;
-	_scale.y_offset = 0;
-	_scale.y_scale = 1.0f;
-	_scale.z_offset = 0;
-	_scale.z_scale = 1.0f;
-
-	// work_cancel in the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
-}
-
-HMC5883::~HMC5883()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-}
-
-int
-HMC5883::init()
-{
-	int ret = ERROR;
-
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
-		goto out;
-
-	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct mag_report[_num_reports];
-
-	if (_reports == nullptr)
-		goto out;
-
-	_oldest_report = _next_report = 0;
-
-	/* get a publish handle on the mag topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
-
-	if (_mag_topic < 0)
-		debug("failed to create sensor_mag object");
-
-	/* set range */
-	set_range(_range_ga);
-
-	ret = OK;
-	/* sensor is ok, but not calibrated */
-	_sensor_ok = true;
-out:
-	return ret;
-}
-
-int HMC5883::set_range(unsigned range)
-{
-	uint8_t range_bits;
-
-	if (range < 1) {
-		range_bits = 0x00;
-		_range_scale = 1.0f / 1370.0f;
-		_range_ga = 0.88f;
-
-	} else if (range <= 1) {
-		range_bits = 0x01;
-		_range_scale = 1.0f / 1090.0f;
-		_range_ga = 1.3f;
-
-	} else if (range <= 2) {
-		range_bits = 0x02;
-		_range_scale = 1.0f / 820.0f;
-		_range_ga = 1.9f;
-
-	} else if (range <= 3) {
-		range_bits = 0x03;
-		_range_scale = 1.0f / 660.0f;
-		_range_ga = 2.5f;
-
-	} else if (range <= 4) {
-		range_bits = 0x04;
-		_range_scale = 1.0f / 440.0f;
-		_range_ga = 4.0f;
-
-	} else if (range <= 4.7f) {
-		range_bits = 0x05;
-		_range_scale = 1.0f / 390.0f;
-		_range_ga = 4.7f;
-
-	} else if (range <= 5.6f) {
-		range_bits = 0x06;
-		_range_scale = 1.0f / 330.0f;
-		_range_ga = 5.6f;
-
-	} else {
-		range_bits = 0x07;
-		_range_scale = 1.0f / 230.0f;
-		_range_ga = 8.1f;
-	}
-
-	int ret;
-
-	/*
-	 * Send the command to set the range
-	 */
-	ret = write_reg(ADDR_CONF_B, (range_bits << 5));
-
-	if (OK != ret)
-		perf_count(_comms_errors);
-
-	uint8_t range_bits_in;
-	ret = read_reg(ADDR_CONF_B, range_bits_in);
-
-	if (OK != ret)
-		perf_count(_comms_errors);
-
-	return !(range_bits_in == (range_bits << 5));
-}
-
-int
-HMC5883::probe()
-{
-	uint8_t data[3] = {0, 0, 0};
-
-	_retries = 10;
-
-	if (read_reg(ADDR_ID_A, data[0]) ||
-	    read_reg(ADDR_ID_B, data[1]) ||
-	    read_reg(ADDR_ID_C, data[2]))
-		debug("read_reg fail");
-
-	_retries = 2;
-
-	if ((data[0] != ID_A_WHO_AM_I) ||
-	    (data[1] != ID_B_WHO_AM_I) ||
-	    (data[2] != ID_C_WHO_AM_I)) {
-		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
-		return -EIO;
-	}
-
-	return OK;
-}
-
-ssize_t
-HMC5883::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct mag_report);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1)
-		return -ENOSPC;
-
-	/* if automatic measurement is enabled */
-	if (_measure_ticks > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the workq thread while we are doing this;
-		 * we are careful to avoid racing with them.
-		 */
-		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
-	do {
-		_oldest_report = _next_report = 0;
-
-		/* trigger a measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* wait for it to complete */
-		usleep(HMC5883_CONVERSION_INTERVAL);
-
-		/* run the collection phase */
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
-	} while (0);
-
-	return ret;
-}
-
-int
-HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
-				return OK;
-
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
-
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
-			return SENSOR_POLLRATE_MANUAL;
-
-		return (1000 / _measure_ticks);
-
-	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct mag_report *buf = new struct mag_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
-
-	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
-
-	case MAGIOCSSAMPLERATE:
-		/* not supported, always 1 sample per poll */
-		return -EINVAL;
-
-	case MAGIOCSRANGE:
-		return set_range(arg);
-
-	case MAGIOCSLOWPASS:
-		/* not supported, no internal filtering */
-		return -EINVAL;
-
-	case MAGIOCSSCALE:
-		/* set new scale factors */
-		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
-		/* check calibration, but not actually return an error */
-		(void)check_calibration();
-		return 0;
-
-	case MAGIOCGSCALE:
-		/* copy out scale factors */
-		memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
-		return 0;
-
-	case MAGIOCCALIBRATE:
-		return calibrate(filp, arg);
-
-	case MAGIOCEXSTRAP:
-		return set_excitement(arg);
-
-	case MAGIOCSELFTEST:
-		return check_calibration();
-
-	default:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
-	}
-}
-
-void
-HMC5883::start()
-{
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_oldest_report = _next_report = 0;
-
-	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
-}
-
-void
-HMC5883::stop()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-HMC5883::cycle_trampoline(void *arg)
-{
-	HMC5883 *dev = (HMC5883 *)arg;
-
-	dev->cycle();
-}
-
-void
-HMC5883::cycle()
-{
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		if (OK != collect()) {
-			log("collection error");
-			/* restart the measurement state machine */
-			start();
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
-				   &_work,
-				   (worker_t)&HMC5883::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL));
-
-			return;
-		}
-	}
-
-	/* measurement phase */
-	if (OK != measure())
-		log("measure error");
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&HMC5883::cycle_trampoline,
-		   this,
-		   USEC2TICK(HMC5883_CONVERSION_INTERVAL));
-}
-
-int
-HMC5883::measure()
-{
-	int ret;
-
-	/*
-	 * Send the command to begin a measurement.
-	 */
-	ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
-
-	if (OK != ret)
-		perf_count(_comms_errors);
-
-	return ret;
-}
-
-int
-HMC5883::collect()
-{
-#pragma pack(push, 1)
-	struct { /* status register and data as read back from the device */
-		uint8_t		x[2];
-		uint8_t		z[2];
-		uint8_t		y[2];
-	}	hmc_report;
-#pragma pack(pop)
-	struct {
-		int16_t		x, y, z;
-	} report;
-	int	ret = -EIO;
-	uint8_t	cmd;
-
-
-	perf_begin(_sample_perf);
-
-	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-
-	/*
-	 * @note  We could read the status register here, which could tell us that
-	 *        we were too early and that the output registers are still being
-	 *        written.  In the common case that would just slow us down, and
-	 *        we're better off just never being early.
-	 */
-
-	/* get measurements from the device */
-	cmd = ADDR_DATA_OUT_X_MSB;
-	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
-
-	if (ret != OK) {
-		perf_count(_comms_errors);
-		debug("data/status read error");
-		goto out;
-	}
-
-	/* swap the data we just received */
-	report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
-	report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
-	report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
-
-	/*
-	 * If any of the values are -4096, there was an internal math error in the sensor.
-	 * Generalise this to a simple range check that will also catch some bit errors.
-	 */
-	if ((abs(report.x) > 2048) ||
-	    (abs(report.y) > 2048) ||
-	    (abs(report.z) > 2048))
-		goto out;
-
-	/*
-	 * RAW outputs
-	 *
-	 * to align the sensor axes with the board, x and y need to be flipped
-	 * and y needs to be negated
-	 */
-	_reports[_next_report].x_raw = report.y;
-	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
-	/* z remains z */
-	_reports[_next_report].z_raw = report.z;
-
-	/* scale values for output */
-
-	/*
-	 * 1) Scale raw value to SI units using scaling from datasheet.
-	 * 2) Subtract static offset (in SI units)
-	 * 3) Scale the statically calibrated values with a linear
-	 *    dynamically obtained factor
-	 *
-	 * Note: the static sensor offset is the number the sensor outputs
-	 * 	 at a nominally 'zero' input. Therefore the offset has to
-	 * 	 be subtracted.
-	 *
-	 *	 Example: A gyro outputs a value of 74 at zero angular rate
-	 *	 	  the offset is 74 from the origin and subtracting
-	 *		  74 from all measurements centers them around zero.
-	 */
-
-	/* to align the sensor axes with the board, x and y need to be flipped */
-	_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
-	/* flip axes and negate value for y */
-	_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
-	/* z remains z */
-	_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
-
-	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
-
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
-	}
-
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-
-	ret = OK;
-
-out:
-	perf_end(_sample_perf);
-	return ret;
-}
-
-int HMC5883::calibrate(struct file *filp, unsigned enable)
-{
-	struct mag_report report;
-	ssize_t sz;
-	int ret = 1;
-
-	// XXX do something smarter here
-	int fd = (int)enable;
-
-	struct mag_scale mscale_previous = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
-	unsigned i;
-
-	warnx("starting mag scale calibration");
-
-	/* do a simple demand read */
-	sz = read(filp, (char *)&report, sizeof(report));
-
-	if (sz != sizeof(report)) {
-		warn("immediate read failed");
-		ret = 1;
-		goto out;
-	}
-
-	warnx("current measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	warnx("time:        %lld", report.timestamp);
-	warnx("sampling 500 samples for scaling offset");
-
-	/* set the queue depth to 10 */
-	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
-		warn("failed to set queue depth");
-		ret = 1;
-		goto out;
-	}
-
-	/* start the sensor polling at 50 Hz */
-	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
-		warn("failed to set 2Hz poll rate");
-		ret = 1;
-		goto out;
-	}
-
-	/* Set to 2.5 Gauss */
-	if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
-		warnx("failed to set 2.5 Ga range");
-		ret = 1;
-		goto out;
-	}
-
-	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
-		warnx("failed to enable sensor calibration mode");
-		ret = 1;
-		goto out;
-	}
-
-	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to get scale / offsets for mag");
-		ret = 1;
-		goto out;
-	}
-
-	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set null scale / offsets for mag");
-		ret = 1;
-		goto out;
-	}
-
-	/* read the sensor 10x and report each value */
-	for (i = 0; i < 500; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = ::poll(&fds, 1, 2000);
-
-		if (ret != 1) {
-			warn("timed out waiting for sensor data");
-			goto out;
-		}
-
-		/* now go get it */
-		sz = ::read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			warn("periodic read failed");
-			goto out;
-
-		} else {
-			avg_excited[0] += report.x;
-			avg_excited[1] += report.y;
-			avg_excited[2] += report.z;
-		}
-
-		//warnx("periodic read %u", i);
-		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	}
-
-	avg_excited[0] /= i;
-	avg_excited[1] /= i;
-	avg_excited[2] /= i;
-
-	warnx("done. Performed %u reads", i);
-	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
-
-	float scaling[3];
-
-	/* calculate axis scaling */
-	scaling[0] = fabsf(1.16f / avg_excited[0]);
-	/* second axis inverted */
-	scaling[1] = fabsf(1.16f / -avg_excited[1]);
-	scaling[2] = fabsf(1.08f / avg_excited[2]);
-
-	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-
-	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
-		goto out;
-	}
-
-	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
-		goto out;
-	}
-
-	/* set scaling in device */
-	mscale_previous.x_scale = scaling[0];
-	mscale_previous.y_scale = scaling[1];
-	mscale_previous.z_scale = scaling[2];
-
-	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to set new scale / offsets for mag");
-		goto out;
-	}
-
-	ret = OK;
-
-out:
-
-	if (ret == OK) {
-		if (!check_scale()) {
-			warnx("mag scale calibration successfully finished.");
-		} else {
-			warnx("mag scale calibration finished with invalid results.");
-			ret = ERROR;
-		}
-
-	} else {
-		warnx("mag scale calibration failed.");
-	}
-
-	return ret;
-}
-
-int HMC5883::check_scale()
-{
-	bool scale_valid;
-
-	if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
-		(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
-		(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
-		/* scale is one */
-		scale_valid = false;
-	} else {
-		scale_valid = true;
-	}
-
-	/* return 0 if calibrated, 1 else */
-	return !scale_valid;
-}
-
-int HMC5883::check_offset()
-{
-	bool offset_valid;
-
-	if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
-		(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
-		(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
-		/* offset is zero */
-		offset_valid = false;
-	} else {
-		offset_valid = true;
-	}
-
-	/* return 0 if calibrated, 1 else */
-	return !offset_valid;
-}
-
-int HMC5883::check_calibration()
-{
-	bool offset_valid = (check_offset() == OK);
-	bool scale_valid  = (check_scale() == OK);
-
-	if (_calibrated != (offset_valid && scale_valid)) {
-		warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
-					  (offset_valid) ? "" : "offset invalid");
-		_calibrated = (offset_valid && scale_valid);
-
-
-		// XXX Change advertisement
-
-		/* notify about state change */
-		struct subsystem_info_s info = {
-			true,
-			true,
-			_calibrated,
-			SUBSYSTEM_TYPE_MAG};
-		static orb_advert_t pub = -1;
-
-		if (pub > 0) {
-			orb_publish(ORB_ID(subsystem_info), pub, &info);
-		} else {
-			pub = orb_advertise(ORB_ID(subsystem_info), &info);
-		}
-	}
-
-	/* return 0 if calibrated, 1 else */
-	return (!_calibrated);
-}
-
-int HMC5883::set_excitement(unsigned enable)
-{
-	int ret;
-	/* arm the excitement strap */
-	uint8_t conf_reg;
-	ret = read_reg(ADDR_CONF_A, conf_reg);
-
-	if (OK != ret)
-		perf_count(_comms_errors);
-
-	if (((int)enable) < 0) {
-		conf_reg |= 0x01;
-
-	} else if (enable > 0) {
-		conf_reg |= 0x02;
-
-	} else {
-		conf_reg &= ~0x03;
-	}
-
-	ret = write_reg(ADDR_CONF_A, conf_reg);
-
-	if (OK != ret)
-		perf_count(_comms_errors);
-
-	uint8_t conf_reg_ret;
-	read_reg(ADDR_CONF_A, conf_reg_ret);
-
-	return !(conf_reg == conf_reg_ret);
-}
-
-int
-HMC5883::write_reg(uint8_t reg, uint8_t val)
-{
-	uint8_t cmd[] = { reg, val };
-
-	return transfer(&cmd[0], 2, nullptr, 0);
-}
-
-int
-HMC5883::read_reg(uint8_t reg, uint8_t &val)
-{
-	return transfer(&reg, 1, &val, 1);
-}
-
-float
-HMC5883::meas_to_float(uint8_t in[2])
-{
-	union {
-		uint8_t	b[2];
-		int16_t	w;
-	} u;
-
-	u.b[0] = in[1];
-	u.b[1] = in[0];
-
-	return (float) u.w;
-}
-
-void
-HMC5883::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace hmc5883
-{
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-const int ERROR = -1;
-
-HMC5883	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-int	calibrate();
-
-/**
- * Start the driver.
- */
-void
-start()
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new HMC5883(HMC5883L_BUS);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
-
-	exit(0);
-
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-	struct mag_report report;
-	ssize_t sz;
-	int ret;
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
-
-	/* do a simple demand read */
-	sz = read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report))
-		err(1, "immediate read failed");
-
-	warnx("single read");
-	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	warnx("time:        %lld", report.timestamp);
-
-	/* set the queue depth to 10 */
-	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
-		errx(1, "failed to set queue depth");
-
-	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
-		errx(1, "failed to set 2Hz poll rate");
-
-	/* read the sensor 5x and report each value */
-	for (unsigned i = 0; i < 5; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 2000);
-
-		if (ret != 1)
-			errx(1, "timed out waiting for sensor data");
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report))
-			err(1, "periodic read failed");
-
-		warnx("periodic read %u", i);
-		warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-		warnx("time:        %lld", report.timestamp);
-	}
-
-	errx(0, "PASS");
-}
-
-
-/**
- * Automatic scale calibration.
- *
- * Basic idea:
- *
- *   output = (ext field +- 1.1 Ga self-test) * scale factor
- *
- * and consequently:
- *
- *   1.1 Ga = (excited - normal) * scale factor
- *   scale factor = (excited - normal) / 1.1 Ga
- *
- *   sxy = (excited - normal) / 766	| for conf reg. B set to 0x60 / Gain = 3
- *   sz  = (excited - normal) / 713	| for conf reg. B set to 0x60 / Gain = 3
- *
- * By subtracting the non-excited measurement the pure 1.1 Ga reading
- * can be extracted and the sensitivity of all axes can be matched.
- *
- * SELF TEST OPERATION
- * To check the HMC5883L for proper operation, a self test feature in incorporated
- * in which the sensor offset straps are excited to create a nominal field strength
- * (bias field) to be measured. To implement self test, the least significant bits
- * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
- * or 10 (negetive bias), e.g. 0x11 or 0x12.
- * Then, by placing the mode register into single-measurement mode (0x01),
- * two data acquisition cycles will be made on each magnetic vector.
- * The first acquisition will be a set pulse followed shortly by measurement
- * data of the external field. The second acquisition will have the offset strap
- * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
- * about a Âą1.1 gauss self test field plus the external field. The first acquisition
- * values will be subtracted from the second acquisition, and the net measurement
- * will be placed into the data output registers.
- * Since self test adds ~1.1 Gauss additional field to the existing field strength,
- * using a reduced gain setting prevents sensor from being saturated and data registers
- * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
- * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
- * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
- * output register. To leave the self test mode, change MS1 and MS0 bit of the
- * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
- * Using the self test method described above, the user can scale sensor
- */
-int calibrate()
-{
-	int ret;
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
-
-	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
-		warnx("failed to enable sensor calibration mode");
-	}
-
-	close(fd);
-
-	if (ret == OK) {
-		errx(0, "PASS");
-
-	} else {
-		errx(1, "FAIL");
-	}
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
-
-	exit(0);
-}
-
-/**
- * Print a little info about the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running");
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	exit(0);
-}
-
-} // namespace
-
-int
-hmc5883_main(int argc, char *argv[])
-{
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(argv[1], "start"))
-		hmc5883::start();
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		hmc5883::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		hmc5883::reset();
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
-		hmc5883::info();
-
-	/*
-	 * Autocalibrate the scaling
-	 */
-	if (!strcmp(argv[1], "calibrate")) {
-		if (hmc5883::calibrate() == 0) {
-			errx(0, "calibration successful");
-
-		} else {
-			errx(1, "calibration failed");
-		}
-	}
-
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'");
-}
diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile
deleted file mode 100644
index 0d2405787..000000000
--- a/apps/drivers/mb12xx/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build the Maxbotix Sonar driver.
-#
-
-APPNAME		= mb12xx
-PRIORITY	= SCHED_PRIORITY_DEFAULT
-STACKSIZE	= 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp
deleted file mode 100644
index 9d0f6bddc..000000000
--- a/apps/drivers/mb12xx/mb12xx.cpp
+++ /dev/null
@@ -1,840 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mb12xx.cpp
- * @author Greg Hulands
- *
- * Driver for the Maxbotix sonar range finders connected via I2C.
- */
-	 
-#include <nuttx/config.h>
-
-#include <drivers/device/i2c.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
-
-#include <arch/board/board.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_range_finder.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
-
-/* Configuration Constants */
-#define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
-#define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
-
-/* MB12xx Registers addresses */
-
-#define MB12XX_TAKE_RANGE_REG	0x51		/* Measure range Register */
-#define MB12XX_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
-#define MB12XX_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
-
-/* Device limits */
-#define MB12XX_MIN_DISTANCE (0.20f)
-#define MB12XX_MAX_DISTANCE (7.65f)
-	 
-#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-class MB12XX : public device::I2C
-{
-public:
-	MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
-	virtual ~MB12XX();
-	
-	virtual int 		init();
-	
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
-	
-	/**
-	* Diagnostics - print some basic information about the driver.
-	*/
-	void				print_info();
-	
-protected:
-	virtual int			probe();
-
-private:
-	float				_min_distance;
-	float				_max_distance;
-	work_s				_work;
-	unsigned			_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	range_finder_report	*_reports;
-	bool				_sensor_ok;
-	int					_measure_ticks;
-	bool				_collect_phase;
-	
-	orb_advert_t		_range_finder_topic;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_comms_errors;
-	perf_counter_t		_buffer_overflows;
-	
-	/**
-	* Test whether the device supported by the driver is present at a
-	* specific address.
-	*
-	* @param address	The I2C bus address to probe.
-	* @return		True if the device is present.
-	*/
-	int					probe_address(uint8_t address);
-	
-	/**
-	* Initialise the automatic measurement state machine and start it.
-	*
-	* @note This function is called at open and error time.  It might make sense
-	*       to make it more aggressive about resetting the bus in case of errors.
-	*/
-	void				start();
-	
-	/**
-	* Stop the automatic measurement state machine.
-	*/
-	void				stop();
-	
-	/**
-	* Set the min and max distance thresholds if you want the end points of the sensors
-	* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
-	* and MB12XX_MAX_DISTANCE
-	*/
-	void				set_minimum_distance(float min);
-	void				set_maximum_distance(float max);
-	float				get_minimum_distance();
-	float				get_maximum_distance();
-	
-	/**
-	* Perform a poll cycle; collect from the previous measurement
-	* and start a new one.
-	*/
-	void				cycle();
-	int					measure();
-	int					collect();
-	/**
-	* Static trampoline from the workq context; because we don't have a
-	* generic workq wrapper yet.
-	*
-	* @param arg		Instance pointer for the driver that is polling.
-	*/
-	static void		cycle_trampoline(void *arg);
-	
-	
-};
-
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
-
-MB12XX::MB12XX(int bus, int address) :
-	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
-	_min_distance(MB12XX_MIN_DISTANCE),
-	_max_distance(MB12XX_MAX_DISTANCE),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_sensor_ok(false),
-	_measure_ticks(0),
-	_collect_phase(false),
-	_range_finder_topic(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
-{
-	// enable debug() calls
-	_debug_enabled = true;
-	
-	// work_cancel in the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
-}
-
-MB12XX::~MB12XX()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-}
-
-int
-MB12XX::init()
-{
-	int ret = ERROR;
-
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
-		goto out;
-
-	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct range_finder_report[_num_reports];
-
-	if (_reports == nullptr)
-		goto out;
-
-	_oldest_report = _next_report = 0;
-
-	/* get a publish handle on the range finder topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
-
-	if (_range_finder_topic < 0)
-		debug("failed to create sensor_range_finder object. Did you start uOrb?");
-
-	ret = OK;
-	/* sensor is ok, but we don't really know if it is within range */
-	_sensor_ok = true;
-out:
-	return ret;
-}
-
-int
-MB12XX::probe()
-{
-	return measure();
-}
-
-void
-MB12XX::set_minimum_distance(float min)
-{
-	_min_distance = min;
-}	
-
-void
-MB12XX::set_maximum_distance(float max)
-{
-	_max_distance = max;
-}	
-
-float
-MB12XX::get_minimum_distance()
-{
-	return _min_distance;
-}
-
-float
-MB12XX::get_maximum_distance()
-{
-	return _max_distance;
-}
-
-int
-MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
-				return OK;
-
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
-
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
-			return SENSOR_POLLRATE_MANUAL;
-
-		return (1000 / _measure_ticks);
-
-	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct range_finder_report *buf = new struct range_finder_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
-		
-	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
-	
-	case RANGEFINDERIOCSETMINIUMDISTANCE:
-	{
-		set_minimum_distance(*(float *)arg);
-		return 0;
-	}
-	break;
-	case RANGEFINDERIOCSETMAXIUMDISTANCE:
-	{
-		set_maximum_distance(*(float *)arg);
-		return 0;
-	}
-	break;
-	default:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
-	}
-}
-
-ssize_t
-MB12XX::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct range_finder_report);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1)
-		return -ENOSPC;
-
-	/* if automatic measurement is enabled */
-	if (_measure_ticks > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the workq thread while we are doing this;
-		 * we are careful to avoid racing with them.
-		 */
-		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
-	do {
-		_oldest_report = _next_report = 0;
-
-		/* trigger a measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* wait for it to complete */
-		usleep(MB12XX_CONVERSION_INTERVAL);
-
-		/* run the collection phase */
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
-	} while (0);
-
-	return ret;
-}
-
-int
-MB12XX::measure()
-{
-	int ret;
-
-	/*
-	 * Send the command to begin a measurement.
-	 */
-	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
-	ret = transfer(&cmd, 1, nullptr, 0);
-
-	if (OK != ret)
-	{
-		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
-		return ret;
-	}
-	ret = OK;
-	
-	return ret;
-}
-
-int
-MB12XX::collect()
-{
-	int	ret = -EIO;
-	
-	/* read from the sensor */
-	uint8_t val[2] = {0, 0};
-	
-	perf_begin(_sample_perf);
-	
-	ret = transfer(nullptr, 0, &val[0], 2);
-	
-	if (ret < 0)
-	{
-		log("error reading from sensor: %d", ret);
-		return ret;
-	}
-	
-	uint16_t distance = val[0] << 8 | val[1];
-	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
-	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].distance = si_units;
-	_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
-	
-	/* publish it */
-	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
-
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
-	}
-
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-
-	ret = OK;
-
-out:
-	perf_end(_sample_perf);
-	return ret;
-	
-	return ret;
-}
-
-void
-MB12XX::start()
-{
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_oldest_report = _next_report = 0;
-
-	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
-	
-	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		SUBSYSTEM_TYPE_RANGEFINDER};
-	static orb_advert_t pub = -1;
-
-	if (pub > 0) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
-}
-
-void
-MB12XX::stop()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-MB12XX::cycle_trampoline(void *arg)
-{
-	MB12XX *dev = (MB12XX *)arg;
-
-	dev->cycle();
-}
-
-void
-MB12XX::cycle()
-{
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		if (OK != collect()) {
-			log("collection error");
-			/* restart the measurement state machine */
-			start();
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
-				   &_work,
-				   (worker_t)&MB12XX::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
-
-			return;
-		}
-	}
-
-	/* measurement phase */
-	if (OK != measure())
-		log("measure error");
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&MB12XX::cycle_trampoline,
-		   this,
-		   USEC2TICK(MB12XX_CONVERSION_INTERVAL));
-}
-
-void
-MB12XX::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace mb12xx
-{
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-const int ERROR = -1;
-
-MB12XX	*g_dev;
-
-void	start();
-void	stop();
-void	test();
-void	reset();
-void	info();
-
-/**
- * Start the driver.
- */
-void
-start()
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new MB12XX(MB12XX_BUS);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
-
-	exit(0);
-
-fail:
-
-	if (g_dev != nullptr) 
-	{
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Stop the driver
- */
-void stop()
-{
-	if (g_dev != nullptr)
-	{
-		delete g_dev;
-		g_dev = nullptr;
-	}
-	else
-	{
-		errx(1, "driver not running");
-	}
-	exit(0);
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-	struct range_finder_report report;
-	ssize_t sz;
-	int ret;
-
-	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
-
-	/* do a simple demand read */
-	sz = read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report))
-		err(1, "immediate read failed");
-
-	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
-	warnx("time:        %lld", report.timestamp);
-
-	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
-		errx(1, "failed to set 2Hz poll rate");
-
-	/* read the sensor 5x and report each value */
-	for (unsigned i = 0; i < 5; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 2000);
-
-		if (ret != 1)
-			errx(1, "timed out waiting for sensor data");
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report))
-			err(1, "periodic read failed");
-
-		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f", (double)report.distance);
-		warnx("time:        %lld", report.timestamp);
-	}
-
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
-
-	exit(0);
-}
-
-/**
- * Print a little info about the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running");
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	exit(0);
-}
-
-} // namespace
-
-int
-mb12xx_main(int argc, char *argv[])
-{
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(argv[1], "start"))
-		mb12xx::start();
-	
-	 /*
-	  * Stop the driver
-	  */
-	 if (!strcmp(argv[1], "stop"))
-		 mb12xx::stop();
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		mb12xx::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		mb12xx::reset();
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
-		mb12xx::info();
-
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
-}
diff --git a/apps/drivers/mpu6000/Makefile b/apps/drivers/mpu6000/Makefile
deleted file mode 100644
index 32df1bdae..000000000
--- a/apps/drivers/mpu6000/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Makefile to build the BMA180 driver.
-#
-
-APPNAME		 = mpu6000
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
deleted file mode 100644
index ce7062046..000000000
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ /dev/null
@@ -1,1219 +0,0 @@
-/****************************************************************************
- *
- *   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 mpu6000.cpp
- *
- * Driver for the Invensense MPU6000 connected via SPI.
- */
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-#include <systemlib/conversions.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/clock.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_hrt.h>
-
-#include <drivers/device/spi.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-
-#define DIR_READ			0x80
-#define DIR_WRITE			0x00
-
-// MPU 6000 registers
-#define MPUREG_WHOAMI			0x75
-#define MPUREG_SMPLRT_DIV		0x19
-#define MPUREG_CONFIG			0x1A
-#define MPUREG_GYRO_CONFIG		0x1B
-#define MPUREG_ACCEL_CONFIG		0x1C
-#define MPUREG_FIFO_EN			0x23
-#define MPUREG_INT_PIN_CFG		0x37
-#define MPUREG_INT_ENABLE		0x38
-#define MPUREG_INT_STATUS		0x3A
-#define MPUREG_ACCEL_XOUT_H		0x3B
-#define MPUREG_ACCEL_XOUT_L		0x3C
-#define MPUREG_ACCEL_YOUT_H		0x3D
-#define MPUREG_ACCEL_YOUT_L		0x3E
-#define MPUREG_ACCEL_ZOUT_H		0x3F
-#define MPUREG_ACCEL_ZOUT_L		0x40
-#define MPUREG_TEMP_OUT_H		0x41
-#define MPUREG_TEMP_OUT_L		0x42
-#define MPUREG_GYRO_XOUT_H		0x43
-#define MPUREG_GYRO_XOUT_L		0x44
-#define MPUREG_GYRO_YOUT_H		0x45
-#define MPUREG_GYRO_YOUT_L		0x46
-#define MPUREG_GYRO_ZOUT_H		0x47
-#define MPUREG_GYRO_ZOUT_L		0x48
-#define MPUREG_USER_CTRL		0x6A
-#define MPUREG_PWR_MGMT_1		0x6B
-#define MPUREG_PWR_MGMT_2		0x6C
-#define MPUREG_FIFO_COUNTH		0x72
-#define MPUREG_FIFO_COUNTL		0x73
-#define MPUREG_FIFO_R_W			0x74
-#define MPUREG_PRODUCT_ID		0x0C
-
-// Configuration bits MPU 3000 and MPU 6000 (not revised)?
-#define BIT_SLEEP			0x40
-#define BIT_H_RESET			0x80
-#define BITS_CLKSEL			0x07
-#define MPU_CLK_SEL_PLLGYROX		0x01
-#define MPU_CLK_SEL_PLLGYROZ		0x03
-#define MPU_EXT_SYNC_GYROX		0x02
-#define BITS_FS_250DPS			0x00
-#define BITS_FS_500DPS			0x08
-#define BITS_FS_1000DPS			0x10
-#define BITS_FS_2000DPS			0x18
-#define BITS_FS_MASK			0x18
-#define BITS_DLPF_CFG_256HZ_NOLPF2	0x00
-#define BITS_DLPF_CFG_188HZ		0x01
-#define BITS_DLPF_CFG_98HZ		0x02
-#define BITS_DLPF_CFG_42HZ		0x03
-#define BITS_DLPF_CFG_20HZ		0x04
-#define BITS_DLPF_CFG_10HZ		0x05
-#define BITS_DLPF_CFG_5HZ		0x06
-#define BITS_DLPF_CFG_2100HZ_NOLPF	0x07
-#define BITS_DLPF_CFG_MASK		0x07
-#define BIT_INT_ANYRD_2CLEAR		0x10
-#define BIT_RAW_RDY_EN			0x01
-#define BIT_I2C_IF_DIS			0x10
-#define BIT_INT_STATUS_DATA		0x01
-
-// Product ID Description for MPU6000
-// high 4 bits 	low 4 bits
-// Product Name	Product Revision
-#define MPU6000ES_REV_C4		0x14
-#define MPU6000ES_REV_C5		0x15
-#define MPU6000ES_REV_D6		0x16
-#define MPU6000ES_REV_D7		0x17
-#define MPU6000ES_REV_D8		0x18
-#define MPU6000_REV_C4			0x54
-#define MPU6000_REV_C5			0x55
-#define MPU6000_REV_D6			0x56
-#define MPU6000_REV_D7			0x57
-#define MPU6000_REV_D8			0x58
-#define MPU6000_REV_D9			0x59
-#define MPU6000_REV_D10			0x5A
-
-
-class MPU6000_gyro;
-
-class MPU6000 : public device::SPI
-{
-public:
-	MPU6000(int bus, spi_dev_e device);
-	virtual ~MPU6000();
-
-	virtual int		init();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void			print_info();
-
-protected:
-	virtual int		probe();
-
-	friend class MPU6000_gyro;
-
-	virtual ssize_t		gyro_read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-private:
-	MPU6000_gyro		*_gyro;
-	uint8_t			_product;	/** product code */
-
-	struct hrt_call		_call;
-	unsigned		_call_interval;
-
-	struct accel_report	_accel_report;
-	struct accel_scale	_accel_scale;
-	float			_accel_range_scale;
-	float			_accel_range_m_s2;
-	orb_advert_t		_accel_topic;
-
-	struct gyro_report	_gyro_report;
-	struct gyro_scale	_gyro_scale;
-	float			_gyro_range_scale;
-	float			_gyro_range_rad_s;
-	orb_advert_t		_gyro_topic;
-
-	unsigned		_reads;
-	perf_counter_t		_sample_perf;
-
-	/**
-	 * Start automatic measurement.
-	 */
-	void			start();
-
-	/**
-	 * Stop automatic measurement.
-	 */
-	void			stop();
-
-	/**
-	 * Static trampoline from the hrt_call context; because we don't have a
-	 * generic hrt wrapper yet.
-	 *
-	 * Called by the HRT in interrupt context at the specified rate if
-	 * automatic polling is enabled.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		measure_trampoline(void *arg);
-
-	/**
-	 * Fetch measurements from the sensor and update the report ring.
-	 */
-	void			measure();
-
-	/**
-	 * Read a register from the MPU6000
-	 *
-	 * @param		The register to read.
-	 * @return		The value that was read.
-	 */
-	uint8_t			read_reg(unsigned reg);
-	uint16_t		read_reg16(unsigned reg);
-
-	/**
-	 * Write a register in the MPU6000
-	 *
-	 * @param reg		The register to write.
-	 * @param value		The new value to write.
-	 */
-	void			write_reg(unsigned reg, uint8_t value);
-
-	/**
-	 * Modify a register in the MPU6000
-	 *
-	 * Bits are cleared before bits are set.
-	 *
-	 * @param reg		The register to modify.
-	 * @param clearbits	Bits in the register to clear.
-	 * @param setbits	Bits in the register to set.
-	 */
-	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
-
-	/**
-	 * Set the MPU6000 measurement range.
-	 *
-	 * @param max_g		The maximum G value the range must support.
-	 * @return		OK if the value can be supported, -ERANGE otherwise.
-	 */
-	int			set_range(unsigned max_g);
-
-	/**
-	 * Swap a 16-bit value read from the MPU6000 to native byte order.
-	 */
-	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
-
-	/**
-	 * Self test
-	 *
-	 * @return 0 on success, 1 on failure
-	 */
-	 int 			self_test();
-
-	/*
-	  set low pass filter frequency
-	 */
-	void _set_dlpf_filter(uint16_t frequency_hz);
-
-};
-
-/**
- * Helper class implementing the gyro driver node.
- */
-class MPU6000_gyro : public device::CDev
-{
-public:
-	MPU6000_gyro(MPU6000 *parent);
-	~MPU6000_gyro();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-protected:
-	friend class MPU6000;
-
-	void			parent_poll_notify();
-private:
-	MPU6000			*_parent;
-};
-
-/** driver 'main' command */
-extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
-
-MPU6000::MPU6000(int bus, spi_dev_e device) :
-	SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
-	_gyro(new MPU6000_gyro(this)),
-	_product(0),
-	_call_interval(0),
-	_accel_range_scale(0.0f),
-	_accel_range_m_s2(0.0f),
-	_accel_topic(-1),
-	_gyro_range_scale(0.0f),
-	_gyro_range_rad_s(0.0f),
-	_gyro_topic(-1),
-	_reads(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
-{
-	// disable debug() calls
-	_debug_enabled = false;
-
-	// default accel scale factors
-	_accel_scale.x_offset = 0;
-	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
-	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
-	_accel_scale.z_scale  = 1.0f;
-
-	// default gyro scale factors
-	_gyro_scale.x_offset = 0;
-	_gyro_scale.x_scale  = 1.0f;
-	_gyro_scale.y_offset = 0;
-	_gyro_scale.y_scale  = 1.0f;
-	_gyro_scale.z_offset = 0;
-	_gyro_scale.z_scale  = 1.0f;
-
-	memset(&_accel_report, 0, sizeof(_accel_report));
-	memset(&_gyro_report, 0, sizeof(_gyro_report));
-	memset(&_call, 0, sizeof(_call));
-}
-
-MPU6000::~MPU6000()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	/* delete the gyro subdriver */
-	delete _gyro;
-
-	/* delete the perf counter */
-	perf_free(_sample_perf);
-}
-
-int
-MPU6000::init()
-{
-	int ret;
-
-	/* do SPI init (and probe) first */
-	ret = SPI::init();
-
-	/* if probe/setup failed, bail now */
-	if (ret != OK) {
-		debug("SPI setup failed");
-		return ret;
-	}
-
-	/* advertise sensor topics */
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
-
-	// Chip reset
-	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
-	up_udelay(10000);
-
-	// Wake up device and select GyroZ clock (better performance)
-	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
-	up_udelay(1000);
-
-	// Disable I2C bus (recommended on datasheet)
-	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
-	up_udelay(1000);
-
-	// SAMPLE RATE
-	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
-	usleep(1000);
-
-	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
-	// was 90 Hz, but this ruins quality and does not improve the
-	// system response
-	_set_dlpf_filter(20);
-	usleep(1000);
-	// Gyro scale 2000 deg/s ()
-	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
-	usleep(1000);
-
-	// correct gyro scale factors
-	// scale to rad/s in SI units
-	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
-	// scaling factor:
-	// 1/(2^15)*(2000/180)*PI
-	_gyro_scale.x_offset = 0;
-	_gyro_scale.x_scale  = 1.0f;
-	_gyro_scale.y_offset = 0;
-	_gyro_scale.y_scale  = 1.0f;
-	_gyro_scale.z_offset = 0;
-	_gyro_scale.z_scale  = 1.0f;
-	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
-	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
-
-	// product-specific scaling
-	switch (_product) {
-	case MPU6000ES_REV_C4:
-	case MPU6000ES_REV_C5:
-	case MPU6000_REV_C4:
-	case MPU6000_REV_C5:
-		// Accel scale 8g (4096 LSB/g)
-		// Rev C has different scaling than rev D
-		write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
-		break;
-
-	case MPU6000ES_REV_D6:
-	case MPU6000ES_REV_D7:
-	case MPU6000ES_REV_D8:
-	case MPU6000_REV_D6:
-	case MPU6000_REV_D7:
-	case MPU6000_REV_D8:
-	case MPU6000_REV_D9:
-	case MPU6000_REV_D10:
-	// default case to cope with new chip revisions, which
-	// presumably won't have the accel scaling bug		
-	default:
-		// Accel scale 8g (4096 LSB/g)
-		write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
-		break;
-	}
-
-	// Correct accel scale factors of 4096 LSB/g
-	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_scale.x_offset = 0;
-	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
-	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
-	_accel_scale.z_scale  = 1.0f;
-	_accel_range_scale = (9.81f / 4096.0f);
-	_accel_range_m_s2 = 8.0f * 9.81f;
-
-	usleep(1000);
-
-	// INT CFG => Interrupt on Data Ready
-	write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
-	usleep(1000);
-	write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
-	usleep(1000);
-
-	// Oscillator set
-	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
-	usleep(1000);
-
-	/* do CDev init for the gyro device node, keep it optional */
-	int gyro_ret = _gyro->init();
-
-	if (gyro_ret != OK) {
-		_gyro_topic = -1;
-	}
-
-	return ret;
-}
-
-int
-MPU6000::probe()
-{
-
-	/* look for a product ID we recognise */
-	_product = read_reg(MPUREG_PRODUCT_ID);
-
-	// verify product revision
-	switch (_product) {
-	case MPU6000ES_REV_C4:
-	case MPU6000ES_REV_C5:
-	case MPU6000_REV_C4:
-	case MPU6000_REV_C5:
-	case MPU6000ES_REV_D6:
-	case MPU6000ES_REV_D7:
-	case MPU6000ES_REV_D8:
-	case MPU6000_REV_D6:
-	case MPU6000_REV_D7:
-	case MPU6000_REV_D8:
-	case MPU6000_REV_D9:
-	case MPU6000_REV_D10:
-		debug("ID 0x%02x", _product);
-		return OK;
-	}
-
-	debug("unexpected ID 0x%02x", _product);
-	return -EIO;
-}
-
-/*
-  set the DLPF filter frequency. This affects both accel and gyro.
- */
-void
-MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
-{
-	uint8_t filter;
-
-	/* 
-	   choose next highest filter frequency available
-	 */
-	if (frequency_hz <= 5) {
-		filter = BITS_DLPF_CFG_5HZ;
-	} else if (frequency_hz <= 10) {
-		filter = BITS_DLPF_CFG_10HZ;
-	} else if (frequency_hz <= 20) {
-		filter = BITS_DLPF_CFG_20HZ;
-	} else if (frequency_hz <= 42) {
-		filter = BITS_DLPF_CFG_42HZ;
-	} else if (frequency_hz <= 98) {
-		filter = BITS_DLPF_CFG_98HZ;
-	} else if (frequency_hz <= 188) {
-		filter = BITS_DLPF_CFG_188HZ;
-	} else if (frequency_hz <= 256) {
-		filter = BITS_DLPF_CFG_256HZ_NOLPF2;
-	} else {
-		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
-	}
-	write_reg(MPUREG_CONFIG, filter);
-}
-
-ssize_t
-MPU6000::read(struct file *filp, char *buffer, size_t buflen)
-{
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (buflen < sizeof(_accel_report))
-		return -ENOSPC;
-
-	/* if automatic measurement is not enabled */
-	if (_call_interval == 0)
-		measure();
-
-	/* copy out the latest reports */
-	memcpy(buffer, &_accel_report, sizeof(_accel_report));
-	ret = sizeof(_accel_report);
-
-	return ret;
-}
-
-int
-MPU6000::self_test()
-{
-	if (_reads == 0) {
-		measure();
-	}
-
-	/* return 0 on success, 1 else */
-	return (_reads > 0) ? 0 : 1;
-}
-
-ssize_t
-MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
-{
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (buflen < sizeof(_gyro_report))
-		return -ENOSPC;
-
-	/* if automatic measurement is not enabled */
-	if (_call_interval == 0)
-		measure();
-
-	/* copy out the latest report */
-	memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
-	ret = sizeof(_gyro_report);
-
-	return ret;
-}
-
-int
-MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_call_interval = 0;
-				return OK;
-
-				/* external signalling not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT:
-				/* XXX 500Hz is just a wild guess */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 500);
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_call_interval == 0);
-
-					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
-
-					/* check against maximum sane rate */
-					if (ticks < 1000)
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					/* XXX this is a bit shady, but no other way to adjust... */
-					_call.period = _call_interval = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_call_interval == 0)
-			return SENSOR_POLLRATE_MANUAL;
-
-		return 1000000 / _call_interval;
-
-	case SENSORIOCSQUEUEDEPTH:
-		/* XXX not implemented */
-		return -EINVAL;
-
-	case SENSORIOCGQUEUEDEPTH:
-		/* XXX not implemented */
-		return -EINVAL;
-
-
-	case ACCELIOCSSAMPLERATE:
-	case ACCELIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
-
-	case ACCELIOCSLOWPASS:
-	case ACCELIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
-		return OK;
-
-	case ACCELIOCSSCALE:
-		{
-			/* copy scale, but only if off by a few percent */
-			struct accel_scale *s = (struct accel_scale *) arg;
-			float sum = s->x_scale + s->y_scale + s->z_scale;
-			if (sum > 2.0f && sum < 4.0f) {
-				memcpy(&_accel_scale, s, sizeof(_accel_scale));
-				return OK;
-			} else {
-				return -EINVAL;
-			}
-		}
-
-	case ACCELIOCGSCALE:
-		/* copy scale out */
-		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
-		return OK;
-
-	case ACCELIOCSRANGE:
-	case ACCELIOCGRANGE:
-		/* XXX not implemented */
-		// XXX change these two values on set:
-		// _accel_range_scale = (9.81f / 4096.0f);
-		// _accel_range_rad_s = 8.0f * 9.81f;
-		return -EINVAL;
-
-	case ACCELIOCSELFTEST:
-		return self_test();
-
-	default:
-		/* give it to the superclass */
-		return SPI::ioctl(filp, cmd, arg);
-	}
-}
-
-int
-MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-		/* these are shared with the accel side */
-	case SENSORIOCSPOLLRATE:
-	case SENSORIOCGPOLLRATE:
-	case SENSORIOCSQUEUEDEPTH:
-	case SENSORIOCGQUEUEDEPTH:
-	case SENSORIOCRESET:
-		return ioctl(filp, cmd, arg);
-
-	case GYROIOCSSAMPLERATE:
-	case GYROIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
-
-	case GYROIOCSLOWPASS:
-	case GYROIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
-		return OK;
-
-	case GYROIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
-		return OK;
-
-	case GYROIOCGSCALE:
-		/* copy scale out */
-		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
-		return OK;
-
-	case GYROIOCSRANGE:
-	case GYROIOCGRANGE:
-		/* XXX not implemented */
-		// XXX change these two values on set:
-		// _gyro_range_scale = xx
-		// _gyro_range_m_s2 = xx
-		return -EINVAL;
-
-	case GYROIOCSELFTEST:
-		return self_test();
-
-	default:
-		/* give it to the superclass */
-		return SPI::ioctl(filp, cmd, arg);
-	}
-}
-
-uint8_t
-MPU6000::read_reg(unsigned reg)
-{
-	uint8_t cmd[2];
-
-	cmd[0] = reg | DIR_READ;
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return cmd[1];
-}
-
-uint16_t
-MPU6000::read_reg16(unsigned reg)
-{
-	uint8_t cmd[3];
-
-	cmd[0] = reg | DIR_READ;
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return (uint16_t)(cmd[1] << 8) | cmd[2];
-}
-
-void
-MPU6000::write_reg(unsigned reg, uint8_t value)
-{
-	uint8_t	cmd[2];
-
-	cmd[0] = reg | DIR_WRITE;
-	cmd[1] = value;
-
-	transfer(cmd, nullptr, sizeof(cmd));
-}
-
-void
-MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
-{
-	uint8_t	val;
-
-	val = read_reg(reg);
-	val &= ~clearbits;
-	val |= setbits;
-	write_reg(reg, val);
-}
-
-int
-MPU6000::set_range(unsigned max_g)
-{
-#if 0
-	uint8_t rangebits;
-	float rangescale;
-
-	if (max_g > 16) {
-		return -ERANGE;
-
-	} else if (max_g > 8) {		/* 16G */
-		rangebits = OFFSET_LSB1_RANGE_16G;
-		rangescale = 1.98;
-
-	} else if (max_g > 4) {		/* 8G */
-		rangebits = OFFSET_LSB1_RANGE_8G;
-		rangescale = 0.99;
-
-	} else if (max_g > 3) {		/* 4G */
-		rangebits = OFFSET_LSB1_RANGE_4G;
-		rangescale = 0.5;
-
-	} else if (max_g > 2) {		/* 3G */
-		rangebits = OFFSET_LSB1_RANGE_3G;
-		rangescale = 0.38;
-
-	} else if (max_g > 1) {		/* 2G */
-		rangebits = OFFSET_LSB1_RANGE_2G;
-		rangescale = 0.25;
-
-	} else {			/* 1G */
-		rangebits = OFFSET_LSB1_RANGE_1G;
-		rangescale = 0.13;
-	}
-
-	/* adjust sensor configuration */
-	modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
-	_range_scale = rangescale;
-#endif
-	return OK;
-}
-
-void
-MPU6000::start()
-{
-	/* make sure we are stopped first */
-	stop();
-
-	/* start polling at the specified rate */
-	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
-}
-
-void
-MPU6000::stop()
-{
-	hrt_cancel(&_call);
-}
-
-void
-MPU6000::measure_trampoline(void *arg)
-{
-	MPU6000 *dev = (MPU6000 *)arg;
-
-	/* make another measurement */
-	dev->measure();
-}
-
-void
-MPU6000::measure()
-{
-#pragma pack(push, 1)
-	/**
-	 * Report conversation within the MPU6000, including command byte and
-	 * interrupt status.
-	 */
-	struct MPUReport {
-		uint8_t		cmd;
-		uint8_t		status;
-		uint8_t		accel_x[2];
-		uint8_t		accel_y[2];
-		uint8_t		accel_z[2];
-		uint8_t		temp[2];
-		uint8_t		gyro_x[2];
-		uint8_t		gyro_y[2];
-		uint8_t		gyro_z[2];
-	} mpu_report;
-#pragma pack(pop)
-
-	struct Report {
-		int16_t		accel_x;
-		int16_t		accel_y;
-		int16_t		accel_z;
-		int16_t		temp;
-		int16_t		gyro_x;
-		int16_t		gyro_y;
-		int16_t		gyro_z;
-	} report;
-
-	/* start measuring */
-	perf_begin(_sample_perf);
-
-	/*
-	 * Fetch the full set of measurements from the MPU6000 in one pass.
-	 */
-	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
-	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
-		return;
-
-	/* count measurement */
-	_reads++;
-
-	/*
-	 * Convert from big to little endian
-	 */
-
-	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
-	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
-	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
-
-	report.temp = int16_t_from_bytes(mpu_report.temp);
-
-	report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
-	report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
-	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
-
-	/*
-	 * Swap axes and negate y
-	 */
-	int16_t accel_xt = report.accel_y;
-	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
-
-	int16_t gyro_xt = report.gyro_y;
-	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
-
-	/*
-	 * Apply the swap
-	 */
-	report.accel_x = accel_xt;
-	report.accel_y = accel_yt;
-	report.gyro_x = gyro_xt;
-	report.gyro_y = gyro_yt;
-
-	/*
-	 * Adjust and scale results to m/s^2.
-	 */
-	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
-
-
-	/*
-	 * 1) Scale raw value to SI units using scaling from datasheet.
-	 * 2) Subtract static offset (in SI units)
-	 * 3) Scale the statically calibrated values with a linear
-	 *    dynamically obtained factor
-	 *
-	 * Note: the static sensor offset is the number the sensor outputs
-	 * 	 at a nominally 'zero' input. Therefore the offset has to
-	 * 	 be subtracted.
-	 *
-	 *	 Example: A gyro outputs a value of 74 at zero angular rate
-	 *	 	  the offset is 74 from the origin and subtracting
-	 *		  74 from all measurements centers them around zero.
-	 */
-
-
-	/* NOTE: Axes have been swapped to match the board a few lines above. */
-
-	_accel_report.x_raw = report.accel_x;
-	_accel_report.y_raw = report.accel_y;
-	_accel_report.z_raw = report.accel_z;
-
-	_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	_accel_report.scaling = _accel_range_scale;
-	_accel_report.range_m_s2 = _accel_range_m_s2;
-
-	_accel_report.temperature_raw = report.temp;
-	_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
-
-	_gyro_report.x_raw = report.gyro_x;
-	_gyro_report.y_raw = report.gyro_y;
-	_gyro_report.z_raw = report.gyro_z;
-
-	_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	_gyro_report.scaling = _gyro_range_scale;
-	_gyro_report.range_rad_s = _gyro_range_rad_s;
-
-	_gyro_report.temperature_raw = report.temp;
-	_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
-
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-	_gyro->parent_poll_notify();
-
-	/* and publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
-	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
-	}
-
-	/* stop measuring */
-	perf_end(_sample_perf);
-}
-
-void
-MPU6000::print_info()
-{
-	printf("reads:          %u\n", _reads);
-}
-
-MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
-	CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
-	_parent(parent)
-{
-}
-
-MPU6000_gyro::~MPU6000_gyro()
-{
-}
-
-void
-MPU6000_gyro::parent_poll_notify()
-{
-	poll_notify(POLLIN);
-}
-
-ssize_t
-MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
-{
-	return _parent->gyro_read(filp, buffer, buflen);
-}
-
-int
-MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	return _parent->gyro_ioctl(filp, cmd, arg);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace mpu6000
-{
-
-MPU6000	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-
-/**
- * Start the driver.
- */
-void
-start()
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
-
-	exit(0);
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-	int fd = -1;
-	int fd_gyro = -1;
-	struct accel_report a_report;
-	struct gyro_report g_report;
-	ssize_t sz;
-
-	/* get the driver */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
-		    ACCEL_DEVICE_PATH);
-
-	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
-
-	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
-
-	/* reset to manual polling */
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
-		err(1, "reset to manual polling");
-
-	/* do a simple demand read */
-	sz = read(fd, &a_report, sizeof(a_report));
-
-	if (sz != sizeof(a_report))
-		err(1, "immediate acc read failed");
-
-	warnx("single read");
-	warnx("time:     %lld", a_report.timestamp);
-	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
-	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
-	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
-	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
-	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
-	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
-	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / 9.81f));
-
-	/* do a simple demand read */
-	sz = read(fd_gyro, &g_report, sizeof(g_report));
-
-	if (sz != sizeof(g_report))
-		err(1, "immediate gyro read failed");
-
-	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
-	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
-	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
-	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
-	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
-	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
-	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
-	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
-
-	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
-	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
-
-
-	/* XXX add poll-rate tests here too */
-
-	reset();
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
-
-	exit(0);
-}
-
-/**
- * Print a little info about the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running");
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	exit(0);
-}
-
-
-} // namespace
-
-int
-mpu6000_main(int argc, char *argv[])
-{
-	/*
-	 * Start/load the driver.
-
-	 */
-	if (!strcmp(argv[1], "start"))
-		mpu6000::start();
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		mpu6000::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		mpu6000::reset();
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info"))
-		mpu6000::info();
-
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
-}
diff --git a/apps/drivers/ms5611/Makefile b/apps/drivers/ms5611/Makefile
deleted file mode 100644
index d8e67cba2..000000000
--- a/apps/drivers/ms5611/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# MS5611 driver
-#
-
-APPNAME		 = ms5611
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
deleted file mode 100644
index 59ab5936e..000000000
--- a/apps/drivers/ms5611/ms5611.cpp
+++ /dev/null
@@ -1,1214 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C.
- */
-
-#include <nuttx/config.h>
-
-#include <drivers/device/i2c.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
-
-#include <arch/board/board.h>
-
-#include <drivers/drv_hrt.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#include <drivers/drv_baro.h>
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-/**
- * Calibration PROM as reported by the device.
- */
-#pragma pack(push,1)
-struct ms5611_prom_s {
-	uint16_t factory_setup;
-	uint16_t c1_pressure_sens;
-	uint16_t c2_pressure_offset;
-	uint16_t c3_temp_coeff_pres_sens;
-	uint16_t c4_temp_coeff_pres_offset;
-	uint16_t c5_reference_temp;
-	uint16_t c6_temp_coeff_temp;
-	uint16_t serial_and_crc;
-};
-
-/**
- * Grody hack for crc4()
- */
-union ms5611_prom_u {
-	uint16_t c[8];
-	struct ms5611_prom_s s;
-};
-#pragma pack(pop)
-
-class MS5611 : public device::I2C
-{
-public:
-	MS5611(int bus);
-	virtual ~MS5611();
-
-	virtual int		init();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void			print_info();
-
-protected:
-	virtual int		probe();
-
-private:
-	union ms5611_prom_u	_prom;
-
-	struct work_s		_work;
-	unsigned		_measure_ticks;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct baro_report	*_reports;
-
-	bool			_collect_phase;
-	unsigned		_measure_phase;
-
-	/* intermediate temperature values per MS5611 datasheet */
-	int32_t			_TEMP;
-	int64_t			_OFF;
-	int64_t			_SENS;
-
-	/* altitude conversion calibration */
-	unsigned		_msl_pressure;	/* in kPa */
-
-	orb_advert_t		_baro_topic;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_measure_perf;
-	perf_counter_t		_comms_errors;
-	perf_counter_t		_buffer_overflows;
-
-	/**
-	 * Test whether the device supported by the driver is present at a
-	 * specific address.
-	 *
-	 * @param address	The I2C bus address to probe.
-	 * @return		True if the device is present.
-	 */
-	int			probe_address(uint8_t address);
-
-	/**
-	 * Initialise the automatic measurement state machine and start it.
-	 *
-	 * @note This function is called at open and error time.  It might make sense
-	 *       to make it more aggressive about resetting the bus in case of errors.
-	 */
-	void			start_cycle();
-
-	/**
-	 * Stop the automatic measurement state machine.
-	 */
-	void			stop_cycle();
-
-	/**
-	 * Perform a poll cycle; collect from the previous measurement
-	 * and start a new one.
-	 *
-	 * This is the heart of the measurement state machine.  This function
-	 * alternately starts a measurement, or collects the data from the
-	 * previous measurement.
-	 *
-	 * When the interval between measurements is greater than the minimum
-	 * measurement interval, a gap is inserted between collection
-	 * and measurement to provide the most recent measurement possible
-	 * at the next interval.
-	 */
-	void			cycle();
-
-	/**
-	 * Static trampoline from the workq context; because we don't have a
-	 * generic workq wrapper yet.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		cycle_trampoline(void *arg);
-
-	/**
-	 * Issue a measurement command for the current state.
-	 *
-	 * @return		OK if the measurement command was successful.
-	 */
-	int			measure();
-
-	/**
-	 * Collect the result of the most recent measurement.
-	 */
-	int			collect();
-
-	/**
-	 * Send a reset command to the MS5611.
-	 *
-	 * This is required after any bus reset.
-	 */
-	int			cmd_reset();
-
-	/**
-	 * Read the MS5611 PROM
-	 *
-	 * @return		OK if the PROM reads successfully.
-	 */
-	int			read_prom();
-
-	/**
-	 * PROM CRC routine ported from MS5611 application note
-	 *
-	 * @param n_prom	Pointer to words read from PROM.
-	 * @return		True if the CRC matches.
-	 */
-	bool			crc4(uint16_t *n_prom);
-
-};
-
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
-
-/*
- * MS5611 internal constants and data structures.
- */
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-
-#define MS5611_BUS			PX4_I2C_BUS_ONBOARD
-#define MS5611_ADDRESS_1		PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
-
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
-#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
-
-
-MS5611::MS5611(int bus) :
-	I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
-	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_collect_phase(false),
-	_measure_phase(0),
-	_TEMP(0),
-	_OFF(0),
-	_SENS(0),
-	_msl_pressure(101325),
-	_baro_topic(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
-	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
-	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
-{
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// work_cancel in the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
-}
-
-MS5611::~MS5611()
-{
-	/* make sure we are truly inactive */
-	stop_cycle();
-
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-}
-
-int
-MS5611::init()
-{
-	int ret = ERROR;
-
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
-		goto out;
-
-	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct baro_report[_num_reports];
-
-	if (_reports == nullptr)
-		goto out;
-
-	_oldest_report = _next_report = 0;
-
-	/* get a publish handle on the baro topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
-
-	if (_baro_topic < 0)
-		debug("failed to create sensor_baro object");
-
-	ret = OK;
-out:
-	return ret;
-}
-
-int
-MS5611::probe()
-{
-	_retries = 10;
-
-	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
-	    (OK == probe_address(MS5611_ADDRESS_2))) {
-		/*
-	    	 * Disable retries; we may enable them selectively in some cases,
-		 * but the device gets confused if we retry some of the commands.
-	    	 */
-		_retries = 0;
-		return OK;
-	}
-
-	return -EIO;
-}
-
-int
-MS5611::probe_address(uint8_t address)
-{
-	/* select the address we are going to try */
-	set_address(address);
-
-	/* send reset command */
-	if (OK != cmd_reset())
-		return -EIO;
-
-	/* read PROM */
-	if (OK != read_prom())
-		return -EIO;
-
-	return OK;
-}
-
-ssize_t
-MS5611::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct baro_report);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1)
-		return -ENOSPC;
-
-	/* if automatic measurement is enabled */
-	if (_measure_ticks > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the workq thread while we are doing this;
-		 * we are careful to avoid racing with them.
-		 */
-		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
-	do {
-		_measure_phase = 0;
-		_oldest_report = _next_report = 0;
-
-		/* do temperature first */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* now do a pressure measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
-	} while (0);
-
-	return ret;
-}
-
-int
-MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop_cycle();
-				_measure_ticks = 0;
-				return OK;
-
-				/* external signalling not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start_cycle();
-
-					return OK;
-				}
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
-
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start_cycle();
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
-			return SENSOR_POLLRATE_MANUAL;
-
-		return (1000 / _measure_ticks);
-
-	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct baro_report *buf = new struct baro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop_cycle();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start_cycle();
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
-
-	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
-
-	case BAROIOCSMSLPRESSURE:
-
-		/* range-check for sanity */
-		if ((arg < 80000) || (arg > 120000))
-			return -EINVAL;
-
-		_msl_pressure = arg;
-		return OK;
-
-	case BAROIOCGMSLPRESSURE:
-		return _msl_pressure;
-
-	default:
-		break;
-	}
-
-	/* give it to the superclass */
-	return I2C::ioctl(filp, cmd, arg);
-}
-
-void
-MS5611::start_cycle()
-{
-
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_measure_phase = 0;
-	_oldest_report = _next_report = 0;
-
-	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
-}
-
-void
-MS5611::stop_cycle()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-MS5611::cycle_trampoline(void *arg)
-{
-	MS5611 *dev = (MS5611 *)arg;
-
-	dev->cycle();
-}
-
-void
-MS5611::cycle()
-{
-	int ret;
-
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		ret = collect();
-		if (ret != OK) {
-			if (ret == -6) {
-				/*
-				 * The ms5611 seems to regularly fail to respond to
-				 * its address; this happens often enough that we'd rather not
-				 * spam the console with the message.
-				 */
-			} else {
-				//log("collection error %d", ret);
-			}
-			/* reset the collection state machine and try again */
-			start_cycle();
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 * Don't inject one after temperature measurements, so we can keep
-		 * doing pressure measurements at something close to the desired rate.
-		 */
-		if ((_measure_phase != 0) &&
-		    (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
-				   &_work,
-				   (worker_t)&MS5611::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
-
-			return;
-		}
-	}
-
-	/* measurement phase */
-	ret = measure();
-	if (ret != OK) {
-		//log("measure error %d", ret);
-		/* reset the collection state machine and try again */
-		start_cycle();
-		return;
-	}
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&MS5611::cycle_trampoline,
-		   this,
-		   USEC2TICK(MS5611_CONVERSION_INTERVAL));
-}
-
-int
-MS5611::measure()
-{
-	int ret;
-
-	perf_begin(_measure_perf);
-
-	/*
-	 * In phase zero, request temperature; in other phases, request pressure.
-	 */
-	uint8_t	cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
-
-	/*
-	 * Send the command to begin measuring.
-	 *
-	 * Disable retries on this command; we can't know whether failure 
-	 * means the device did or did not see the write.
-	 */
-	_retries = 0;
-	ret = transfer(&cmd_data, 1, nullptr, 0);
-
-	if (OK != ret)
-		perf_count(_comms_errors);
-
-	perf_end(_measure_perf);
-
-	return ret;
-}
-
-int
-MS5611::collect()
-{
-	int ret;
-	uint8_t cmd;
-	uint8_t data[3];
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} cvt;
-
-	/* read the most recent measurement */
-	cmd = 0;
-
-	perf_begin(_sample_perf);
-
-	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-
-	ret = transfer(&cmd, 1, &data[0], 3);
-	if (ret != OK) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	/* fetch the raw value */
-	cvt.b[0] = data[2];
-	cvt.b[1] = data[1];
-	cvt.b[2] = data[0];
-	cvt.b[3] = 0;
-	uint32_t raw = cvt.w;
-
-	/* handle a measurement */
-	if (_measure_phase == 0) {
-
-		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
-
-		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
-
-		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
-
-		/* temperature compensation */
-		if (_TEMP < 2000) {
-
-			int32_t T2 = POW2(dT) >> 31;
-
-			int64_t f = POW2((int64_t)_TEMP - 2000);
-			int64_t OFF2 = 5 * f >> 1;
-			int64_t SENS2 = 5 * f >> 2;
-
-			if (_TEMP < -1500) {
-				int64_t f2 = POW2(_TEMP + 1500);
-				OFF2 += 7 * f2;
-				SENS2 += 11 * f2 >> 1;
-			}
-
-			_TEMP -= T2;
-			_OFF  -= OFF2;
-			_SENS -= SENS2;
-		}
-
-	} else {
-
-		/* pressure calculation, result in Pa */
-		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
-
-		/* generate a new report */
-		_reports[_next_report].temperature = _TEMP / 100.0f;
-		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
-
-		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
-
-		/*
-		 * PERFORMANCE HINT:
-		 *
-		 * The single precision calculation is 50 microseconds faster than the double
-		 * precision variant. It is however not obvious if double precision is required.
-		 * Pending more inspection and tests, we'll leave the double precision variant active.
-		 *
-		 * Measurements:
-		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
-		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
-		 */
-#if 0/* USE_FLOAT */
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
-		const float a  = -6.5f / 1000f;	/* temperature gradient in degrees per metre */
-		const float g  = 9.80665f;	/* gravity constant in m/s/s */
-		const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		float p1 = _msl_pressure / 1000.0f;
-
-		/* measured pressure in kPa */
-		float p = P / 1000.0f;
-
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#else
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
-		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
-		const double g  = 9.80665;	/* gravity constant in m/s/s */
-		const double R  = 287.05;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		double p1 = _msl_pressure / 1000.0;
-
-		/* measured pressure in kPa */
-		double p = P / 1000.0;
-
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#endif
-		/* publish it */
-		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
-
-		/* post a report to the ring - note, not locked */
-		INCREMENT(_next_report, _num_reports);
-
-		/* if we are running up against the oldest report, toss it */
-		if (_next_report == _oldest_report) {
-			perf_count(_buffer_overflows);
-			INCREMENT(_oldest_report, _num_reports);
-		}
-
-		/* notify anyone waiting for data */
-		poll_notify(POLLIN);
-	}
-
-	/* update the measurement state machine */
-	INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
-
-	perf_end(_sample_perf);
-
-	return OK;
-}
-
-int
-MS5611::cmd_reset()
-{
-	unsigned	old_retrycount = _retries;
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
-
-	/* bump the retry count */
-	_retries = 10;
-	result = transfer(&cmd, 1, nullptr, 0);
-	_retries = old_retrycount;
-
-	return result;
-}
-
-int
-MS5611::read_prom()
-{
-	uint8_t		prom_buf[2];
-	union {
-		uint8_t		b[2];
-		uint16_t	w;
-	} cvt;
-
-	/*
-	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
-	 * called immediately after reset.
-	 */
-	usleep(3000);
-
-	/* read and convert PROM words */
-	for (int i = 0; i < 8; i++) {
-		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
-
-		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
-			break;
-
-		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		cvt.b[0] = prom_buf[1];
-		cvt.b[1] = prom_buf[0];
-		_prom.c[i] = cvt.w;
-	}
-
-	/* calculate CRC and return success/failure accordingly */
-	return crc4(&_prom.c[0]) ? OK : -EIO;
-}
-
-bool
-MS5611::crc4(uint16_t *n_prom)
-{
-	int16_t cnt;
-	uint16_t n_rem;
-	uint16_t crc_read;
-	uint8_t n_bit;
-
-	n_rem = 0x00;
-
-	/* save the read crc */
-	crc_read = n_prom[7];
-
-	/* remove CRC byte */
-	n_prom[7] = (0xFF00 & (n_prom[7]));
-
-	for (cnt = 0; cnt < 16; cnt++) {
-		/* uneven bytes */
-		if (cnt & 1) {
-			n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
-
-		} else {
-			n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
-		}
-
-		for (n_bit = 8; n_bit > 0; n_bit--) {
-			if (n_rem & 0x8000) {
-				n_rem = (n_rem << 1) ^ 0x3000;
-
-			} else {
-				n_rem = (n_rem << 1);
-			}
-		}
-	}
-
-	/* final 4 bit remainder is CRC value */
-	n_rem = (0x000F & (n_rem >> 12));
-	n_prom[7] = crc_read;
-
-	/* return true if CRCs match */
-	return (0x000F & crc_read) == (n_rem ^ 0x00);
-}
-
-void
-MS5611::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-	printf("TEMP:           %d\n", _TEMP);
-	printf("SENS:           %lld\n", _SENS);
-	printf("OFF:            %lld\n", _OFF);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
-
-	printf("factory_setup             %u\n", _prom.s.factory_setup);
-	printf("c1_pressure_sens          %u\n", _prom.s.c1_pressure_sens);
-	printf("c2_pressure_offset        %u\n", _prom.s.c2_pressure_offset);
-	printf("c3_temp_coeff_pres_sens   %u\n", _prom.s.c3_temp_coeff_pres_sens);
-	printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
-	printf("c5_reference_temp         %u\n", _prom.s.c5_reference_temp);
-	printf("c6_temp_coeff_temp        %u\n", _prom.s.c6_temp_coeff_temp);
-	printf("serial_and_crc            %u\n", _prom.s.serial_and_crc);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace ms5611
-{
-
-MS5611	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-void	calibrate(unsigned altitude);
-
-/**
- * Start the driver.
- */
-void
-start()
-{
-	int fd;
-
-	if (g_dev != nullptr)
-		errx(1, "already started");
-
-	/* create the driver */
-	g_dev = new MS5611(MS5611_BUS);
-
-	if (g_dev == nullptr)
-		goto fail;
-
-	if (OK != g_dev->init())
-		goto fail;
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
-
-	exit(0);
-
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	errx(1, "driver start failed");
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test()
-{
-	struct baro_report report;
-	ssize_t sz;
-	int ret;
-
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
-
-	/* do a simple demand read */
-	sz = read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report))
-		err(1, "immediate read failed");
-
-	warnx("single read");
-	warnx("pressure:    %10.4f", (double)report.pressure);
-	warnx("altitude:    %11.4f", (double)report.altitude);
-	warnx("temperature: %8.4f", (double)report.temperature);
-	warnx("time:        %lld", report.timestamp);
-
-	/* set the queue depth to 10 */
-	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
-		errx(1, "failed to set queue depth");
-
-	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
-		errx(1, "failed to set 2Hz poll rate");
-
-	/* read the sensor 5x and report each value */
-	for (unsigned i = 0; i < 5; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 2000);
-
-		if (ret != 1)
-			errx(1, "timed out waiting for sensor data");
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report))
-			err(1, "periodic read failed");
-
-		warnx("periodic read %u", i);
-		warnx("pressure:    %10.4f", (double)report.pressure);
-		warnx("altitude:    %11.4f", (double)report.altitude);
-		warnx("temperature: %8.4f", (double)report.temperature);
-		warnx("time:        %lld", report.timestamp);
-	}
-
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "failed ");
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
-
-	exit(0);
-}
-
-/**
- * Print a little info about the driver.
- */
-void
-info()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running");
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	exit(0);
-}
-
-/**
- * Calculate actual MSL pressure given current altitude
- */
-void
-calibrate(unsigned altitude)
-{
-	struct baro_report report;
-	float	pressure;
-	float	p1;
-
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
-
-	/* start the sensor polling at max */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
-		errx(1, "failed to set poll rate");
-
-	/* average a few measurements */
-	pressure = 0.0f;
-
-	for (unsigned i = 0; i < 20; i++) {
-		struct pollfd fds;
-		int ret;
-		ssize_t sz;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 1000);
-
-		if (ret != 1)
-			errx(1, "timed out waiting for sensor data");
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report))
-			err(1, "sensor read failed");
-
-		pressure += report.pressure;
-	}
-
-	pressure /= 20;		/* average */
-	pressure /= 10;		/* scale from millibar to kPa */
-
-	/* tropospheric properties (0-11km) for standard atmosphere */
-	const float T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
-	const float a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
-	const float g  = 9.80665f;	/* gravity constant in m/s/s */
-	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
-
-	warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
-
-	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
-
-	warnx("calculated MSL pressure %10.4fkPa", p1);
-
-	/* save as integer Pa */
-	p1 *= 1000.0f;
-
-	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
-		err(1, "BAROIOCSMSLPRESSURE");
-
-	exit(0);
-}
-
-} // namespace
-
-int
-ms5611_main(int argc, char *argv[])
-{
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(argv[1], "start"))
-		ms5611::start();
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test"))
-		ms5611::test();
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset"))
-		ms5611::reset();
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info"))
-		ms5611::info();
-
-	/*
-	 * Perform MSL pressure calibration given an altitude in metres
-	 */
-	if (!strcmp(argv[1], "calibrate")) {
-		if (argc < 2)
-			errx(1, "missing altitude");
-
-		long altitude = strtol(argv[2], nullptr, 10);
-
-		ms5611::calibrate(altitude);
-	}
-
-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
-}