Merge pull request #1734 from PX4/ros_messagelayer_merge_attctrl_posctrl

Ros messagelayer merge attctrl posctrl

30 files changed, 628 insertions, 403 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/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index 4fa24275f..0f45b6536 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 	outputs.output[3] = motor4;
 	static orb_advert_t pub = 0;
 	if (pub == 0) {
-		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+		/* advertise to channel 0 / primary */
+		pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
 	}
 
 	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
@@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 		fsync(ardrone_fd);
 
 		/* publish just written values */
-		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
+		orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
 
 		if (ret == sizeof(buf)) {
 			return OK;
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index 2b5fef4d7..92b752a28 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -91,6 +91,7 @@
 #define BATT_SMBUS_MANUFACTURE_INFO		0x25	///< cell voltage register
 #define BATT_SMBUS_CURRENT			0x2a	///< current register
 #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS	(1000000 / 10)	///< time in microseconds, measure at 10hz
+#define BATT_SMBUS_TIMEOUT_MS		10000000	///< timeout looking for battery 10seconds after startup
 
 #define BATT_SMBUS_PEC_POLYNOMIAL		0x07	///< Polynomial for calculating PEC
 
@@ -171,11 +172,13 @@ private:
 	uint8_t			get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
 
 	// internal variables
+	bool			_enabled;	///< true if we have successfully connected to battery
 	work_s			_work;		///< work queue for scheduling reads
 	RingBuffer		*_reports;	///< buffer of recorded voltages, currents
 	struct battery_status_s _last_report;	///< last published report, used for test()
 	orb_advert_t		_batt_topic;	///< uORB battery topic
 	orb_id_t		_batt_orb_id;	///< uORB battery topic ID
+	uint64_t		_start_time;	///< system time we first attempt to communicate with battery
 };
 
 namespace
@@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
 
 BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
 	I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
+	_enabled(false),
 	_work{},
 	_reports(nullptr),
 	_batt_topic(-1),
-	_batt_orb_id(nullptr)
+	_batt_orb_id(nullptr),
+	_start_time(0)
 {
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
+
+	// capture startup time
+	_start_time = hrt_absolute_time();
 }
 
 BATT_SMBUS::~BATT_SMBUS()
@@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg)
 void
 BATT_SMBUS::cycle()
 {
+	// get current time
+	uint64_t now = hrt_absolute_time();
+
+	// exit without rescheduling if we have failed to find a battery after 10 seconds
+	if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
+		warnx("did not find smart battery");
+		return;
+	}
+
 	// read data from sensor
 	struct battery_status_s new_report;
 
 	// set time of reading
-	new_report.timestamp = hrt_absolute_time();
+	new_report.timestamp = now;
 
 	// read voltage
 	uint16_t tmp;
@@ -375,6 +392,9 @@ BATT_SMBUS::cycle()
 
 		// notify anyone waiting for data
 		poll_notify(POLLIN);
+
+		// record we are working
+		_enabled = true;
 	}
 
 	// schedule a fresh cycle call when the measurement is done
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/drv_accel.h b/src/drivers/drv_accel.h
index 1f98d966b..1eca6155b 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.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
@@ -81,9 +81,7 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel0);
-ORB_DECLARE(sensor_accel1);
-ORB_DECLARE(sensor_accel2);
+ORB_DECLARE(sensor_accel);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index 3e28d3d3d..3d275d619 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.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
@@ -63,9 +63,7 @@ struct baro_report {
 /*
  * ObjDev tag for raw barometer data.
  */
-ORB_DECLARE(sensor_baro0);
-ORB_DECLARE(sensor_baro1);
-ORB_DECLARE(sensor_baro2);
+ORB_DECLARE(sensor_baro);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h
index 9c278f6c5..b757da545 100644
--- a/src/drivers/drv_blinkm.h
+++ b/src/drivers/drv_blinkm.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
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 41b013a44..5e0334a18 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.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
@@ -81,9 +81,7 @@ struct gyro_scale {
 /*
  * ObjDev tag for raw gyro data.
  */
-ORB_DECLARE(sensor_gyro0);
-ORB_DECLARE(sensor_gyro1);
-ORB_DECLARE(sensor_gyro2);
+ORB_DECLARE(sensor_gyro);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 5ddf5d08e..193c816e0 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.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
@@ -44,6 +44,7 @@
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
+
 #define MAG_DEVICE_PATH		"/dev/mag"
 
 /**
@@ -79,15 +80,8 @@ struct mag_scale {
 /*
  * ObjDev tag for raw magnetometer data.
  */
-ORB_DECLARE(sensor_mag0);
-ORB_DECLARE(sensor_mag1);
-ORB_DECLARE(sensor_mag2);
+ORB_DECLARE(sensor_mag);
 
-/*
- * mag device types, for _device_id
- */
-#define DRV_MAG_DEVTYPE_HMC5883 1
-#define DRV_MAG_DEVTYPE_LSM303D 2
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 713618545..e0b16fa5c 100644
--- a/src/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.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
@@ -35,7 +35,9 @@
 #define _DRV_UORB_H
 
 /**
- * @file uORB published object driver.
+ * @file drv_orb_dev.h
+ * 
+ * uORB published object driver.
  */
 
 #include <sys/types.h>
@@ -84,4 +86,7 @@
 /** Get the global advertiser handle for the topic */
 #define ORBIOCGADVERTISER	_ORBIOC(13)
 
+/** Get the priority for the topic */
+#define ORBIOCGPRIORITY		_ORBIOC(14)
+
 #endif /* _DRV_UORB_H */
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 0f8362f58..12d51aeaa 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -45,6 +45,7 @@
 #include "drv_orb_dev.h"
 
 #define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
+#define MB12XX_MAX_RANGEFINDERS	12	//Maximum number of RangeFinders that can be connected
 
 enum RANGE_FINDER_TYPE {
 	RANGE_FINDER_TYPE_LASER = 0,
@@ -67,6 +68,8 @@ struct range_finder_report {
 	float minimum_distance;			/**< minimum distance the sensor can measure */
 	float maximum_distance;			/**< maximum distance the sensor can measure */
 	uint8_t valid;				/**< 1 == within sensor range, 0 = outside sensor range */
+	float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */
+	uint8_t just_updated;			/** number of the most recent measurement sensor */
 };
 
 /**
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 5e4598de8..467dace08 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -43,6 +43,24 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "drv_device.h"
+
+/**
+ * Sensor type definitions.
+ *
+ * Used to create a unique device id for redundant and combo sensors
+ */
+
+#define DRV_MAG_DEVTYPE_HMC5883 0x01
+#define DRV_MAG_DEVTYPE_LSM303D 0x02
+#define DRV_ACC_DEVTYPE_LSM303D 0x11
+#define DRV_ACC_DEVTYPE_BMA180  0x12
+#define DRV_ACC_DEVTYPE_MPU6000 0x13
+#define DRV_GYR_DEVTYPE_MPU6000 0x21
+#define DRV_GYR_DEVTYPE_L3GD20  0x22
+#define DRV_RNG_DEVTYPE_MB12XX  0x31
+#define DRV_RNG_DEVTYPE_LL40LS  0x32
+
 /*
  * ioctl() definitions
  *
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 05dfc99b7..8157bc7e6 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
-		warnx("try baudrate: %d\n", speed);
-
 	default:
 		warnx("ERR: baudrate: %d\n", baud);
 		return -EINVAL;
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 9b5c8133b..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 */
@@ -330,8 +332,9 @@ HIL::task_main()
 	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);
+	int dummy;
+	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+				   &outputs, &dummy, ORB_PRIO_LOW);
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -423,7 +426,7 @@ HIL::task_main()
 				}
 
 				/* and publish for anyone that cares to see */
