diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 4e528f8cdf..f30733694e 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -24,18 +24,18 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
-#MODULES		+= drivers/ll40ls
+MODULES		+= drivers/ll40ls
 MODULES		+= drivers/trone
-#MODULES		+= drivers/mb12xx
+MODULES		+= drivers/mb12xx
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-#MODULES		+= drivers/blinkm
+MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/airspeed
-#MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
-#MODULES		+= drivers/frsky_telemetry
+MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 
 #
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index aa2e01229a..256c2a6365 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -27,15 +27,15 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
-# MODULES		+= drivers/sf0x
+MODULES		+= drivers/sf0x
 MODULES		+= drivers/ll40ls
-# MODULES		+= drivers/trone
+MODULES		+= drivers/trone
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-# MODULES		+= drivers/hott
-# MODULES		+= drivers/hott/hott_telemetry
-# MODULES		+= drivers/hott/hott_sensors
-# MODULES		+= drivers/blinkm
+MODULES		+= drivers/hott
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
+MODULES		+= drivers/blinkm
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index c360f722bb..22563838bd 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -26,6 +26,9 @@ MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/pca8574
 MODULES		+= drivers/roboclaw
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
@@ -33,6 +36,7 @@ MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
 MODULES		+= systemcmds/top
+MODULES		+= modules/sensors
 
 #
 # System commands
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 188885be2d..882ec6aa2d 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -114,7 +114,6 @@ __BEGIN_DECLS
  * Note that these are unshifted addresses.
  */
 #define PX4_I2C_OBDEV_HMC5883	0x1e
-#define PX4_I2C_OBDEV_MS5611	0x76
 #define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
 #define PX4_I2C_OBDEV_LED	0x55
 
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 75b1f65fdb..31dbdbcd3f 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -69,6 +69,14 @@
 
 #include "ms5611.h"
 
+enum MS5611_BUS {
+	MS5611_BUS_ALL = 0,
+	MS5611_BUS_I2C_INTERNAL,
+	MS5611_BUS_I2C_EXTERNAL,
+	MS5611_BUS_SPI_INTERNAL,
+	MS5611_BUS_SPI_EXTERNAL
+};
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -784,15 +792,38 @@ MS5611::print_info()
 namespace ms5611
 {
 
-/* initialize explicitely for clarity */
-MS5611	*g_dev_ext = nullptr;
-MS5611	*g_dev_int = nullptr;
+/*
+  list of supported bus configurations
+ */
+struct ms5611_bus_option {
+	enum MS5611_BUS busid;
+	const char *devpath;
+	MS5611_constructor interface_constructor;
+	uint8_t busnum;
+	MS5611 *dev;
+} bus_options[] = {
+#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT)
+	{ MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL },
+#endif
+#ifdef PX4_SPIDEV_BARO
+	{ MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL },
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	{ MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL },
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION
+	{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#endif
+};
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
 
-void	start(bool external_bus);
-void	test(bool external_bus);
-void	reset(bool external_bus);
+bool	start_bus(struct ms5611_bus_option &bus);
+struct ms5611_bus_option &find_bus(enum MS5611_BUS busid);
+void	start(enum MS5611_BUS busid);
+void	test(enum MS5611_BUS busid);
+void	reset(enum MS5611_BUS busid);
 void	info();
-void	calibrate(unsigned altitude, bool external_bus);
+void	calibrate(unsigned altitude, enum MS5611_BUS busid);
 void	usage();
 
 /**
@@ -845,89 +876,87 @@ crc4(uint16_t *n_prom)
 /**
  * Start the driver.
  */
-void
-start(bool external_bus)
+bool
+start_bus(struct ms5611_bus_option &bus)
 {
-	int fd;
+	if (bus.dev != nullptr)
+		errx(1,"bus option already started");
+
 	prom_u prom_buf;
-
-	if (external_bus && (g_dev_ext != nullptr)) {
-		/* if already started, the still command succeeded */
-		errx(0, "ext already started");
-	} else if (!external_bus && (g_dev_int != nullptr)) {
-		/* if already started, the still command succeeded */
-		errx(0, "int already started");
-	}
-
-	device::Device *interface = nullptr;
-
-	/* create the driver, try SPI first, fall back to I2C if unsuccessful */
-	if (MS5611_spi_interface != nullptr)
-            interface = MS5611_spi_interface(prom_buf, external_bus);
-	if (interface == nullptr && (MS5611_i2c_interface != nullptr))
-		interface = MS5611_i2c_interface(prom_buf);
-
-	if (interface == nullptr)
-		errx(1, "failed to allocate an interface");
-
+	device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
 	if (interface->init() != OK) {
 		delete interface;
-		errx(1, "interface init failed");
+		warnx("no device on bus %u", (unsigned)bus.busid);
+		return false;
 	}
 
-	if (external_bus) {
-		g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT);
-		if (g_dev_ext == nullptr) {
-			delete interface;
-			errx(1, "failed to allocate driver");
-		}
-
-		if (g_dev_ext->init() != OK)
-			goto fail;
-
-		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
-
-	} else {
-
-		g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT);
-		if (g_dev_int == nullptr) {
-			delete interface;
-			errx(1, "failed to allocate driver");
-		}
-
-		if (g_dev_int->init() != OK)
-			goto fail;
-
-		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
-
+	bus.dev = new MS5611(interface, prom_buf, bus.devpath);
+	if (bus.dev != nullptr && OK != bus.dev->init()) {
+		delete bus.dev;
+		bus.dev = NULL;
+		return false;
 	}
+			
+	int fd = open(bus.devpath, O_RDONLY);
 
 	/* set the poll rate to default, starts automatic data collection */
-
-	if (fd < 0) {
-		warnx("can't open baro device");
-		goto fail;
+	if (fd == -1) {
+		errx(1, "can't open baro device");
 	}
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		warnx("failed setting default poll rate");
-		goto fail;
+		close(fd);
+		errx(1, "failed setting default poll rate");
 	}
 
+	close(fd);
+	return true;
+}
+
+
+/**
+ * Start the driver.
+ *
+ * This function call only returns once the driver
+ * is either successfully up and running or failed to start.
+ */
+void
+start(enum MS5611_BUS busid)
+{
+	uint8_t i;
+	bool started = false;
+
+	for (i=0; i<NUM_BUS_OPTIONS; i++) {
+		if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
+			// this device is already started
+			continue;
+		}
+		if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
+			// not the one that is asked for
+			continue;
+		}
+		started |= start_bus(bus_options[i]);
+	}
+
+	if (!started)
+		errx(1, "driver start failed");
+
+	// one or more drivers started OK
 	exit(0);
