Merge remote-tracking branch 'upstream/master'

17 files changed, 249 insertions, 195 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 7f1b21a95..7d4b7d880 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -55,6 +55,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
@@ -98,7 +102,7 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_create().
  */
@@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			
+
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 6b14f5945..d0253a8d1 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -571,7 +571,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -595,7 +595,7 @@ BlinkM::led()
 					led_color_8 = LED_BLUE;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;
@@ -647,14 +647,14 @@ BlinkM::led()
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
+							if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
 							/* TODO: add other Auto modes */
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
 							else
 								led_color_4 = LED_OFF;
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 188885be2..882ec6aa2 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/drv_accel.h b/src/drivers/drv_accel.h
index 1eca6155b..52e024c91 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -97,6 +97,8 @@ ORB_DECLARE(sensor_accel);
 /** set the accel internal sample rate to at least (arg) Hz */
 #define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
 
+#define ACCEL_SAMPLERATE_DEFAULT    1000003	/**< default sample rate */
+
 /** return the accel internal sample rate in Hz */
 #define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
 
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 5e0334a18..1f2bc35c4 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -93,6 +93,8 @@ ORB_DECLARE(sensor_gyro);
 /** set the gyro internal sample rate to at least (arg) Hz */
 #define GYROIOCSSAMPLERATE	_GYROIOC(0)
 
+#define GYRO_SAMPLERATE_DEFAULT    1000003	/**< default sample rate */
+
 /** return the gyro internal sample rate in Hz */
 #define GYROIOCGSAMPLERATE	_GYROIOC(1)
 
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 058425676..767ff5803 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,8 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
@@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[])
 int
 HIL::set_mode(Mode mode)
 {
-	/* 
+	/*
 	 * Configure for PWM output.
 	 *
 	 * Note that regardless of the configured mode, the task is always
@@ -269,19 +271,19 @@ HIL::set_mode(Mode mode)
 		/* 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 */
@@ -514,12 +516,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		g_hil->set_pwm_rate(arg);
 		break;
 
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		break;
 
 	case PWM_SERVO_SET(2):
@@ -660,7 +662,7 @@ int
 hil_new_mode(PortMode new_mode)
 {
 	// uint32_t gpio_bits;
-	
+
 
 //	/* reset to all-inputs */
 //	g_hil->ioctl(0, GPIO_RESET, 0);
@@ -692,17 +694,17 @@ hil_new_mode(PortMode new_mode)
 		/* 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;
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index f583bced4..547bb6868 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -784,8 +784,9 @@ L3GD20::set_samplerate(unsigned frequency)
 {
 	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
 
-	if (frequency == 0)
+	if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) {
 		frequency = _is_l3g4200d ? 800 : 760;
+	}
 
 	/*
 	 * Use limits good for H or non-H models. Rates are slightly different
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 2a0bf522b..6edb9d787 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1307,8 +1307,9 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG1_RATE_BITS_A;
 
-	if (frequency == 0)
+	if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
 		frequency = 1600;
+	}
 
 	if (frequency <= 100) {
 		setbits |= REG1_RATE_100HZ_A;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 15412984b..1331744ae 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,6 +74,7 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index e322e8b3a..1c4dfb89e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -402,7 +402,7 @@ private:
 	/*
 	  set sample rate (approximate) - 1kHz to 5Hz
 	*/
-	void _set_sample_rate(uint16_t desired_sample_rate_hz);
+	void _set_sample_rate(unsigned desired_sample_rate_hz);
 
 	/*
 	  check that key registers still have the right value
@@ -794,13 +794,19 @@ MPU6000::probe()
   set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
 */
 void
-MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz)
 {
-  uint8_t div = 1000 / desired_sample_rate_hz;
-  if(div>200) div=200;
-  if(div<1) div=1;
-  write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
-  _sample_rate = 1000 / div;
+	if (desired_sample_rate_hz == 0 ||
+			desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT ||
+			desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) {
+		desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE;
+	}
+
+	uint8_t div = 1000 / desired_sample_rate_hz;
+	if(div>200) div=200;
+	if(div<1) div=1;
+	write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
+	_sample_rate = 1000 / div;
 }
 
 /*
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 75b1f65fd..31dbdbcd3 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;
-	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");
+	if (bus.dev != nullptr)
+		errx(1,"bus option already started");
 
+	prom_u prom_buf;
+	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");
 	}
 
-	exit(0);
+	close(fd);
+	return true;
+}
 
-fail:
 
-	if (g_dev_int != nullptr) {
-		delete g_dev_int;
-		g_dev_int = nullptr;
-	}
+/**
+ * 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;
 
-	if (g_dev_ext != nullptr) {
-		delete g_dev_ext;
-		g_dev_ext = nullptr;
+	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]);
 	}
 
-	errx(1, "driver start failed");
+	if (!started)
+		errx(1, "driver start failed");
+
+	// one or more drivers started OK
+	exit(0);
+}
+
+
+/**
+ * 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();
-	}
-
-	if (g_dev_int) {
-		warnx("int:");
-		g_dev_int->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();
+		}
 	}
-
 	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 3f1f6c473..c8211b8dd 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 87d9b94a6..ca651409f 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 5234ce8d6..554a1d659 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/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 363d2cdcf..8fcdc8023 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -70,6 +70,10 @@
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
@@ -138,11 +142,11 @@ private:
 
 	uint32_t	_groups_required;
 	uint32_t	_groups_subscribed;
-	int		_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
-	actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t	_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
+	int		_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	int		_actuator_output_topic_instance;
-	pollfd	_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
@@ -508,7 +512,7 @@ PX4FMU::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	_poll_fds_num = 0;
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
@@ -585,7 +589,7 @@ PX4FMU::task_main()
 			}
 
 			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					orb_set_interval(_control_subs[i], update_rate_in_ms);
 				}
@@ -612,7 +616,7 @@ PX4FMU::task_main()
 
 			/* get controls for required topics */
 			unsigned poll_id = 0;
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -745,7 +749,7 @@ PX4FMU::task_main()
 
 	}
 
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
@@ -1142,7 +1146,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		 * and PWM under control of the flight config
 		 * parameters. Note that this does not allow for
 		 * changing a set of pins to be used for serial on