-				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+				orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
 			}
 		}
 
@@ -513,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):
@@ -659,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);
@@ -691,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/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 2a10b0063..b1605a5b0 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -66,9 +66,9 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/drv_device.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 
 #include <float.h>
 #include <getopt.h>
@@ -156,10 +156,9 @@ private:
 	float 			_range_ga;
 	bool			_collect_phase;
 	int			_class_instance;
+	int			_orb_class_instance;
 
 	orb_advert_t		_mag_topic;
-	orb_advert_t		_subsystem_pub;
-	orb_id_t		_mag_orb_id;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -347,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
 	_range_ga(1.3f),
 	_collect_phase(false),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_mag_topic(-1),
-	_subsystem_pub(-1),
-	_mag_orb_id(nullptr),
 	_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")),
@@ -418,7 +416,6 @@ HMC5883::init()
 	reset();
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
-	_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -725,6 +722,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		debug("MAGIOCGEXTERNAL in main driver");
 		return _interface->ioctl(cmd, dummy);
 
+	case DEVIOCGDEVICEID:
+		return _interface->ioctl(cmd, dummy);
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
@@ -846,6 +846,7 @@ HMC5883::collect()
 
 	perf_begin(_sample_perf);
 	struct mag_report new_report;
+	bool sensor_is_onboard = false;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	new_report.timestamp = hrt_absolute_time();
@@ -898,7 +899,8 @@ HMC5883::collect()
 
 	// XXX revisit for SPI part, might require a bus type IOCTL
 	unsigned dummy;
-	if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) {
+	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
+	if (sensor_is_onboard) {
 		// convert onboard so it matches offboard for the
 		// scaling below
 		report.y = -report.y;
@@ -921,9 +923,10 @@ HMC5883::collect()
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(_mag_orb_id, _mag_topic, &new_report);
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(_mag_orb_id, &new_report);
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
+				&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
 
 			if (_mag_topic < 0)
 				debug("ADVERT FAIL");
@@ -1181,24 +1184,6 @@ int HMC5883::check_calibration()
 		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};
-
-		if (!(_pub_blocked)) {
-			if (_subsystem_pub > 0) {
-				orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
-			} else {
-				_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
-			}
-		}
 	}
 
 	/* return 0 if calibrated, 1 else */
@@ -1305,9 +1290,9 @@ struct hmc5883_bus_option {
 	uint8_t busnum;
 	HMC5883	*dev;
 } bus_options[] = {
-	{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+	{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
 #ifdef PX4_I2C_BUS_ONBOARD
-	{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+	{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
 #endif
 #ifdef PX4_SPIDEV_HMC
 	{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
@@ -1347,13 +1332,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
 	}
 			
 	int fd = open(bus.devpath, O_RDONLY);
-	if (fd < 0)
+	if (fd < 0) {
 		return false;
+	}
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		close(fd);
 		errx(1,"Failed to setup poll rate");
 	}
+	close(fd);
 
 	return true;
 }
diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp
index 782ea62fe..f86c1af6b 100644
--- a/src/drivers/hmc5883/hmc5883_i2c.cpp
+++ b/src/drivers/hmc5883/hmc5883_i2c.cpp
@@ -53,6 +53,7 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
 
 #include "hmc5883.h"
 
@@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus)
 HMC5883_I2C::HMC5883_I2C(int bus) :
 	I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
 }
 
 HMC5883_I2C::~HMC5883_I2C()
@@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg)
 			return 0;
 		}
 
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
 	default:
 		ret = -EINVAL;
 	}
diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp
index 25a2f2b40..aec990ca8 100644
--- a/src/drivers/hmc5883/hmc5883_spi.cpp
+++ b/src/drivers/hmc5883/hmc5883_spi.cpp
@@ -53,6 +53,7 @@
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
 
 #include "hmc5883.h"
 #include <board_config.h>
@@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus)
 HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) :
 	SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
 }
 
 HMC5883_SPI::~HMC5883_SPI()