+}
 
-fail:
 
-	if (g_dev_int != nullptr) {
-		delete g_dev_int;
-		g_dev_int = nullptr;
-	}
-
-	if (g_dev_ext != nullptr) {
-		delete g_dev_ext;
-		g_dev_ext = nullptr;
-	}
-
-	errx(1, "driver start failed");
+/**
+ * find a bus structure for a busid
+ */
+struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
+{
+	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+		if ((busid == MS5611_BUS_ALL ||
+		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
+			return bus_options[i];
+		}
+	}	
+	errx(1,"bus %u not started", (unsigned)busid);
 }
 
 /**
@@ -936,20 +965,16 @@ fail:
  * and automatic modes.
  */
 void
-test(bool external_bus)
+test(enum MS5611_BUS busid)
 {
+	struct ms5611_bus_option &bus = find_bus(busid);
 	struct baro_report report;
 	ssize_t sz;
 	int ret;
 
 	int fd;
 
-	if (external_bus) {
-		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
-	} else {
-		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
-	}
-
+	fd = open(bus.devpath, O_RDONLY);
 	if (fd < 0)
 		err(1, "open failed (try 'ms5611 start' if the driver is not running)");
 
@@ -998,6 +1023,7 @@ test(bool external_bus)
 		warnx("time:        %lld", report.timestamp);
 	}
 
+	close(fd);
 	errx(0, "PASS");
 }
 
@@ -1005,16 +1031,12 @@ test(bool external_bus)
  * Reset the driver.
  */
 void