-		 * FMUv1 
+		 * FMUv1
 		 */
 		switch (arg) {
 		case 0:
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 653d0d5d7..3a9e8ef24 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -75,6 +75,10 @@
 #include <systemlib/circuit_breaker.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
@@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state()
 
 		if (armed.ready_to_arm) {
 			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-			
+
 		} else {
 			clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 		}
@@ -2585,7 +2589,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_RC_CONFIG: {
 		/* enable setting of RC configuration without relying
-		   on param_get() 
+		   on param_get()
 		*/
 		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
 		if (config->channel >= RC_INPUT_MAX_CHANNELS) {
@@ -3267,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")) {
 
@@ -3396,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/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index fdaa7f27b..5b945e452 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -54,6 +54,7 @@
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -65,7 +66,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
 	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
 	_controlPoll(),
-	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_actuators(ORB_ID(actuator_controls_0), 20),
 	_motor1Position(0),
 	_motor1Speed(0),
 	_motor1Overflow(0),
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	uint8_t rbuf[50];
 	usleep(5000);
 	int nread = read(_uart, rbuf, 50);
-	if (nread < 6) { 
+	if (nread < 6) {
 		printf("failed to read\n");
 		return -1;
 	}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
 	uint8_t checksum = rbuf[5];
-	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
 		checksum_mask;
 	// check if checksum is valid
 	if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
 	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
-		_motor1Position = float(int64_t(count) + 
+		_motor1Position = float(int64_t(count) +
 			_motor1Overflow*overflowAmount)/_pulsesPerRev;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
-		_motor2Position = float(int64_t(count) + 
+		_motor2Position = float(int64_t(count) +
 			_motor2Overflow*overflowAmount)/_pulsesPerRev;
 	}
 	return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	return -1;
 }
 
-int RoboClaw::resetEncoders() 
+int RoboClaw::resetEncoders()
 {
 	uint16_t sum = 0;
 	return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 	return sum;
 }
 
-int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 		size_t n_data, uint16_t & prev_sum)
 {
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 	return write(_uart, buf, n_data + 3);
 }
 
-int roboclawTest(const char *deviceName, uint8_t address, 
+int roboclawTest(const char *deviceName, uint8_t address,
 		uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");