@@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg)
 			return 0;
 		}
 
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
 	default:
 	{
 		ret = -EINVAL;
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 08bc1fead..f583bced4 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -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
@@ -33,7 +33,7 @@
 
 /**
  * @file l3gd20.cpp
- * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
  *
  * Note: With the exception of the self-test feature, the ST L3G4200D is
  *       also supported by this driver.
@@ -179,6 +179,12 @@ static const int ERROR = -1;
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 #define L3GD20_TEMP_OFFSET_CELSIUS		40
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 #ifndef SENSOR_BOARD_ROTATION_DEFAULT
 #define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
 #endif
@@ -214,14 +220,14 @@ private:
 
 	struct hrt_call		_call;
 	unsigned		_call_interval;
-	
+
 	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
-	orb_id_t		_orb_id;
+	int			_orb_class_instance;
 	int			_class_instance;
 
 	unsigned		_current_rate;
@@ -274,6 +280,13 @@ private:
 	void			disable_i2c();
 
 	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
-	_orb_id(nullptr),
+	_orb_class_instance(-1),
 	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -411,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
+
 	// default scale factors
 	_gyro_scale.x_offset = 0;
 	_gyro_scale.x_scale  = 1.0f;
@@ -456,20 +471,6 @@ L3GD20::init()
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-	switch (_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_orb_id = ORB_ID(sensor_gyro0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_orb_id = ORB_ID(sensor_gyro1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_orb_id = ORB_ID(sensor_gyro2);
-			break;
-	}
-
 	reset();
 
 	measure();
@@ -478,7 +479,8 @@ L3GD20::init()
 	struct gyro_report grp;
 	_reports->get(&grp);
 
-	_gyro_topic = orb_advertise(_orb_id, &grp);
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
+		&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
 
 	if (_gyro_topic < 0) {
 		debug("failed to create sensor_gyro publication");
@@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
@@ -867,7 +869,7 @@ L3GD20::reset()
 	disable_i2c();
 
 	/* set default configuration */
-	write_checked_reg(ADDR_CTRL_REG1, 
+	write_checked_reg(ADDR_CTRL_REG1,
                           REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
 	write_checked_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
 	write_checked_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
@@ -911,7 +913,7 @@ L3GD20::check_registers(void)
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -974,7 +976,7 @@ L3GD20::measure()
               we waited for DRDY, but did not see DRDY on all axes
               when we captured. That means a transfer error of some sort
              */
-            perf_count(_errors);            
+            perf_count(_errors);
             return;
         }
 #endif
@@ -994,7 +996,7 @@ L3GD20::measure()
 	 */
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_bad_registers);
-	
+
 	switch (_orientation) {
 
 		case SENSOR_BOARD_ROTATION_000_DEG:
@@ -1050,7 +1052,7 @@ L3GD20::measure()
 	/* publish for subscribers */
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_orb_id, _gyro_topic, &report);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
 	}
 
 	_read++;
@@ -1072,7 +1074,7 @@ L3GD20::print_info()
         for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i]);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 57754e4c0..2a0bf522b 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -211,6 +211,12 @@ static const int ERROR = -1;
 
 #define LSM303D_ONE_G					9.80665f
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
 
@@ -275,7 +281,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
-	orb_id_t		_accel_orb_id;
+	int			_accel_orb_class_instance;
 	int			_accel_class_instance;
 
 	unsigned		_accel_read;
@@ -295,7 +301,7 @@ private:
 
 	enum Rotation		_rotation;
 
-	// values used to 
+	// values used to
 	float			_last_accel[3];
 	uint8_t			_constant_accel_count;
 
@@ -330,6 +336,13 @@ private:
 	void			disable_i2c();
 
 	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -507,7 +520,7 @@ private:
 	LSM303D				*_parent;
 
 	orb_advert_t			_mag_topic;
-	orb_id_t			_mag_orb_id;
+	int				_mag_orb_class_instance;
 	int				_mag_class_instance;
 
 	void				measure();
@@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
-	_accel_orb_id(nullptr),
+	_accel_orb_class_instance(-1),
 	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
@@ -556,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_constant_accel_count(0),
 	_checked_next(0)
 {
-	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
 
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
+	
+	/* Prime _mag with parents devid. */
+	_mag->_device_id.devid = _device_id.devid;
+	_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
+
 	// default scale factors
 	_accel_scale.x_offset = 0.0f;
 	_accel_scale.x_scale  = 1.0f;
@@ -618,7 +638,6 @@ LSM303D::init()
 	if (_accel_reports == nullptr)
 		goto out;
 
-	/* advertise accel topic */
 	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
@@ -641,26 +660,14 @@ LSM303D::init()
 	_mag_reports->get(&mrp);
 
 	/* measurement will have generated a report, publish */
-	switch (_mag->_mag_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag2);
-			break;
-	}
-
-	_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
+	_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
+		&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
 
 	if (_mag->_mag_topic < 0) {
 		warnx("ADVERT ERR");
 	}
 
+
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
 	/* advertise sensor topic, measure manually to initialize valid report */
@@ -668,21 +675,8 @@ LSM303D::init()
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
-	switch (_accel_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_accel_orb_id = ORB_ID(sensor_accel0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_accel_orb_id = ORB_ID(sensor_accel1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_accel_orb_id = ORB_ID(sensor_accel2);
-			break;
-	}
-
-	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
+		&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
 
 	if (_accel_topic < 0) {
 		warnx("ADVERT ERR");
@@ -712,7 +706,7 @@ LSM303D::reset()
 	disable_i2c();
 
 	/* enable accel*/
-	write_checked_reg(ADDR_CTRL_REG1, 
+	write_checked_reg(ADDR_CTRL_REG1,
 			  REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
 
 	/* enable mag */
@@ -746,7 +740,7 @@ LSM303D::probe()
 
 	/* verify that the device is attached and functioning */
 	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
-	
+
 	if (success) {
 		_checked_values[0] = WHO_I_AM;
 		return OK;
@@ -1019,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return SENSOR_POLLRATE_MANUAL;
 
 		return 1000000 / _call_mag_interval;
-	
+
 	case SENSORIOCSQUEUEDEPTH: {
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
@@ -1424,7 +1418,7 @@ LSM303D::check_registers(void)
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -1548,7 +1542,7 @@ LSM303D::measure()
 		perf_count(_bad_values);
 		_constant_accel_count = 0;
 	}
-	    
+
 	_last_accel[0] = x_in_new;
 	_last_accel[1] = y_in_new;
 	_last_accel[2] = z_in_new;
@@ -1570,7 +1564,7 @@ LSM303D::measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_accel_orb_id, _accel_topic, &accel_report);
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1641,7 +1635,7 @@ LSM303D::mag_measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
@@ -1666,7 +1660,7 @@ LSM303D::print_info()
         for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i]);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
@@ -1742,7 +1736,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
 	_mag_topic(-1),
-	_mag_orb_id(nullptr),
+	_mag_orb_class_instance(-1),
 	_mag_class_instance(-1)
 {
 }
@@ -1783,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
 int
 LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->mag_ioctl(filp, cmd, arg);
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->mag_ioctl(filp, cmd, arg);
+	}
 }
 
 void
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 9cf568658..a4fb7bcc2 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -34,6 +34,7 @@
 /**
  * @file mb12xx.cpp
  * @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
  *
  * Driver for the Maxbotix sonar range finders connected via I2C.
  */
@@ -54,6 +55,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <vector>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -72,7 +74,7 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
+#define MB12XX_BUS 		PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
 #define MB12XX_DEVICE_PATH	"/dev/mb12xx"
 
@@ -83,10 +85,12 @@
 #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_MIN_DISTANCE 	(0.20f)
+#define MB12XX_MAX_DISTANCE 	(7.65f)
+
+#define MB12XX_CONVERSION_INTERVAL 	100000 /* 60ms for one sonar */
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* 30ms between each sonar measurement (watch out for interference!) */
 
-#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -133,12 +137,19 @@ private:
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
 
+	uint8_t				_cycle_counter;	/* counter in cycle to change i2c adresses */
+	int					_cycling_rate;	/* */
+	uint8_t				_index_counter;	/* temporary sonar i2c address */
+	std::vector<uint8_t>	addr_ind; 	/* temp sonar i2c address vector */
+	std::vector<float>	_latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+
+
 	/**
 	* 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.
+	* @return			True if the device is present.
 	*/
 	int					probe_address(uint8_t address);
 
@@ -178,7 +189,7 @@ private:
 	*
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
-	static void		cycle_trampoline(void *arg);
+	static void			cycle_trampoline(void *arg);
 
 
 };