-reset(bool external_bus)
+reset(enum MS5611_BUS busid)
 {
+	struct ms5611_bus_option &bus = find_bus(busid);
 	int fd;
 
-	if (external_bus) {
-		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
-	} else {
-		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
-	}
-
+	fd = open(bus.devpath, O_RDONLY);
 	if (fd < 0)
 		err(1, "failed ");
 
@@ -1033,19 +1055,13 @@ reset(bool external_bus)
 void
 info()
 {
-	if (g_dev_ext == nullptr && g_dev_int == nullptr)
-		errx(1, "driver not running");
-
-	if (g_dev_ext) {
-		warnx("ext:");
-		g_dev_ext->print_info();
+	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+		struct ms5611_bus_option &bus = bus_options[i];
+		if (bus.dev != nullptr) {
+			warnx("%s", bus.devpath);
+			bus.dev->print_info();
+		}
 	}
-
-	if (g_dev_int) {
-		warnx("int:");
-		g_dev_int->print_info();
-	}
-
 	exit(0);
 }
 
@@ -1053,19 +1069,16 @@ info()
  * Calculate actual MSL pressure given current altitude
  */
 void
-calibrate(unsigned altitude, bool external_bus)
+calibrate(unsigned altitude, enum MS5611_BUS busid)
 {
+	struct ms5611_bus_option &bus = find_bus(busid);
 	struct baro_report report;
 	float	pressure;
 	float	p1;
 
 	int fd;
 
-	if (external_bus) {
-		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
-	} else {
-		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
-	}
+	fd = open(bus.devpath, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "open failed (try 'ms5611 start' if the driver is not running)");
@@ -1120,6 +1133,7 @@ calibrate(unsigned altitude, bool external_bus)
 	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
 		err(1, "BAROIOCSMSLPRESSURE");
 
+	close(fd);
 	exit(0);
 }
 
@@ -1128,7 +1142,10 @@ usage()
 {
 	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
 	warnx("options:");
-	warnx("    -X    (external bus)");
+	warnx("    -X    (external I2C bus)");
+	warnx("    -I    (intternal I2C bus)");
+	warnx("    -S    (external SPI bus)");
+	warnx("    -s    (internal SPI bus)");
 }
 
 } // namespace
@@ -1136,14 +1153,23 @@ usage()
 int
 ms5611_main(int argc, char *argv[])
 {
-	bool external_bus = false;
+	enum MS5611_BUS busid = MS5611_BUS_ALL;
 	int ch;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "X")) != EOF) {
+	while ((ch = getopt(argc, argv, "XISs")) != EOF) {
 		switch (ch) {
 		case 'X':
-			external_bus = true;
+			busid = MS5611_BUS_I2C_EXTERNAL;
+			break;
+		case 'I':
+			busid = MS5611_BUS_I2C_INTERNAL;
+			break;
+		case 'S':
+			busid = MS5611_BUS_SPI_EXTERNAL;
+			break;
+		case 's':
+			busid = MS5611_BUS_SPI_INTERNAL;
 			break;
 		default:
 			ms5611::usage();
@@ -1153,29 +1179,23 @@ ms5611_main(int argc, char *argv[])
 
 	const char *verb = argv[optind];
 
-	if (argc > optind+1) {
-		if (!strcmp(argv[optind+1], "-X")) {
-			external_bus = true;
-		}
-	}
-
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(verb, "start"))
-		ms5611::start(external_bus);
+		ms5611::start(busid);
 
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(verb, "test"))
-		ms5611::test(external_bus);
+		ms5611::test(busid);
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(verb, "reset"))
-		ms5611::reset(external_bus);
+		ms5611::reset(busid);
 
 	/*
 	 * Print driver information.
@@ -1192,7 +1212,7 @@ ms5611_main(int argc, char *argv[])
 
 		long altitude = strtol(argv[optind+1], nullptr, 10);
 
-		ms5611::calibrate(altitude, external_bus);
+		ms5611::calibrate(altitude, busid);
 	}
 
 	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
index 3f1f6c4738..c8211b8dd1 100644
--- a/src/drivers/ms5611/ms5611.h
+++ b/src/drivers/ms5611/ms5611.h
@@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom);
 } /* namespace */
 
 /* interface factories */
-extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function;
-extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
-
+extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function;
+extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function;
+typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
index 87d9b94a6e..ca651409f9 100644
--- a/src/drivers/ms5611/ms5611_i2c.cpp
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -56,14 +56,6 @@
 
 #include "board_config.h"
 
-#ifdef PX4_I2C_OBDEV_MS5611
-
-#ifndef PX4_I2C_BUS_ONBOARD
-	#define MS5611_BUS		1
-#else
-	#define MS5611_BUS		PX4_I2C_BUS_ONBOARD
-#endif
-
 #define MS5611_ADDRESS_1		0x76	/* address select pins pulled high (PX4FMU series v1.6+) */
 #define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
 
@@ -74,7 +66,7 @@ device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf);
 class MS5611_I2C : public device::I2C
 {
 public:
-	MS5611_I2C(int bus, ms5611::prom_u &prom_buf);
+	MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf);
 	virtual ~MS5611_I2C();
 
 	virtual int	init();