@@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) :
 	_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"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
+	_cycle_counter(0),	/* initialising counter for cycling function to zero */
+	_cycling_rate(0),	/* initialising cycling rate (which can differ depending on one sonar or multiple) */
+	_index_counter(0) 	/* initialising temp sonar i2c address to zero */
+
 {
-	// enable debug() calls
+	/* enable debug() calls */
 	_debug_enabled = false;
 
-	// work_cancel in the dtor will explode if we don't do this...
+	/* work_cancel in the dtor will explode if we don't do this... */
 	memset(&_work, 0, sizeof(_work));
 }
 
@@ -223,7 +238,7 @@ MB12XX::~MB12XX()
 		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
 	}
 
-	// free perf counters
+	/* free perf counters */
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
 	perf_free(_buffer_overflows);
@@ -242,6 +257,9 @@ MB12XX::init()
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
+	_index_counter = MB12XX_BASEADDR;	/* set temp sonar i2c address to base adress */
+	set_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
+
 	if (_reports == nullptr) {
 		goto out;
 	}
@@ -250,16 +268,51 @@ MB12XX::init()
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 		/* get a publish handle on the range finder topic */
-		struct range_finder_report rf_report;
-		measure();
-		_reports->get(&rf_report);
+		struct range_finder_report rf_report = {};
+
 		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
 
 		if (_range_finder_topic < 0) {
-			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+			log("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
+
+	// XXX we should find out why we need to wait 200 ms here
+	usleep(200000);
+
+	/* check for connected rangefinders on each i2c port:
+	   We start from i2c base address (0x70 = 112) and count downwards
+	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
+	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
+		_index_counter = MB12XX_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */
+		set_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
+		int ret2 = measure();
+
+		if (ret2 == 0) { /* sonar is present -> store address_index in array */
+			addr_ind.push_back(_index_counter);
+			debug("sonar added");
+			_latest_sonar_measurements.push_back(200);
 		}
 	}
 
+	_index_counter = MB12XX_BASEADDR;
+	set_address(_index_counter); /* set i2c port back to base adress for rest of driver */
+
+	/* if only one sonar detected, no special timing is required between firing, so use default */
+	if (addr_ind.size() == 1) {
+		_cycling_rate = MB12XX_CONVERSION_INTERVAL;
+
+	} else {
+		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	/* show the connected sonars in terminal */
+	for (unsigned i = 0; i < addr_ind.size(); i++) {
+		log("sonar %d with address %d added", (i + 1), addr_ind[i]);
+	}
+
+	debug("Number of sonars connected: %d", addr_ind.size());
+
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
@@ -325,11 +378,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
+					_measure_ticks = USEC2TICK(_cycling_rate);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
 						start();
+
 					}
 
 					return OK;
@@ -341,10 +395,10 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+					int ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+					if (ticks < USEC2TICK(_cycling_rate)) {
 						return -EINVAL;
 					}
 
@@ -414,6 +468,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 {
+
 	unsigned count = buflen / sizeof(struct range_finder_report);
 	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
 	int ret = 0;
@@ -453,7 +508,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* wait for it to complete */
-		usleep(MB12XX_CONVERSION_INTERVAL);
+		usleep(_cycling_rate * 2);
 
 		/* run the collection phase */
 		if (OK != collect()) {
@@ -474,17 +529,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 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);
+		debug("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -506,7 +563,7 @@ MB12XX::collect()
 	ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		debug("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
@@ -519,7 +576,39 @@ MB12XX::collect()
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
-	report.distance = si_units;
+
+	/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
+	if (addr_ind.size() == 1) {
+		report.distance = si_units;
+
+		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
+			report.distance_vector[i] = 0;
+		}
+
+		report.just_updated = 0;
+
+	} else {
+		/* for multiple sonars connected */
+
+		/* don't use the orginial single sonar variable */
+		report.distance = 0;
+
+		/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values 			of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest 			value for each connected sonar */
+		_latest_sonar_measurements[_cycle_counter] = si_units;
+
+		for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
+			report.distance_vector[i] = _latest_sonar_measurements[i];
+		}
+
+		/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been 		collected as this could be of use for Kalman filters */
+		report.just_updated = _index_counter;
+
+		/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
+		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
+			report.distance_vector[addr_ind.size() + i] = 0;
+		}
+	}
+
 	report.minimum_distance = get_minimum_distance();
 	report.maximum_distance = get_maximum_distance();
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
@@ -545,12 +634,13 @@ MB12XX::collect()
 void
 MB12XX::start()
 {
+
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
+	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
 
 	/* notify about state change */
 	struct subsystem_info_s info = {
@@ -564,8 +654,10 @@ MB12XX::start()
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
 
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
 	}
 }
 
@@ -578,21 +670,24 @@ MB12XX::stop()
 void
 MB12XX::cycle_trampoline(void *arg)
 {
+
 	MB12XX *dev = (MB12XX *)arg;
 
 	dev->cycle();
+
 }
 
 void
 MB12XX::cycle()
 {
-	/* collection phase? */
 	if (_collect_phase) {
+		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
+		set_address(_index_counter);
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
-			/* restart the measurement state machine */
+			debug("collection error");
+			/* if error restart the measurement state machine */
 			start();
 			return;
 		}
@@ -600,25 +695,37 @@ MB12XX::cycle()
 		/* next phase is measurement */
 		_collect_phase = false;
 
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+		/* change i2c adress to next sonar */
+		_cycle_counter = _cycle_counter + 1;
+
+		if (_cycle_counter >= addr_ind.size()) {
+			_cycle_counter = 0;
+		}
+
+		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
+		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
+
+		if (_measure_ticks > USEC2TICK(_cycling_rate)) {
 
 			/* 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));
-
+				   _measure_ticks - USEC2TICK(_cycling_rate));
 			return;
 		}
 	}
 
-	/* measurement phase */
+	/* Measurement (firing) phase */
+
+	/* ensure sonar i2c adress is still correct */
+	_index_counter = addr_ind[_cycle_counter];
+	set_address(_index_counter);
+
+	/* Perform measurement */
 	if (OK != measure()) {
-		log("measure error");
+		debug("measure error sonar adress %d", _index_counter);
 	}
 
 	/* next phase is collection */
@@ -629,7 +736,8 @@ MB12XX::cycle()
 		   &_work,
 		   (worker_t)&MB12XX::cycle_trampoline,
 		   this,
-		   USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+		   USEC2TICK(_cycling_rate));
+
 }
 
 void
@@ -750,7 +858,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated);
 	warnx("time:        %lld", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
@@ -779,7 +887,12 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f", (double)report.distance);
+
+		/* Print the sonar rangefinder report sonar distance vector */
+		for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) {
+			warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1);
+		}
+
 		warnx("time:        %lld", report.timestamp);
 	}
 