@@ -113,12 +105,12 @@ private:
 };
 
 device::Device *
-MS5611_i2c_interface(ms5611::prom_u &prom_buf)
+MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 {
-	return new MS5611_I2C(MS5611_BUS, prom_buf);
+	return new MS5611_I2C(busnum, prom_buf);
 }
 
-MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) :
+MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) :
 	I2C("MS5611_I2C", nullptr, bus, 0, 400000),
 	_prom(prom)
 {
@@ -274,5 +266,3 @@ MS5611_I2C::_read_prom()
 	/* calculate CRC and return success/failure accordingly */
 	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
 }
-
-#endif /* PX4_I2C_OBDEV_MS5611 */
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 5234ce8d6b..554a1d6596 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -60,14 +60,14 @@
 #define DIR_WRITE			(0<<7)
 #define ADDR_INCREMENT			(1<<6)
 
-#ifdef PX4_SPIDEV_BARO
+#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO)
 
 device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus);
 
 class MS5611_SPI : public device::SPI
 {
 public:
-	MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf);
+	MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf);
 	virtual ~MS5611_SPI();
 
 	virtual int	init();
@@ -115,20 +115,21 @@ private:
 };
 
 device::Device *
-MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus)
+MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 {
-	if (external_bus) {
-		#ifdef PX4_SPI_BUS_EXT
-		return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
-		#else
+#ifdef PX4_SPI_BUS_EXT
+	if (busnum == PX4_SPI_BUS_EXT) {
+#ifdef PX4_SPIDEV_EXT_BARO
+		return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
+#else
 		return nullptr;
-		#endif
-	} else {
-		return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+#endif
 	}
+#endif
+	return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
 }
 
-MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
+MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
 	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
 	_prom(prom_buf)
 {
@@ -281,4 +282,4 @@ MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	return transfer(send, recv, len);
 }
 
-#endif /* PX4_SPIDEV_BARO */
+#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index ae7f85b870..3a9e8ef24d 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -3271,7 +3271,23 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "safety_off")) {
+		int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
+		if (ret != OK) {
+			printf("failed to disable safety\n");
+			exit(1);
+		}
+		exit(0);
+	}
 
+	if (!strcmp(argv[1], "safety_on")) {
+		int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
+		if (ret != OK) {
+			printf("failed to enable safety\n");
+			exit(1);
+		}
+		exit(0);
+	}
 
 	if (!strcmp(argv[1], "recovery")) {
 
@@ -3400,6 +3416,6 @@ px4io_main(int argc, char *argv[])
 
 out:
 	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
-	        "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
+	        "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n"
 	        "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
 }
diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index ca13d62655..af90c25600 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -66,6 +66,7 @@
 #define _SYSTEMLIB_ERR_H
 
 #include <stdarg.h>
+#include "visibility.h"
 
 __BEGIN_DECLS
 
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 6f021459ce..764e33179c 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -56,6 +56,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
+#include <systemlib/systemlib.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -682,9 +683,11 @@ namespace
 {
 
 ORBDevMaster	*g_dev;
+bool pubsubtest_passed = false;
 
 struct orb_test {
 	int val;
+	hrt_abstime time;
 };
 
 ORB_DEFINE(orb_test, struct orb_test);
@@ -718,6 +721,46 @@ test_note(const char *fmt, ...)
 	return OK;
 }
 
+int pubsublatency_main(void)
+{
+	/* poll on test topic and output latency */
+	float latency_integral = 0.0f;
+
+	/* wakeup source(s) */
+	struct pollfd fds[1];
+
+	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
+
+	fds[0].fd = test_multi_sub;
+	fds[0].events = POLLIN;
+
+	struct orb_test t;
+
+	const unsigned maxruns = 10;
+
+	for (unsigned i = 0; i < maxruns; i++) {
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
+		orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t);
+
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		hrt_abstime elt = hrt_elapsed_time(&t.time);
+		latency_integral += elt;
+	}
+
+	orb_unsubscribe(test_multi_sub);
+
+	warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns));
+
+	pubsubtest_passed = true;
+
+	return OK;
+}
+
 int
 test()
 {
@@ -779,8 +822,7 @@ test()
 	int instance0;
 	int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
 
-		test_note("advertised");
-	usleep(300000);
+	test_note("advertised");
 
 	int instance1;
 	int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
@@ -795,8 +837,7 @@ test()
 	if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
 		return test_fail("mult. pub0 fail");
 
-		test_note("published");
-	usleep(300000);
+	test_note("published");
 
 	t.val = 203;
 	if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
@@ -805,19 +846,19 @@ test()
 	/* subscribe to both topics and ensure valid data is received */
 	int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
 
-	if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u))
 		return test_fail("sub #0 copy failed: %d", errno);
 