@@ -830,7 +943,7 @@ info()
 	exit(0);
 }
 
-} // namespace
+} /* namespace */
 
 int
 mb12xx_main(int argc, char *argv[])
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1055487cb..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>
@@ -456,8 +457,9 @@ MK::task_main()
 	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);
+	int dummy;
+	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+				   &outputs, &dummy, ORB_PRIO_HIGH);
 
 	/* advertise the blctrl status */
 	esc_status_s esc;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 168b34ea9..e322e8b3a 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -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
@@ -175,6 +175,12 @@
 
 #define MPU6000_ONE_G					9.80665f
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 /*
   the MPU6000 can only handle high SPI bus speeds on the sensor and
   interrupt status registers. All other registers have a maximum 1MHz
@@ -234,7 +240,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
-	orb_id_t		_accel_orb_id;
+	int			_accel_orb_class_instance;
 	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
@@ -361,6 +367,13 @@ private:
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
 	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
+	/**
 	 * Measurement self test
 	 *
 	 * @return 0 on success, 1 on failure
@@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
 									     MPUREG_INT_ENABLE,
 									     MPUREG_INT_PIN_CFG };
 
-					   
+
 
 /**
  * Helper class implementing the gyro driver node.
@@ -457,7 +470,7 @@ protected:
 private:
 	MPU6000			*_parent;
 	orb_advert_t		_gyro_topic;
-	orb_id_t		_gyro_orb_id;
+	int			_gyro_orb_class_instance;
 	int			_gyro_class_instance;
 
 	/* do not allow to copy this class due to pointer data members */
@@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
-	_accel_orb_id(nullptr),
+	_accel_orb_class_instance(-1),
 	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
 	_gyro_scale{},
@@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	// disable debug() calls
 	_debug_enabled = false;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
+
+	/* Prime _gyro with parents devid. */
+	_gyro->_device_id.devid = _device_id.devid;
+	_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
+
 	// default accel scale factors
 	_accel_scale.x_offset = 0;
 	_accel_scale.x_scale  = 1.0f;
@@ -596,6 +615,7 @@ MPU6000::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+
 	/* do CDev init for the gyro device node, keep it optional */
 	ret = _gyro->init();
 	/* if probe/setup failed, bail now */
@@ -613,22 +633,8 @@ MPU6000::init()
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
-	switch (_accel_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_accel_orb_id = ORB_ID(sensor_accel0);
-			break;
-		
-		case CLASS_DEVICE_SECONDARY:
-			_accel_orb_id = ORB_ID(sensor_accel1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_accel_orb_id = ORB_ID(sensor_accel2);
-			break;
-
-	}
-
-	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
+		&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
 
 	if (_accel_topic < 0) {
 		warnx("ADVERT FAIL");
@@ -639,22 +645,8 @@ MPU6000::init()
 	struct gyro_report grp;
 	_gyro_reports->get(&grp);
 
-	switch (_gyro->_gyro_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
-			break;
-
-	}
-
-	_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
+	_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
+		&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
 
 	if (_gyro->_gyro_topic < 0) {
 		warnx("ADVERT FAIL");
@@ -683,7 +675,7 @@ int MPU6000::reset()
 		// for it to come out of sleep
 		write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 		up_udelay(1000);
-		
+
 		// Disable I2C bus (recommended on datasheet)
 		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
 		irqrestore(state);
@@ -741,7 +733,7 @@ int MPU6000::reset()
 	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		
+	// presumably won't have the accel scaling bug
 	default:
 		// Accel scale 8g (4096 LSB/g)
 		write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
@@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 {
 	uint8_t filter;
 
-	/* 
+	/*
 	   choose next highest filter frequency available
 	 */
         if (frequency_hz == 0) {
@@ -921,26 +913,50 @@ MPU6000::gyro_self_test()
 	if (self_test())
 		return 1;
 
-	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
-	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+	/*
+	 * Maximum deviation of 20 degrees, according to
+	 * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
+	 * Section 6.1, initial ZRO tolerance
+	 */
+	const float max_offset = 0.34f;
+	/* 30% scale error is chosen to catch completely faulty units but
+	 * to let some slight scale error pass. Requires a rate table or correlation
+	 * with mag rotations + data fit to
+	 * calibrate properly and is not done by default.
+	 */
+	const float max_scale = 0.3f;
+
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */
+	if (fabsf(_gyro_scale.x_offset) > max_offset)
 		return 1;
-	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+
+	/* evaluate gyro scale, complain if off by more than 30% */
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale)
 		return 1;
 
-	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+	if (fabsf(_gyro_scale.y_offset) > max_offset)
 		return 1;
-	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale)
 		return 1;
 
-	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+	if (fabsf(_gyro_scale.z_offset) > max_offset)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale)
 		return 1;
-	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+
+	/* check if all scales are zero */
+	if ((fabsf(_gyro_scale.x_offset) < 0.000001f) &&
+		(fabsf(_gyro_scale.y_offset) < 0.000001f) &&
+		(fabsf(_gyro_scale.z_offset) < 0.000001f)) {
+		/* if all are zero, this device is not calibrated */
 		return 1;
+	}
 
 	return 0;
 }
 
 
+
 /*
   perform a self-test comparison to factory trim values. This takes
   about 200ms and will return OK if the current values are within 14%
@@ -958,7 +974,7 @@ MPU6000::factory_self_test()
 	// gyro self test has to be done at 250DPS
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
 
-	struct MPUReport mpu_report;	
+	struct MPUReport mpu_report;
 	float accel_baseline[3];
 	float gyro_baseline[3];
 	float accel[3];
@@ -988,10 +1004,10 @@ MPU6000::factory_self_test()
 	}
 
 #if 1
-	write_reg(MPUREG_GYRO_CONFIG, 
-		  BITS_FS_250DPS | 
-		  BITS_GYRO_ST_X | 
-		  BITS_GYRO_ST_Y | 
+	write_reg(MPUREG_GYRO_CONFIG,
+		  BITS_FS_250DPS |
+		  BITS_GYRO_ST_X |
+		  BITS_GYRO_ST_Y |
 		  BITS_GYRO_ST_Z);
 
 	// accel 8g, self-test enabled all axes
@@ -1061,7 +1077,7 @@ MPU6000::factory_self_test()
 			::printf("FAIL\n");
 			ret = -EIO;
 		}
-	}	
+	}
 	for (uint8_t i=0; i<3; i++) {
 		float diff = gyro[i]-gyro_baseline[i];
 		float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
@@ -1076,7 +1092,7 @@ MPU6000::factory_self_test()
 			::printf("FAIL\n");
 			ret = -EIO;
 		}
-	}	
+	}
 
 	write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
 	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
@@ -1223,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
-		
+
 		irqstate_t flags = irqsave();
 		if (!_accel_reports->resize(arg)) {
 			irqrestore(flags);
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
@@ -1512,13 +1528,13 @@ MPU6000::check_registers(void)
 	  the data registers.
 	*/
 	uint8_t v;
-	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != 
+	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
 	    _checked_values[_checked_next]) {
 		/*
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -1631,7 +1647,7 @@ MPU6000::measure()
 		_register_wait--;
 		return;
 	}
-	    
+
 
 	/*
 	 * Swap axes and negate y
@@ -1692,7 +1708,7 @@ MPU6000::measure()
 	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
 	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	
+
 	arb.x = _accel_filter_x.apply(x_in_new);
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
@@ -1713,7 +1729,7 @@ MPU6000::measure()
 	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	
+
 	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
@@ -1739,12 +1755,12 @@ MPU6000::measure()
 		perf_begin(_controller_latency_perf);
 		perf_begin(_system_latency_perf);
 		/* publish it */
-		orb_publish(_accel_orb_id, _accel_topic, &arb);
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1767,7 +1783,7 @@ MPU6000::print_info()
         for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
@@ -1794,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
-	_gyro_orb_id(nullptr),
+	_gyro_orb_class_instance(-1),
 	_gyro_class_instance(-1)
 {
 }
@@ -1839,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
 int
 MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->gyro_ioctl(filp, cmd, arg);
+
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->gyro_ioctl(filp, cmd, arg);
+	}
 }
 
 /**
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 0a793cbc9..75b1f65fd 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -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
@@ -57,6 +57,7 @@
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/device.h>
 #include <drivers/drv_baro.h>
@@ -134,8 +135,7 @@ protected:
 	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
-	orb_id_t		_orb_id;
-
+	int			_orb_class_instance;
 	int			_class_instance;
 
 	perf_counter_t		_sample_perf;
@@ -172,6 +172,13 @@ protected:
 	void			cycle();
 
 	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
+
+	/**
 	 * Static trampoline from the workq context; because we don't have a
 	 * generic workq wrapper yet.
 	 *
@@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
 	_SENS(0),
 	_msl_pressure(101325),
 	_baro_topic(-1),
+	_orb_class_instance(-1),
 	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
 	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
@@ -263,7 +271,6 @@ MS5611::init()
 
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(BARO_DEVICE_PATH);
-	_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
 
 	struct baro_report brp;
 	/* do a first measurement cycle to populate reports with valid data */
@@ -303,7 +310,9 @@ MS5611::init()
 
 		ret = OK;
 