-	if (t.val != 103)
-		return test_fail("sub #0 val. mismatch: %d", t.val);
+	if (u.val != 103)
+		return test_fail("sub #0 val. mismatch: %d", u.val);
 
 	int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
 
-	if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t))
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u))
 		return test_fail("sub #1 copy failed: %d", errno);
 
-	if (t.val != 203)
-		return test_fail("sub #1 val. mismatch: %d", t.val);
+	if (u.val != 203)
+		return test_fail("sub #1 val. mismatch: %d", u.val);
 
 	/* test priorities */
 	int prio;
@@ -833,6 +874,30 @@ test()
 	if (prio != ORB_PRIO_MIN)
 		return test_fail("prio: %d", prio);
 
+	/* test pub / sub latency */
+
+	int pubsub_task = task_spawn_cmd("uorb_latency",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       1500,
+				       (main_t)&pubsublatency_main,
+				       nullptr);
+
+	/* give the test task some data */
+	while (!pubsubtest_passed) {
+		t.val = 303;
+		t.time = hrt_absolute_time();
+		if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
+			return test_fail("mult. pub0 timing fail");
+
+		/* simulate >800 Hz system operation */
+		usleep(1000);
+	}
+
+	if (pubsub_task < 0) {
+		return test_fail("failed launching task");
+	}
+
 	return test_note("PASS");
 }
 
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 0dc333f0ad..1d728e8579 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -32,8 +32,7 @@ SRCS			 = test_adc.c \
 			   test_ppm_loopback.c \
 			   test_rc.c \
 			   test_conv.cpp \