-		_baro_topic = orb_advertise(_orb_id, &brp);
+		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
+				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+
 
 		if (_baro_topic < 0) {
 			warnx("failed to create sensor_baro publication");
@@ -725,7 +734,7 @@ MS5611::collect()
 		/* publish it */
 		if (!(_pub_blocked)) {
 			/* publish it */
-			orb_publish(_orb_id, _baro_topic, &report);
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 		}
 
 		if (_reports->force(&report)) {
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
index f0b3fd61d..3f1f6c473 100644
--- a/src/drivers/ms5611/ms5611.h
+++ b/src/drivers/ms5611/ms5611.h
@@ -37,12 +37,12 @@
  * Shared defines for the ms5611 driver.
  */
 
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
-#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+#define ADDR_RESET_CMD				0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start temperature conversion */
+#define ADDR_DATA							0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP				0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1					0xA2	/* address of 6x 2 bytes calibration data */
 
 /* interface ioctls */
 #define IOCTL_RESET			2
diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h
new file mode 100644
index 000000000..b391b1f6a
--- /dev/null
+++ b/src/drivers/px4flow/i2c_frame.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * 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 i2c_frame.h
+ * Definition of i2c frames.
+ * @author Thomas Boehm <thomas.boehm@fortiss.org>
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#ifndef I2C_FRAME_H_
+#define I2C_FRAME_H_
+#include <inttypes.h>
+
+
+typedef  struct i2c_frame
+{
+    uint16_t frame_count;
+    int16_t pixel_flow_x_sum;
+    int16_t pixel_flow_y_sum;
+    int16_t flow_comp_m_x;
+    int16_t flow_comp_m_y;
+    int16_t qual;
+    int16_t gyro_x_rate;
+    int16_t gyro_y_rate;
+    int16_t gyro_z_rate;
+    uint8_t gyro_range;
+    uint8_t sonar_timestamp;
+    int16_t ground_distance;
+} i2c_frame;
+
+#define I2C_FRAME_SIZE (sizeof(i2c_frame))
+
+
+typedef struct i2c_integral_frame
+{
+    uint16_t frame_count_since_last_readout;
+    int16_t pixel_flow_x_integral;
+    int16_t pixel_flow_y_integral;
+    int16_t gyro_x_rate_integral;
+    int16_t gyro_y_rate_integral;
+    int16_t gyro_z_rate_integral;
+    uint32_t integration_timespan;
+    uint32_t sonar_timestamp;
+    uint16_t ground_distance;
+    int16_t gyro_temperature;
+    uint8_t qual;
+} i2c_integral_frame;
+
+#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
+
+#endif /* I2C_FRAME_H_ */
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 9c9c1b0f8..bb0cdbbb6 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -93,38 +93,11 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-struct i2c_frame {
-	uint16_t frame_count;
-	int16_t pixel_flow_x_sum;
-	int16_t pixel_flow_y_sum;
-	int16_t flow_comp_m_x;
-	int16_t flow_comp_m_y;
-	int16_t qual;
-	int16_t gyro_x_rate;
-	int16_t gyro_y_rate;
-	int16_t gyro_z_rate;
-	uint8_t gyro_range;
-	uint8_t sonar_timestamp;
-	int16_t ground_distance;
-};
-struct i2c_frame f;
+#include "i2c_frame.h"
 
-struct i2c_integral_frame {
-	uint16_t frame_count_since_last_readout;
-	int16_t pixel_flow_x_integral;
-	int16_t pixel_flow_y_integral;
-	int16_t gyro_x_rate_integral;
-	int16_t gyro_y_rate_integral;
-	int16_t gyro_z_rate_integral;
-	uint32_t integration_timespan;
-	uint32_t time_since_last_sonar_update;
-	uint16_t ground_distance;
-	int16_t gyro_temperature;
-	uint8_t qual;
-} __attribute__((packed));
+struct i2c_frame f;
 struct i2c_integral_frame f_integral;
 
-
 class PX4FLOW: public device::I2C
 {
 public:
@@ -150,8 +123,7 @@ private:
 	RingBuffer			*_reports;
 	bool				_sensor_ok;
 	int					_measure_ticks;
-	bool				_collect_phase;
-
+	bool				_collect_phase; 
 	orb_advert_t		_px4flow_topic;
 
 	perf_counter_t		_sample_perf;
@@ -261,10 +233,10 @@ out:
 int
 PX4FLOW::probe()
 {
-	uint8_t val[22];
+	uint8_t val[I2C_FRAME_SIZE];
 
 	// to be sure this is not a ll40ls Lidar (which can also be on
-	// 0x42) we check if a 22 byte transfer works from address
+	// 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
 	// 0. The ll40ls gives an error for that, whereas the flow
 	// happily returns some data
 	if (transfer(nullptr, 0, &val[0], 22) != OK) {
@@ -469,16 +441,16 @@ PX4FLOW::collect()
 	int ret = -EIO;
 
 	/* read from the sensor */
-	uint8_t val[47] = { 0 };
+	uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
 
 	perf_begin(_sample_perf);
 
 	if (PX4FLOW_REG == 0x00) {
-		ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
+		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 	if (PX4FLOW_REG == 0x16) {
-		ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
+		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 	if (ret < 0) {
@@ -489,46 +461,12 @@ PX4FLOW::collect()
 	}
 
 	if (PX4FLOW_REG == 0) {
-		f.frame_count = val[1] << 8 | val[0];
-		f.pixel_flow_x_sum = val[3] << 8 | val[2];
-		f.pixel_flow_y_sum = val[5] << 8 | val[4];
-		f.flow_comp_m_x = val[7] << 8 | val[6];
-		f.flow_comp_m_y = val[9] << 8 | val[8];
-		f.qual = val[11] << 8 | val[10];
-		f.gyro_x_rate = val[13] << 8 | val[12];
-		f.gyro_y_rate = val[15] << 8 | val[14];
-		f.gyro_z_rate = val[17] << 8 | val[16];
-		f.gyro_range = val[18];
-		f.sonar_timestamp = val[19];
-		f.ground_distance = val[21] << 8 | val[20];
-
-		f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
-		f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
-		f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
-		f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
-		f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
-		f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
-		f_integral.integration_timespan = val[37] << 24 | val[36] << 16
-						  | val[35] << 8 | val[34];
-		f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
-				| val[39] << 8 | val[38];
-		f_integral.ground_distance = val[43] << 8 | val[42];
-		f_integral.gyro_temperature = val[45] << 8 | val[44];
-		f_integral.qual = val[46];
+		memcpy(&f, val, I2C_FRAME_SIZE);
+		memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 	if (PX4FLOW_REG == 0x16) {
-		f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
-		f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
-		f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
-		f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
-		f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
-		f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
-		f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
-		f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
-		f_integral.ground_distance = val[21] << 8 | val[20];
-		f_integral.gyro_temperature = val[23] << 8 | val[22];
-		f_integral.qual = val[24];
+		memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 
@@ -544,7 +482,7 @@ PX4FLOW::collect()
 	report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
 	report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
 	report.integration_timespan = f_integral.integration_timespan; //microseconds
-	report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
+	report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
 	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
 
 	report.sensor_id = 0;
@@ -828,7 +766,7 @@ test()
 		warnx("ground_distance: %0.2f m",
 		      (double) f_integral.ground_distance / 1000);
 		warnx("time since last sonar update [us]: %i",
-		      f_integral.time_since_last_sonar_update);
+		      f_integral.sonar_timestamp);
 		warnx("quality integration average : %i", f_integral.qual);
 		warnx("quality : %i", f.qual);
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 436672040..8fcdc8023 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -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
@@ -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];
-	orb_id_t	_actuator_output_topic;
-	pollfd	_poll_fds[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[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
@@ -256,7 +260,7 @@ PX4FMU::PX4FMU() :
 	_groups_required(0),
 	_groups_subscribed(0),
 	_control_subs{-1},
-	_actuator_output_topic(nullptr),
+	_actuator_output_topic_instance(-1),
 	_poll_fds_num(0),
 	_pwm_limit{},
 	_failsafe_pwm{0},
@@ -327,8 +331,6 @@ PX4FMU::init()
 		log("default PWM output device");
 	}
 
-	_actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance);
-
 	/* reset GPIOs */
 	gpio_reset();
 
@@ -510,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]);
@@ -587,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);
 				}
@@ -614,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]);
@@ -679,10 +681,10 @@ PX4FMU::task_main()
 
 				/* publish mixed control outputs */
 				if (_outputs_pub < 0) {
-					_outputs_pub = orb_advertise(_actuator_output_topic, &outputs);
+					_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
 				} else {
 
-					orb_publish(_actuator_output_topic, _outputs_pub, &outputs);
+					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
 				}
 			}
 		}
@@ -747,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;
@@ -1144,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:
@@ -1637,12 +1639,15 @@ sensor_reset(int ms)
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
-	if (fd < 0)
+	if (fd < 0) {
 		errx(1, "open fail");
+	}
 
-	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
-		err(1, "servo arm failed");
+	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) {
+		warnx("sensor rail reset failed");
+	}
 
+	close(fd);
 }
 
 void
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 96ebedd83..ae7f85b87 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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
@@ -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>
@@ -834,7 +838,7 @@ PX4IO::init()
 	_task = task_spawn_cmd("px4io",
 					SCHED_DEFAULT,
 					SCHED_PRIORITY_ACTUATOR_OUTPUTS,
-					2000,
+					1800,
 					(main_t)&PX4IO::task_main_trampoline,
 					nullptr);
 
@@ -1102,7 +1106,7 @@ PX4IO::io_set_control_state(unsigned group)
 	uint16_t 		regs[_max_actuators];
 
 	/* get controls */
-	bool changed;
+	bool changed = false;
 
 	switch (group) {
 		case 0:
@@ -1144,11 +1148,13 @@ PX4IO::io_set_control_state(unsigned group)
 		break;
 	}
 
-	if (!changed)
+	if (!changed) {
 		return -1;
+	}
 
-	for (unsigned i = 0; i < _max_controls; i++)
+	for (unsigned i = 0; i < _max_controls; i++) {
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
+	}
 
 	/* copy values to registers in IO */
 	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
@@ -1197,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;
 		}
@@ -1679,17 +1685,12 @@ PX4IO::io_publish_pwm_outputs()
 
 	/* lazily advertise on first publication */
 	if (_to_outputs == 0) {
-		_to_outputs = orb_advertise((_primary_pwm_device ?
-					     ORB_ID_VEHICLE_CONTROLS :
-					     ORB_ID(actuator_outputs_1)),
-					    &outputs);
+		int instance;
+		_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+					    &outputs, &instance, ORB_PRIO_MAX);
 
 	} else {
-		orb_publish((_primary_pwm_device ?
-			     ORB_ID_VEHICLE_CONTROLS :
-			     ORB_ID(actuator_outputs_1)),
-			    _to_outputs,
-			    &outputs);
+		orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
 	}
 
 	return OK;
@@ -1773,12 +1774,12 @@ PX4IO::print_debug()
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 	int io_fd = -1;
 
-	if (io_fd < 0) {
+	if (io_fd <= 0) {
 		io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
 	}
 
 	/* read IO's output */
-	if (io_fd > 0) {
+	if (io_fd >= 0) {
 		pollfd fds[1];
 		fds[0].fd = io_fd;
 		fds[0].events = POLLIN;
@@ -2278,6 +2279,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
 		*(unsigned *)arg = _lockdown_override;
+		break;
 
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 		/* force safety swith off */
@@ -2587,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) {
@@ -2853,10 +2855,10 @@ checkcrc(int argc, char *argv[])
 	}
 
 	if (ret != OK) {
-		printf("check CRC failed - %d\n", ret);
+		printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret);
 		exit(1);
 	}
-	printf("CRCs match\n");
+	printf("[PX4IO::checkcrc] CRCs match\n");
 	exit(0);
 }
 
@@ -3003,11 +3005,14 @@ monitor(void)
 
 		fds[0].fd = 0;
 		fds[0].events = POLLIN;
-		poll(fds, 1, 2000);
+		if (poll(fds, 1, 2000) < 0) {
+			errx(1, "poll fail");
+		}
 
 		if (fds[0].revents == POLLIN) {
-			int c;
-			read(0, &c, 1);
+			/* control logic is to cancel with any key */
+			char c;
+			(void)read(0, &c, 1);
 
 			if (cancels-- == 0) {
 				printf("\033[2J\033[H"); /* move cursor home and clear screen */
@@ -3069,12 +3074,13 @@ lockdown(int argc, char *argv[])
 
 					if (ret > 0) {
 
-						read(0, &c, 1);
+						if (read(0, &c, 1) > 0) {
 
-						if (c != 'y') {
-							exit(0);
-						} else if (c == 'y') {
-							break;
+							if (c != 'y') {
+								exit(0);
+							} else if (c == 'y') {
+								break;
+							}
 						}
 					}
 
@@ -3237,7 +3243,13 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "limit")) {
 
 		if ((argc > 2)) {
-			g_dev->set_update_rate(atoi(argv[2]));
+			int limitrate = atoi(argv[2]);
+
+			if (limitrate > 0) {
+				g_dev->set_update_rate(limitrate);
+			} else {
+				errx(1, "invalid rate: %d", limitrate);
+			}
 
 		} else {
 			errx(1, "missing argument (50 - 500 Hz)");
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index fb16f891f..02e527695 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -621,6 +621,7 @@ int
 PX4IO_Uploader::reboot()
 {
 	send(PROTO_REBOOT);
+	up_udelay(100*1000); // Ensure the farend is in wait for char.
 	send(PROTO_EOC);
 
 	return OK;
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index eb9453b50..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>
 
@@ -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");