-			   test_mount.c \
-			   test_mtd.c
+			   test_mount.c
 
 EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion
 
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
deleted file mode 100644
index 43231ccad8..0000000000
--- a/src/systemcmds/tests/test_mtd.c
+++ /dev/null
@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 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 test_mtd.c
- *
- * Param storage / file test.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <sys/stat.h>
-#include <poll.h>
-#include <dirent.h>
-#include <stdio.h>
-#include <stddef.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <systemlib/err.h>
-#include <systemlib/perf_counter.h>
-#include <string.h>
-
-#include <drivers/drv_hrt.h>
-
-#include "tests.h"
-
-#define PARAM_FILE_NAME "/fs/mtd_params"
-
-static int check_user_abort(int fd);
-static void print_fail(void);
-static void print_success(void);
-
-int check_user_abort(int fd) {
-	/* check if user wants to abort */
-	char c;
-
-	struct pollfd fds;
-	int ret;
-	fds.fd = 0; /* stdin */
-	fds.events = POLLIN;
-	ret = poll(&fds, 1, 0);
-
-	if (ret > 0) {
-
-		read(0, &c, 1);
-
-		switch (c) {
-		case 0x03: // ctrl-c
-		case 0x1b: // esc
-		case 'c':
-		case 'q':
-		{
-			warnx("Test aborted.");
-			fsync(fd);
-			close(fd);
-			return OK;
-			/* not reached */
-			}
-		}
-	}
-
-	return 1;
-}
-
-void print_fail()
-{
-	printf("<[T]: MTD: FAIL>\n");
-}
-
-void print_success()
-{
-	printf("<[T]: MTD: OK>\n");
-}
-
-int
-test_mtd(int argc, char *argv[])
-{
-	unsigned iterations= 0;
-
-	/* check if microSD card is mounted */
-	struct stat buffer;
-	if (stat(PARAM_FILE_NAME, &buffer)) {
-		warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
-		print_fail();
-		return 1;
-	}
-
-	// XXX get real storage space here
-	unsigned file_size = 4096;
-
-	/* perform tests for a range of chunk sizes */
-	unsigned chunk_sizes[] = {256, 512, 4096};
-
-	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
-
-		printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
-
-		uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
-
-		/* fill write buffer with known values */
-		for (unsigned i = 0; i < sizeof(write_buf); i++) {
-			/* this will wrap, but we just need a known value with spacing */
-			write_buf[i] = i+11;
-		}
-
-		uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
-		hrt_abstime start, end;
-
-		int fd = open(PARAM_FILE_NAME, O_RDONLY);
-		int rret = read(fd, read_buf, chunk_sizes[c]);
-		close(fd);
-		if (rret <= 0) {
-			err(1, "read error");
-		}
-
-		fd = open(PARAM_FILE_NAME, O_WRONLY);
-
-		printf("printing 2 percent of the first chunk:\n");
-		for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) {
-			printf("%02X", read_buf[i]);
-		}
-		printf("\n");
-
-		iterations = file_size / chunk_sizes[c];
-
-		start = hrt_absolute_time();
-		for (unsigned i = 0; i < iterations; i++) {
-			int wret = write(fd, write_buf, chunk_sizes[c]);
-
-			if (wret != (int)chunk_sizes[c]) {
-				warn("WRITE ERROR!");
-				print_fail();
-				return 1;
-			}
-
-			fsync(fd);
-
-			if (!check_user_abort(fd))
-				return OK;
-
-		}
-		end = hrt_absolute_time();
-
-		warnx("write took %llu us", (end - start));
-
-		close(fd);
-		fd = open(PARAM_FILE_NAME, O_RDONLY);
-
-		/* read back data for validation */
-		for (unsigned i = 0; i < iterations; i++) {
-			int rret2 = read(fd, read_buf, chunk_sizes[c]);
-
-			if (rret2 != (int)chunk_sizes[c]) {
-				warnx("READ ERROR!");
-				print_fail();
-				return 1;
-			}
-			
-			/* compare value */
-			bool compare_ok = true;
-
-			for (unsigned j = 0; j < chunk_sizes[c]; j++) {
-				if (read_buf[j] != write_buf[j]) {
-					warnx("COMPARISON ERROR: byte %d", j);
-					print_fail();
-					compare_ok = false;
-					break;
-				}
-			}
-
-			if (!compare_ok) {
-				warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
-				print_fail();
-				return 1;
-			}
-
-			if (!check_user_abort(fd))
-				return OK;
-
-		}
-
-
-		close(fd);
-
-	}
-
-	/* fill the file with 0xFF to make it look new again */
-	char ffbuf[64];
-	memset(ffbuf, 0xFF, sizeof(ffbuf));
-	int fd = open(PARAM_FILE_NAME, O_WRONLY);
-	for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) {
-		int ret = write(fd, ffbuf, sizeof(ffbuf));
-
-		if (ret != sizeof(ffbuf)) {
-			warnx("ERROR! Could not fill file with 0xFF");
-			close(fd);
-			print_fail();
-			return 1;
-		}
-	}
-
-	(void)close(fd);
-	print_success();
-
-	return 0;
-}
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index ad55e14102..69fb0e57bd 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -112,7 +112,6 @@ extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
 extern int	test_mount(int argc, char *argv[]);
-extern int	test_mtd(int argc, char *argv[]);
 extern int	test_mathlib(int argc, char *argv[]);
 
 __END_DECLS
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 0f56704e6f..2f586fa85b 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -109,7 +109,6 @@ const struct {
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"mtd",			test_mtd,	0},
 #ifndef TESTS_MATHLIB_DISABLE
 	{"mathlib",		test_mathlib,	0},
 #endif
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index ee99b5a419..a87c23be22 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -23,11 +23,14 @@ include_directories(${CMAKE_SOURCE_DIR})
 include_directories(${PX_SRC})
 include_directories(${PX_SRC}/modules)
 include_directories(${PX_SRC}/lib)
+include_directories(${PX_SRC}/drivers)
 
 add_definitions(-D__EXPORT=)
 add_definitions(-D__PX4_TESTS)
 add_definitions(-Dnoreturn_function=)
 add_definitions(-Dmain_t=int)
+add_definitions(-DERROR=-1)
+add_definitions(-DOK=0)
 
 # check
 add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp
index ee4f3d1d68..15a21c86b7 100644
--- a/unittests/sbus2_test.cpp
+++ b/unittests/sbus2_test.cpp
@@ -11,7 +11,7 @@
 #include "gtest/gtest.h"
 
 TEST(SBUS2Test, SBUS2) {
-	char *filepath = "testdata/sbus2_r7008SB.txt";
+	const char *filepath = "testdata/sbus2_r7008SB.txt";
 
 	FILE *fp;
 	fp = fopen(filepath,"rt");