Merge branch 'px4dev_new_param' of github.com:PX4/Firmware into px4dev_new_param

33 files changed, 912 insertions, 318 deletions

diff --git a/Debug/NuttX b/Debug/NuttX
index b6d4e74f0..5396ab6b1 100644
--- a/Debug/NuttX
+++ b/Debug/NuttX
@@ -74,6 +74,29 @@ document showheap
 end
 
 ################################################################################
+# Task file listing 
+################################################################################
+
+define showfiles
+	set $task = (struct _TCB *)$arg0
+	set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file)
+	printf "%d files\n", $nfiles
+	set $index = 0
+	while $index < $nfiles
+		set $file = &($task->filelist->fl_files[$index])
+		printf "%d: inode %p f_priv %p\n", $index, $file->f_inode, $file->f_priv
+		if $file->f_inode != 0
+			printf "    i_name %s  i_private %p\n", &$file->f_inode->i_name[0], $file->f_inode->i_private
+		end
+		set $index = $index + 1
+	end
+end
+
+document showfiles
+.    showfiles <TCB pointer>
+.        Prints the files opened by a task.
+
+################################################################################
 # Task display
 ################################################################################
 
diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors
index b6b4c65cb..f913e82af 100644
--- a/ROMFS/scripts/rc.sensors
+++ b/ROMFS/scripts/rc.sensors
@@ -7,8 +7,9 @@
 # Start sensor drivers here.
 #
 
-#ms5611 start
-#mpu6000 start
+ms5611 start
+mpu6000 start
+hmc5883 start
 
 #
 # Start the sensor collection task.
diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS
index 7b8d51338..9e564e2cc 100755
--- a/ROMFS/scripts/rcS
+++ b/ROMFS/scripts/rcS
@@ -31,6 +31,8 @@ else
 	echo "[init] no microSD card found"
 fi
 
+usleep 500
+
 #
 # Look for an init script on the microSD card.
 #
@@ -82,6 +84,7 @@ else
 		if [ -f /etc/init.d/rc.PX4IOAR ]
 		then
 			echo "[init] reading /etc/init.d/rc.PX4IOAR"
+			usleep 500
 			sh /etc/init.d/rc.PX4IOAR
 		fi
 	else
@@ -97,6 +100,7 @@ else
 		if [ -f /etc/init.d/rc.PX4IO ]
 		then
 			echo "[init] reading /etc/init.d/rc.PX4IO"
+			usleep 500
 			sh /etc/init.d/rc.PX4IO
 		fi
 	else
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index fdf6c9d91..09cbdd4e9 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -138,7 +138,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 	/* advertise attitude */
-	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 
 	int loopcounter = 0;
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 424bef140..75946fb24 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -96,8 +96,8 @@ static int leds;
 static int buzzer;
 static int mavlink_fd;
 static bool commander_initialized = false;
-static struct vehicle_status_s current_status;	/**< Main state machine */
-static int stat_pub;
+static struct vehicle_status_s current_status; /**< Main state machine */
+static orb_advert_t stat_pub;
 
 static uint16_t nofix_counter = 0;
 static uint16_t gotfix_counter = 0;
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index fe1addaeb..a59291778 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -48,6 +48,8 @@
 /**
  * mag report structure.  Reads from the device must be in multiples of this
  * structure.
+ *
+ * Output values are in gauss.
  */
 struct mag_report {
 	float x;
@@ -99,4 +101,10 @@ ORB_DECLARE(sensor_mag);
 /** set the mag scaling constants to the structure pointed to by (arg) */
 #define MAGIOCSSCALE		_MAGIOC(5)
 
+/** copy the mag scaling constants to the structure pointed to by (arg) */
+#define MAGIOCGSCALE		_MAGIOC(6)
+
+/** perform self-calibration, update scale factors to canonical units */
+#define MAGIOCCALIBRATE		_MAGIOC(7)
+
 #endif /* _DRV_MAG_H */
diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h
index b3fc01a5f..50288f690 100644
--- a/apps/drivers/drv_orb_dev.h
+++ b/apps/drivers/drv_orb_dev.h
@@ -81,4 +81,7 @@
 /** Set the minimum interval at which the topic can be seen to be updated for this subscription */
 #define ORBIOCSETINTERVAL	_ORBIOC(12)
 
+/** Get the global advertiser handle for the topic */
+#define ORBIOCGADVERTISER	_ORBIOC(13)
+
 #endif /* _DRV_UORB_H */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 64a0a932e..0fa1f365d 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -43,6 +43,7 @@
 
 #include <sys/types.h>
 #include <stdint.h>
+#include <stdlib.h>
 #include <stdbool.h>
 #include <semaphore.h>
 #include <string.h>
@@ -63,7 +64,6 @@
 
 #include <drivers/drv_mag.h>
 
-
 /*
  * HMC5883 internal constants and data structures.
  */
@@ -137,17 +137,17 @@ protected:
 	virtual int		probe();
 
 private:
-	struct work_s		_work;
+	work_s			_work;
 	unsigned		_measure_ticks;
 
 	unsigned		_num_reports;
 	volatile unsigned	_next_report;
 	volatile unsigned	_oldest_report;
-	struct mag_report	*_reports;
-
+	mag_report		*_reports;
+	mag_scale		_scale;
 	bool			_collect_phase;
 
-	int			_mag_topic;
+	orb_advert_t		_mag_topic;
 
 	unsigned		_reads;
 	unsigned		_measure_errors;
@@ -257,6 +257,7 @@ HMC5883::HMC5883(int bus) :
 	_next_report(0),
 	_oldest_report(0),
 	_reports(nullptr),
+	_mag_topic(-1),
 	_reads(0),
 	_measure_errors(0),
 	_read_errors(0),
@@ -266,6 +267,14 @@ HMC5883::HMC5883(int bus) :
 	// enable debug() calls
 	_debug_enabled = true;
 
+	// default scaling
+	_scale.x_offset = 0;
+	_scale.x_scale = 1.0f / 1090.0f;	/* default range scale from counts to gauss */
+	_scale.y_offset = 0;
+	_scale.y_scale = 1.0f / 1090.0f;	/* default range scale from counts to gauss */
+	_scale.z_offset = 0;
+	_scale.z_scale = 1.0f / 1090.0f;	/* default range scale from counts to gauss */
+
 	// work_cancel in the dtor will explode if we don't do this...
 	_work.worker = nullptr;
 }
@@ -287,21 +296,10 @@ HMC5883::init()
 
 	/* do I2C init (and probe) first */
 	ret = I2C::init();
+
 	if (ret != OK)
 		goto out;
 
-	/* assuming we're good, advertise the object */
-	struct mag_report m;
-
-	/* if this fails (e.g. no object in the system) that's OK */
-	memset(&m, 0, sizeof(m));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &m);
-
-	if (_mag_topic < 0) {
-		debug("failed to create sensor_baro object");
-	} else {
-		ret = 0;
-	}
 out:
 	return ret;
 }
@@ -340,7 +338,6 @@ HMC5883::close_last(struct file *filp)
 int
 HMC5883::probe()
 {
-	uint8_t	cmd[] = { ADDR_STATUS };
 	uint8_t data[3];
 
 	if (read_reg(ADDR_ID_A, data[0]) ||
@@ -348,7 +345,7 @@ HMC5883::probe()
 	    read_reg(ADDR_ID_C, data[2]))
 		debug("read_reg fail");
 
-	if ((data[0] != ID_A_WHO_AM_I) || 
+	if ((data[0] != ID_A_WHO_AM_I) ||
 	    (data[1] != ID_B_WHO_AM_I) ||
 	    (data[2] != ID_C_WHO_AM_I)) {
 		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
@@ -434,7 +431,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 				_measure_ticks = 0;
 				return OK;
 
-				/* external signalling not supported */
+				/* external signalling (DRDY) not supported */
 			case MAG_POLLRATE_EXTERNAL:
 
 				/* zero would be bad */
@@ -486,7 +483,30 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
+	case MAGIOCSSCALE:
+		/* set new scale factors */
+		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
+		return 0;
+
+	case MAGIOCGSCALE:
+		/* copy out scale factors */
+		memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
+		return 0;
+
+	case MAGIOCCALIBRATE:
+		/* XXX perform auto-calibration */
+		return -EINVAL;
+
+	case MAGIOCSSAMPLERATE:
+		/* not supported, always 1 sample per poll */
+		return -EINVAL;
+
+	case MAGIOCSLOWPASS:
+		/* not supported, no internal filtering */
+		return -EINVAL;
+
 	case MAGIOCSREPORTFORMAT:
+		/* not supported, no custom report format */
 		return -EINVAL;
 
 	default:
@@ -526,6 +546,25 @@ HMC5883::cycle_trampoline(void *arg)
 void
 HMC5883::cycle()
 {
+	/*
+	 * We have to publish the mag topic in the context of the workq
+	 * in order to ensure that the descriptor is valid when we go to publish.
+	 *
+	 * @bug	We can't really ever be torn down and restarted, since this
+	 *      descriptor will never be closed and on the restart we will be
+	 *      unable to re-advertise.
+	 */
+	if (_mag_topic == -1) {
+		struct mag_report m;
+
+		/* if this fails (e.g. no object in the system) we will cope */
+		memset(&m, 0, sizeof(m));
+		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &m);
+
+		if (_mag_topic < 0)
+			debug("failed to create sensor_mag object");
+	}
+
 	/* collection phase? */
 	if (_collect_phase) {
 
@@ -579,6 +618,7 @@ HMC5883::measure()
 	 * Send the command to begin a measurement.
 	 */
 	ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
+
 	if (OK != ret)
 		_measure_errors++;
 
@@ -591,33 +631,56 @@ HMC5883::collect()
 #pragma pack(push, 1)
 	struct { /* status register and data as read back from the device */
 		uint8_t		x[2];
-		uint8_t		z[2];
 		uint8_t		y[2];
-		uint8_t		status;
+		uint8_t		z[2];
 	}	hmc_report;
 #pragma pack(pop)
+	struct {
+		int16_t		x, y, z;
+	} report;
 	int	ret = -EIO;
-	uint8_t	cmd[1];
+	uint8_t	cmd;
+
 
 	perf_begin(_sample_perf);
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	_reports[_next_report].timestamp = hrt_absolute_time();
 
+	/*
+	 * @note  We could read the status register here, which could tell us that
+	 *        we were too early and that the output registers are still being
+	 *        written.  In the common case that would just slow us down, and
+	 *        we're better off just never being early.
+	 */
+
 	/* get measurements from the device */
-	cmd[0] = ADDR_DATA_OUT_X_MSB;
-	ret = transfer(&cmd[0], 1, &hmc_report.x[0], sizeof(hmc_report));
+	cmd = ADDR_DATA_OUT_X_MSB;
+	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
+
 	if (ret != OK) {
 		debug("data/status read error");
 		goto out;
 	}
 
-	/* XXX check status? */
+	/* swap the data we just received */
+	report.x = ((int16_t)hmc_report.x[0] << 8) + hmc_report.x[1];
+	report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1];
+	report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1];
+
+	/*
+	 * If any of the values are -4096, there was an internal math error in the sensor.
+	 * Generalise this to a simple range check that will also catch some bit errors.
+	 */
+	if ((abs(report.x) > 2048) ||
+	    (abs(report.y) > 2048) ||
+	    (abs(report.z) > 2048))
+		goto out;
 
-	/* XXX scaling */
-	_reports[_next_report].x = meas_to_float(hmc_report.x);
-	_reports[_next_report].y = meas_to_float(hmc_report.y);
-	_reports[_next_report].z = meas_to_float(hmc_report.z);
+	/* scale values for output */
+	_reports[_next_report].x = report.x * _scale.x_scale + _scale.x_offset;
+	_reports[_next_report].y = report.y * _scale.y_scale + _scale.y_offset;
+	_reports[_next_report].z = report.z * _scale.z_scale + _scale.z_offset;
 
 	/* publish it */
 	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 7576fc1d9..999790e8a 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -181,12 +181,12 @@ private:
 	struct accel_report	_accel_report;
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
-	int			_accel_topic;
+	orb_advert_t		_accel_topic;
 
 	struct gyro_report	_gyro_report;
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
-	int			_gyro_topic;
+	orb_advert_t		_gyro_topic;
 
 	unsigned		_reads;
 	perf_counter_t		_sample_perf;
@@ -288,9 +288,9 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
-	_accel_range_scale(1.0f),
+	_accel_range_scale(0.02f),
 	_accel_topic(-1),
-	_gyro_range_scale(1.0f),
+	_gyro_range_scale(0.02f),
 	_gyro_topic(-1),
 	_reads(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
@@ -826,7 +826,7 @@ test()
 		fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
 		if (fd < 0) {
-			reason = "can't open driver";
+			reason = "can't open driver, use <mpu6000 start> first";
 			break;
 		}
 
@@ -841,9 +841,10 @@ test()
 		printf("single read\n");
 		fflush(stdout);
 		printf("time:        %lld\n", report.timestamp);
-		printf("x:           %f\n", report.x);
-		printf("y:           %f\n", report.y);
-		printf("z:           %f\n", report.z);
+		printf("x:         %5.2f\n", (double)report.x);
+		printf("y:         %5.2f\n", (double)report.y);
+		printf("z:         %5.2f\n", (double)report.z);
+		printf("test:         %8.4f\n", 1.5);
 
 	} while (0);
 
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
index 6c6951d9b..8f8f96217 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/apps/drivers/ms5611/ms5611.cpp
@@ -125,7 +125,7 @@ private:
 	int32_t			_dT;
 	int64_t			_temp64;
 
-	int			_baro_topic;
+	orb_advert_t		_baro_topic;
 
 	unsigned		_reads;
 	unsigned		_measure_errors;
@@ -246,6 +246,7 @@ MS5611::MS5611(int bus) :
 	_measure_phase(0),
 	_dT(0),
 	_temp64(0),
+	_baro_topic(-1),
 	_reads(0),
 	_measure_errors(0),
 	_read_errors(0),
@@ -277,18 +278,6 @@ MS5611::init()
 	/* do I2C init (and probe) first */
 	ret = I2C::init();
 
-	/* assuming we're good, advertise the object */
-	if (ret == OK) {
-		struct baro_report b;
-
-		/* if this fails (e.g. no object in the system) that's OK */
-		memset(&b, 0, sizeof(b));
-		_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
-
-		if (_baro_topic < 0)
-			debug("failed to create sensor_baro object");
-	}
-
 	return ret;
 }
 
@@ -538,6 +527,25 @@ MS5611::cycle_trampoline(void *arg)
 void
 MS5611::cycle()
 {
+	/*
+	 * We have to publish the baro topic in the context of the workq
+	 * in order to ensure that the descriptor is valid when we go to publish.
+	 *
+	 * @bug	We can't really ever be torn down and restarted, since this
+	 *      descriptor will never be closed and on the restart we will be
+	 *      unable to re-advertise.
+	 */
+	if (_baro_topic == -1) {
+		struct baro_report b;
+
+		/* if this fails (e.g. no object in the system) we will cope */
+		memset(&b, 0, sizeof(b));
+		_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
+
+		if (_baro_topic < 0)
+			debug("failed to create sensor_baro object");
+	}
+
 	/* collection phase? */
 	if (_collect_phase) {
 
diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c
index fd34a5dac..7f655964d 100644
--- a/apps/examples/px4_simple_app/px4_simple_app.c
+++ b/apps/examples/px4_simple_app/px4_simple_app.c
@@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[])
 	/* advertise attitude topic */
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
-	int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	/* one could wait for multiple topics with this technique, just using one here */
 	struct pollfd fds[] = {
@@ -103,7 +103,7 @@ int px4_simple_app_main(int argc, char *argv[])
 				att.roll = raw.accelerometer_m_s2[0];
 				att.pitch = raw.accelerometer_m_s2[1];
 				att.yaw = raw.accelerometer_m_s2[2];
-				orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
+				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 			/* there could be more file descriptors here, in the form like:
 			 * if (fds[1..n].revents & POLLIN) {}
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index 4e3a5eafe..42534a0a7 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -683,7 +683,7 @@ int fixedwing_control_main(int argc, char *argv[])
 
 	/* Set up to publish fixed wing control messages */
 	struct fixedwing_control_s control;
-	int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
+	orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
 
 	/* Subscribe to global position, attitude and rc */
 	struct vehicle_global_position_s global_pos;
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index cef70601b..0333c7100 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -311,7 +311,7 @@ void *mtk_loop(void *arg)
 	/* advertise GPS topic */
 	struct vehicle_gps_position_s mtk_gps_d;
 	mtk_gps = &mtk_gps_d;
-	int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
+	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */
diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c
index 4b520d403..54912b6d3 100644
--- a/apps/gps/nmea_helper.c
+++ b/apps/gps/nmea_helper.c
@@ -176,7 +176,7 @@ void *nmea_loop(void *arg)
 	/* advertise GPS topic */
 	struct vehicle_gps_position_s nmea_gps_d = {0};
 	nmea_gps = &nmea_gps_d;
-	int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
+	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 771dfbd6c..a629e904d 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -842,7 +842,7 @@ void *ubx_loop(void *arg)
 
 	ubx_gps = &ubx_gps_d;
 
-	int gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
+	orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 1bcbb2aa4..1b23f6751 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -73,6 +73,8 @@
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include "waypoints.h"
 #include "mavlink_log.h"
 
@@ -113,7 +115,7 @@ static struct vehicle_status_s v_status;
 static struct rc_channels_s rc;
 
 /* HIL publishers */
-static int pub_hil_attitude = -1;
+static orb_advert_t pub_hil_attitude = -1;
 
 /** HIL attitude */
 static struct vehicle_attitude_s hil_attitude;
@@ -126,16 +128,13 @@ static struct ardrone_motors_setpoint_s ardrone_motors;
 
 static struct vehicle_command_s vcmd;
 
-static int pub_hil_global_pos = -1;
-static int ardrone_motors_pub = -1;
-static int cmd_pub = -1;
-static int sensor_sub = -1;
-static int att_sub = -1;
-static int global_pos_sub = -1;
+static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t ardrone_motors_pub = -1;
+static orb_advert_t cmd_pub = -1;
 static int local_pos_sub = -1;
-static int flow_pub = -1;
-static int global_position_setpoint_pub = -1;
-static int local_position_setpoint_pub = -1;
+static orb_advert_t flow_pub = -1;
+static orb_advert_t global_position_setpoint_pub = -1;
+static orb_advert_t local_position_setpoint_pub = -1;
 static bool mavlink_hil_enabled = false;
 
 static char mavlink_message_string[51] = {0};
@@ -146,6 +145,30 @@ static enum {
 	MAVLINK_INTERFACE_MODE_ONBOARD
 } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
 
+static struct mavlink_subscriptions {
+	int sensor_sub;
+	int att_sub;
+	int global_pos_sub;
+	int act_0_sub;
+	int act_1_sub;
+	int act_2_sub;
+	int act_3_sub;
+	int gps_sub;
+	int man_control_sp_sub;
+	bool initialized;
+} mavlink_subs = {
+	.sensor_sub = 0,
+	.att_sub = 0,
+	.global_pos_sub = 0,
+	.act_0_sub = 0,
+	.act_1_sub = 0,
+	.act_2_sub = 0,
+	.act_3_sub = 0,
+	.gps_sub = 0,
+	.man_control_sp_sub = 0,
+	.initialized = false
+};
+
 
 /* 3: Define waypoint helper functions */
 void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -459,23 +482,33 @@ static void *receiveloop(void *arg)
 	return NULL;
 }
 
-static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
+static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
 {
 	int ret = OK;
 
 	switch (mavlink_msg_id) {
 		case MAVLINK_MSG_ID_SCALED_IMU:
 			/* senser sub triggers scaled IMU */
-			orb_set_interval(sensor_sub, min_interval);
+			if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
 			break;
 		case MAVLINK_MSG_ID_RAW_IMU:
 			/* senser sub triggers RAW IMU */
-			orb_set_interval(sensor_sub, min_interval);
+			if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
 			break;
 		case MAVLINK_MSG_ID_ATTITUDE:
 			/* attitude sub triggers attitude */
-			orb_set_interval(att_sub, min_interval);
+			if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval);
+			break;
+		case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
+			/* actuator_outputs triggers this message */
+			if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval);
+			if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
+			if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
+			if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
 			break;
+		case MAVLINK_MSG_ID_MANUAL_CONTROL:
+			/* manual_control_setpoint triggers this message */
+			if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval);
 		default:
 			/* not found */
 			ret = ERROR;
@@ -494,13 +527,16 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
  */
 static void *uorb_receiveloop(void *arg)
 {
+	/* obtain reference to task's subscriptions */
+	struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg;
+
 	/* Set thread name */
 	prctl(PR_SET_NAME, "mavlink uORB", getpid());
 
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
 	/* number of messages */
-	const ssize_t fdsc = 15;
+	const ssize_t fdsc = 25;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -515,28 +551,30 @@ static void *uorb_receiveloop(void *arg)
 		struct vehicle_local_position_setpoint_s local_sp;
 		struct vehicle_global_position_setpoint_s global_sp;
 		struct vehicle_attitude_setpoint_s att_sp;
+		struct actuator_outputs_s act_outputs;
+		struct manual_control_setpoint_s man_control;
 	} buf;
 
 	/* --- SENSORS RAW VALUE --- */
 	/* subscribe to ORB for sensors raw */
-	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	fds[fdsc_count].fd = sensor_sub;
+	subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	fds[fdsc_count].fd = subs->sensor_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
 	/* --- ATTITUDE VALUE --- */
 	/* subscribe to ORB for attitude */
-	att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	orb_set_interval(att_sub, 100);
-	fds[fdsc_count].fd = att_sub;
+	subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	orb_set_interval(subs->att_sub, 100);
+	fds[fdsc_count].fd = subs->att_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
 	/* --- GPS VALUE --- */
 	/* subscribe to ORB for attitude */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(gps_sub, 1000);	/* 1Hz updates */
-	fds[fdsc_count].fd = gps_sub;
+	subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	orb_set_interval(subs->gps_sub, 1000);	/* 1Hz updates */
+	fds[fdsc_count].fd = subs->gps_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -577,7 +615,7 @@ static void *uorb_receiveloop(void *arg)
 
 	/* --- GLOBAL POS VALUE --- */
 	/* struct already globally allocated and topic already subscribed */
-	fds[fdsc_count].fd = global_pos_sub;
+	fds[fdsc_count].fd = subs->global_pos_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -608,12 +646,39 @@ static void *uorb_receiveloop(void *arg)
 	/* --- ATTITUDE SETPOINT VALUE --- */
 	/* subscribe to ORB for attitude setpoint */
 	/* struct already allocated */
-	int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	orb_set_interval(spa_sub, 2000);	/* 0.5 Hz updates */
 	fds[fdsc_count].fd = spa_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/** --- ACTUATOR OUTPUTS --- */
+	subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
+	fds[fdsc_count].fd = subs->act_0_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+	subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
+	fds[fdsc_count].fd = subs->act_1_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+	subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
+	fds[fdsc_count].fd = subs->act_2_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+	subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
+	fds[fdsc_count].fd = subs->act_3_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/** --- MAPPED MANUAL CONTROL INPUTS --- */
+	subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	fds[fdsc_count].fd = subs->man_control_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/* all subscriptions initialized, return success */
+	subs->initialized = true;
+
 	unsigned int sensors_raw_counter = 0;
 	unsigned int attitude_counter = 0;
 	unsigned int gps_counter = 0;
@@ -651,7 +716,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 
 				/* copy sensors raw data into local buffer */
-				orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
+				orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
 
 				/* send raw imu data */
 				mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
@@ -667,7 +732,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 
 				/* copy attitude data into local buffer */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
+				orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att);
 
 				/* send sensor values */
 				mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
@@ -678,7 +743,7 @@ static void *uorb_receiveloop(void *arg)
 			/* --- GPS VALUE --- */
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy gps data into local buffer */
-				orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
+				orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps);
 				/* GPS position */
 				mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible);
 
@@ -801,7 +866,7 @@ static void *uorb_receiveloop(void *arg)
 			/* --- VEHICLE GLOBAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy global position data into local buffer */
-				orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+				orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos);
 				uint64_t timestamp = global_pos.timestamp;
 				int32_t lat = global_pos.lat;
 				int32_t lon = global_pos.lon;
@@ -845,6 +910,126 @@ static void *uorb_receiveloop(void *arg)
 				orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
 				mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
 			}
+
+			/* --- ACTUATOR OUTPUTS 0 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					0 /* port 0 */,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				// if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+				// 	mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+				// 	1 /* port 1 */,
+				// 	buf.act_outputs.output[ 8],
+				// 	buf.act_outputs.output[ 9],
+				// 	buf.act_outputs.output[10],
+				// 	buf.act_outputs.output[11],
+				// 	buf.act_outputs.output[12],
+				// 	buf.act_outputs.output[13],
+				// 	buf.act_outputs.output[14],
+				// 	buf.act_outputs.output[15]);
+				// }
+			}
+
+			/* --- ACTUATOR OUTPUTS 1 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					3 /* port 3 */,
+					buf.act_outputs.output[ 8],
+					buf.act_outputs.output[ 9],
+					buf.act_outputs.output[10],
+					buf.act_outputs.output[11],
+					buf.act_outputs.output[12],
+					buf.act_outputs.output[13],
+					buf.act_outputs.output[14],
+					buf.act_outputs.output[15]);
+				}
+			}
+
+			/* --- ACTUATOR OUTPUTS 2 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					5 /* port 5 */,
+					buf.act_outputs.output[ 8],
+					buf.act_outputs.output[ 9],
+					buf.act_outputs.output[10],
+					buf.act_outputs.output[11],
+					buf.act_outputs.output[12],
+					buf.act_outputs.output[13],
+					buf.act_outputs.output[14],
+					buf.act_outputs.output[15]);
+				}
+			}
+
+			/* --- ACTUATOR OUTPUTS 3 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					7 /* port 7 */,
+					buf.act_outputs.output[ 8],
+					buf.act_outputs.output[ 9],
+					buf.act_outputs.output[10],
+					buf.act_outputs.output[11],
+					buf.act_outputs.output[12],
+					buf.act_outputs.output[13],
+					buf.act_outputs.output[14],
+					buf.act_outputs.output[15]);
+				}
+			}
+
+			/* --- MAPPED MANUAL CONTROL INPUTS --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy local position data into local buffer */
+				orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
+				mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
+					buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1);
+			}
 		}
 	}
 
@@ -891,7 +1076,8 @@ void handleMessage(mavlink_message_t *msg)
 		mavlink_command_long_t cmd_mavlink;
 		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
-		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
 			//check for MAVLINK terminate command
 			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 				/* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app
@@ -1299,8 +1485,8 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	/* topics to subscribe globally */
 	/* subscribe to ORB for global position */
-	global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(global_pos_sub, 1000);	/* 1Hz active updates */
+	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	orb_set_interval(mavlink_subs.global_pos_sub, 1000);	/* 1Hz active updates */
 	/* subscribe to ORB for local position */
 	local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	orb_set_interval(local_pos_sub, 1000);	/* 1Hz active updates */
@@ -1315,7 +1501,7 @@ int mavlink_thread_main(int argc, char *argv[])
 	pthread_attr_init(&uorb_attr);
 	/* Set stack size, needs more than 4000 bytes */
 	pthread_attr_setstacksize(&uorb_attr, 4096);
-	pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
+	pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs);
 
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);
@@ -1323,27 +1509,52 @@ int mavlink_thread_main(int argc, char *argv[])
 	uint16_t counter = 0;
 	int lowspeed_counter = 0;
 
+	/* make sure all threads have registered their subscriptions */
+	while (!mavlink_subs.initialized) {
+		usleep(500);
+	}
+
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 921600) {
 		/* 500 Hz / 2 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 2);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
+		/* 200 Hz / 5 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
+		/* 5 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
 	} else if (baudrate >= 460800) {
 		/* 250 Hz / 4 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 5);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
+		/* 50 Hz / 20 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
+		/* 2 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 	} else if (baudrate >= 115200) {
 		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 50);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
+		/* 10 Hz / 100 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
+		/* 1 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
+		/* 5 Hz / 200 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+		/* 0.2 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
 	} else {
 		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
+		/* 0.5 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
+		/* 0.1 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
 	}
 
 	while (!thread_should_exit) {
@@ -1351,7 +1562,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		if (mavlink_exit_requested) break;
 
 		/* get local and global position */
-		orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+		orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
 		orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 
 		/* check if waypoint has been reached against the last positions */
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index baee507b2..33e7ab7e7 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -78,13 +78,11 @@ static enum {
 
 static bool thread_should_exit;
 static int mc_task;
+static bool motor_test_mode = false;
 
 static int
 mc_thread_main(int argc, char *argv[])
 {
-	bool motor_test_mode = false;
-
-	/* structures */
 	/* declare and safely initialize all structs */
 	struct vehicle_status_s state;
 	memset(&state, 0, sizeof(state));
@@ -109,7 +107,7 @@ mc_thread_main(int argc, char *argv[])
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	//int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	// int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
 	/* rate-limit the attitude subscription to 200Hz to pace our loop */
@@ -119,9 +117,10 @@ mc_thread_main(int argc, char *argv[])
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
 		actuators.control[i] = 0.0f;
-	int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-	armed.armed = true;
-	int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	armed.armed = false;
+	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -136,6 +135,8 @@ mc_thread_main(int argc, char *argv[])
 
 		perf_begin(mc_loop_perf);
 
+		/* get a local copy of system state */
+		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 		/* get a local copy of manual setpoint */
 		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 		/* get a local copy of attitude */
@@ -150,7 +151,7 @@ mc_thread_main(int argc, char *argv[])
 			att_sp.roll_body = 0.0f;
 			att_sp.pitch_body = 0.0f;
 			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
+			att_sp.thrust = 0.1f;
 		} else {
 			if (state.state_machine == SYSTEM_STATE_MANUAL ||
 				state.state_machine == SYSTEM_STATE_GROUND_READY ||
@@ -159,28 +160,30 @@ mc_thread_main(int argc, char *argv[])
 				state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
 				state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
 				att_sp.thrust = manual.throttle;
+				armed.armed = true;
 
 			} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
 				/* immediately cut off motors */
-				att_sp.thrust = 0.0f;
+				armed.armed = false;
 
 			} else {
 				/* limit motor throttle to zero for an unknown mode */
-				att_sp.thrust = 0.0f;
+				armed.armed = false;
 			}
 			
 		}
 
 		multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
-		//ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
 
 		/* publish the result */
+		orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 		perf_end(mc_loop_perf);
 	}
 
-	printf("[multirotor att control] stopping.\n");
+	printf("[multirotor att control] stopping, disarming motors.\n");
 
 	/* kill all outputs */
 	armed.armed = false;
@@ -208,8 +211,9 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] {start|stop}\n");
+	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|stop}\n");
 	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
+	fprintf(stderr, "    -t enables motor test mode with 10%% thrust\n");
 	exit(1);
 }
 
@@ -220,7 +224,7 @@ int multirotor_att_control_main(int argc, char *argv[])
 	control_mode = CONTROL_MODE_RATES;
 	unsigned int optioncount = 0;
 
-	while ((ch = getopt(argc, argv, "m:")) != EOF) {
+	while ((ch = getopt(argc, argv, "tm:")) != EOF) {
 		switch (ch) {
 		case 'm':
 			if (!strcmp(optarg, "rates")) {
@@ -231,24 +235,33 @@ int multirotor_att_control_main(int argc, char *argv[])
 				usage("unrecognized -m value");
 			}
 			optioncount += 2;
+			break;
+		case 't':
+			motor_test_mode = true;
+			optioncount += 1;
+			break;
+		case ':':
+			usage("missing parameter");
+			break;
 		default:
+			fprintf(stderr, "option: -%c\n", ch);
 			usage("unrecognized option");
 		}
 	}
 	argc -= optioncount;
-	argv += optioncount;
+	//argv += optioncount;
 
 	if (argc < 1)
 		usage("missing command");
 
-	if (!strcmp(argv[1], "start")) {
+	if (!strcmp(argv[1+optioncount], "start")) {
 
 		thread_should_exit = false;
 		mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[1+optioncount], "stop")) {
 		thread_should_exit = true;
 		exit(0);
 	}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index b21a20ad1..886ce304e 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -239,4 +239,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	actuators->control[1] = pitch_control;
 	actuators->control[2] = yaw_rate_control;
 	actuators->control[3] = motor_thrust;
+
+	motor_skip_counter++;
 }
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c
index ff32fb460..3f9c72517 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control.c
+++ b/apps/multirotor_pos_control/multirotor_pos_control.c
@@ -89,7 +89,7 @@ mpc_thread_main(int argc, char *argv[])
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 
 	/* publish attitude setpoint */
-	int att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
 	while (1) {
 		/* get a local copy of the vehicle state */
diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c
index 773cd87ff..17482dc0a 100644
--- a/apps/position_estimator/position_estimator_main.c
+++ b/apps/position_estimator/position_estimator_main.c
@@ -297,7 +297,7 @@ int position_estimator_main(int argc, char *argv[])
 		.lon = lon_current * 1e7,
 		.alt = gps.alt
 	};
-	int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+	orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
 
 	printf("[multirotor position estimator] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
 
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index e56ff1c3a..97d7d39b7 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -96,16 +96,16 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
 	mag_values.y = raw->magnetometer_ga[1]*456.0f;
 	mag_values.z = raw->magnetometer_ga[2]*456.0f;
 
-	static int i = 0;
-
-	if (i == 500) {
-		printf("gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
-			gyro_values.x, gyro_values.y, gyro_values.z,
-			accel_values.x, accel_values.y, accel_values.z,
-			mag_values.x, mag_values.y, mag_values.z);
-		i = 0;
-	}
-	i++;
+	// static int i = 0;
+
+	// if (i == 500) {
+	// 	printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
+	// 		gyro_values.x, gyro_values.y, gyro_values.z,
+	// 		accel_values.x, accel_values.y, accel_values.z,
+	// 		mag_values.x, mag_values.y, mag_values.z);
+	// 	i = 0;
+	// }
+	// i++;
 
 	attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
 
@@ -154,7 +154,7 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 	bool publishing = false;
 
 	/* advertise attitude */
-	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 	publishing = true;
 
 	struct pollfd fds[] = {
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 9d8847246..b5db823bd 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -64,6 +64,7 @@
 #include <arch/board/up_pwm_servo.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
 
 
 class FMUServo : public device::CDev
@@ -88,6 +89,7 @@ private:
 	int		_task;
 	int		_t_actuators;
 	int		_t_armed;
+	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
 
 	volatile bool	_task_should_exit;
@@ -98,15 +100,12 @@ private:
 	actuator_controls_s _controls;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main();
+	void		task_main() __attribute__((noreturn));
 
-	static int	control_callback_trampoline(uintptr_t handle,
+	static int	control_callback(uintptr_t handle,
 			uint8_t control_group,
 			uint8_t control_index,
 			float &input);
-	int		control_callback(uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
 };
 
 namespace
@@ -212,6 +211,11 @@ FMUServo::task_main()
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_armed, 100);		/* 10Hz update rate */
 
+	/* advertise the mixed control outputs */
+	struct actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+
 	struct pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
@@ -237,7 +241,6 @@ FMUServo::task_main()
 
 		/* do we have a control update? */
 		if (fds[0].revents & POLLIN) {
-			float outputs[num_outputs];
 
 			/* get controls - must always do this to avoid spinning */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
@@ -246,14 +249,20 @@ FMUServo::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				_mixers->mix(&outputs[0], num_outputs);
+				_mixers->mix(&outputs.output[0], num_outputs);
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
 					/* scale for PWM output 900 - 2100us */
-					up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
+					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+
+					/* output to the servo */
+					up_pwm_servo_set(i, outputs.output[i]);
 				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
 			}
 		}
 
@@ -264,13 +273,14 @@ FMUServo::task_main()
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 
-			/* update PMW servo armed status */
+			/* update PWM servo armed status */
 			up_pwm_servo_arm(aa.armed);
 		}
 	}
 
 	::close(_t_actuators);
 	::close(_t_armed);
+	::close(_t_outputs);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -285,24 +295,14 @@ FMUServo::task_main()
 }
 
 int
-FMUServo::control_callback_trampoline(uintptr_t handle,
+FMUServo::control_callback(uintptr_t handle,
 				      uint8_t control_group,
 				      uint8_t control_index,
 				      float &input)
 {
-	return ((FMUServo *)handle)->control_callback(control_group, control_index, input);
-}
-
-int
-FMUServo::control_callback(uint8_t control_group,
-			   uint8_t control_index,
-			   float &input)
-{
-	/* XXX currently only supporting group zero */
-	if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS))
-		return -1;
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-	input = _controls.control[control_index];
+	input = controls->control[control_index];
 	return 0;
 }
 
@@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MIXERIOCADDSIMPLE: {
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
-			SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
 
 			if (mixer->check()) {
 				delete mixer;
@@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			} else {
 				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
 
 				_mixers->add_mixer(mixer);
 			}
@@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 				_mixers = nullptr;
 			}
 
-			_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
 			if (_mixers->load_from_file(path) != 0) {
 				delete _mixers;
@@ -564,15 +566,13 @@ fake(int argc, char *argv[])
 
 	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
 
-	int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
 
 	if (handle < 0) {
 		puts("advertise failed");
 		exit(1);
 	}
 
-	close(handle);
-
 	exit(0);
 }
 
diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp
index a5def874d..c3371c138 100644
--- a/apps/px4/px4io/driver/px4io.cpp
+++ b/apps/px4/px4io/driver/px4io.cpp
@@ -99,7 +99,7 @@ protected:
 	void			set_channels(unsigned count, const servo_position_t *data);
 
 private:
-	int			_publication;
+	orb_advert_t		_publication;
 	struct rc_input_values	_input;
 };
 
diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile
index ad8849454..cb1c3c618 100644
--- a/apps/px4/tests/Makefile
+++ b/apps/px4/tests/Makefile
@@ -37,6 +37,6 @@
 
 APPNAME		 = tests
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 8096
+STACKSIZE	 = 12000
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
index 91397d11c..87ea7f058 100644
--- a/apps/px4/tests/test_sensors.c
+++ b/apps/px4/tests/test_sensors.c
@@ -90,11 +90,11 @@ struct {
 	const char	*path;
 	int	(* test)(int argc, char *argv[]);
 } sensors[] = {
-	{"l3gd20",	"/dev/l3gd20",	l3gd20},
 	{"bma180",	"/dev/bma180",	bma180},
+	{"mpu6000",	"/dev/accel",	mpu6000},
+	{"l3gd20",	"/dev/l3gd20",	l3gd20},
 	{"hmc5883l",	"/dev/hmc5883l",	hmc5883l},
 	{"ms5611",	"/dev/ms5611",	ms5611},
-	{"mpu6000",	"/dev/accel",	mpu6000},
 //    {"lis331",	"/dev/lis331",	lis331},
 	{NULL, NULL, NULL}
 };
@@ -253,6 +253,9 @@ l3gd20(int argc, char *argv[])
 static int
 bma180(int argc, char *argv[])
 {
+	// XXX THIS SENSOR IS OBSOLETE
+	// TEST REMAINS, BUT ALWAYS RETURNS OK
+
 	printf("\tBMA180: test start\n");
 	fflush(stdout);
 
@@ -264,7 +267,7 @@ bma180(int argc, char *argv[])
 
 	if (fd < 0) {
 		printf("\tBMA180: open fail\n");
-		return ERROR;
+		return OK;
 	}
 
 //	if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
@@ -283,7 +286,7 @@ bma180(int argc, char *argv[])
 	if (ret != sizeof(buf)) {
 		printf("\tBMA180: read1 fail (%d)\n", ret);
 		close(fd);
-		return ERROR;
+		return OK;
 
 	} else {
 		printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
@@ -297,7 +300,7 @@ bma180(int argc, char *argv[])
 	if (ret != sizeof(buf)) {
 		printf("\tBMA180: read2 fail (%d)\n", ret);
 		close(fd);
-		return ERROR;
+		return OK;
 
 	} else {
 		printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile
index f8dc64ed1..125839bb3 100644
--- a/apps/sensors/Makefile
+++ b/apps/sensors/Makefile
@@ -37,7 +37,6 @@
 
 APPNAME		= sensors
 PRIORITY	= SCHED_PRIORITY_MAX-5
-# Reduce to 4096 on next occasion
-STACKSIZE	= 8000
+STACKSIZE	= 4096
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index ae1e8d202..65ae336d9 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -57,6 +57,7 @@
 #include <arch/board/drv_bma180.h>
 #include <arch/board/drv_l3gd20.h>
 #include <arch/board/drv_hmc5883l.h>
+#include <drivers/drv_accel.h>
 #include <arch/board/up_adc.h>
 
 #include <systemlib/systemlib.h>
@@ -108,15 +109,17 @@ static int sensors_timer_loop_counter = 0;
 
 /* File descriptors for all sensors */
 static int fd_gyro = -1;
-static int fd_accelerometer = -1;
-static int fd_magnetometer = -1;
-static int fd_barometer = -1;
-static int fd_adc = -1;
 
 static bool thread_should_exit = false;
 static bool thread_running = false;
 static int sensors_task;
 
+static int fd_bma180 = -1;
+static int fd_magnetometer = -1;
+static int fd_barometer = -1;
+static int fd_adc = -1;
+static int fd_accelerometer = -1;
+
 /* Private functions declared static */
 static void sensors_timer_loop(void *arg);
 
@@ -126,8 +129,8 @@ extern unsigned ppm_decoded_channels;
 extern uint64_t ppm_last_valid_decode;
 #endif
 
-/* file handle that will be used for publishing sensor data */
-static int sensor_pub;
+/* ORB topic publishing our results */
+static orb_advert_t sensor_pub;
 
 PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
@@ -240,30 +243,70 @@ static int sensors_init(void)
 
 	/* open gyro */
 	fd_gyro = open("/dev/l3gd20", O_RDONLY);
+	int errno_gyro = (int)*get_errno_ptr();
 
-	if (fd_gyro < 0) {
-		fprintf(stderr, "[sensors]   L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-		fflush(stderr);
-		/* this sensor is critical, exit on failed init */
-		errno = ENOSYS;
-		return ERROR;
-
-	} else {
+	if (!(fd_gyro < 0)) {
 		printf("[sensors]   L3GD20 open ok\n");
 	}
 
-	/* open accelerometer */
-	fd_accelerometer = open("/dev/bma180", O_RDONLY);
+	/* open accelerometer, prefer the MPU-6000 */
+	fd_accelerometer = open("/dev/accel", O_RDONLY);
+	int errno_accelerometer = (int)*get_errno_ptr();
+
+	if (!(fd_accelerometer < 0)) {
+		printf("[sensors]   Accelerometer open ok\n");
+	}
 
+
+	/* only attempt to use BMA180 if MPU-6000 is not available */
+	int errno_bma180 = 0;
 	if (fd_accelerometer < 0) {
-		fprintf(stderr, "[sensors]   BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-		fflush(stderr);
-		/* this sensor is critical, exit on failed init */
+		fd_bma180 = open("/dev/bma180", O_RDONLY);
+		errno_bma180 = (int)*get_errno_ptr();
+
+		if (!(fd_bma180 < 0)) {
+			printf("[sensors]   Accelerometer (BMA180) open ok\n");
+		}
+	} else {
+		fd_bma180 = -1;
+	}
+
+	/* fail if no accelerometer is available */
+	if (fd_accelerometer < 0 && fd_bma180 < 0) {
+		/* print error message only if both failed, discard message else at all to not confuse users */
+		if (fd_accelerometer < 0) {
+			fprintf(stderr, "[sensors]   MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
+			fflush(stderr);
+			/* this sensor is redundant with BMA180 */
+		}
+		
+		if (fd_bma180 < 0) {
+			fprintf(stderr, "[sensors]   BMA180: open fail (err #%d): %s\n", errno_bma180, strerror(errno_bma180));
+			fflush(stderr);
+			/* this sensor is redundant with MPU-6000 */
+		}
+
 		errno = ENOSYS;
 		return ERROR;
+	}
 
-	} else {
-		printf("[sensors]   BMA180 open ok\n");
+	/* fail if no gyro is available */
+	if (fd_accelerometer < 0 && fd_bma180 < 0) {
+		/* print error message only if both failed, discard message else at all to not confuse users */
+		if (fd_accelerometer < 0) {
+			fprintf(stderr, "[sensors]   MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
+			fflush(stderr);
+			/* this sensor is redundant with BMA180 */
+		}
+		
+		if (fd_gyro < 0) {
+			fprintf(stderr, "[sensors]   L3GD20 open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro));
+			fflush(stderr);
+			/* this sensor is critical, exit on failed init */
+		}
+
+		errno = ENOSYS;
+		return ERROR;
 	}
 
 	/* open adc */
@@ -280,16 +323,18 @@ static int sensors_init(void)
 		printf("[sensors]   ADC open ok\n");
 	}
 
-	/* configure gyro */
-	if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
-		fprintf(stderr, "[sensors]   L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-		fflush(stderr);
-		/* this sensor is critical, exit on failed init */
-		errno = ENOSYS;
-		return ERROR;
+	/* configure gyro - if its not available and we got here the MPU-6000 is for sure available */
+	if (fd_gyro > 0) {
+		if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
+			fprintf(stderr, "[sensors]   L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+			fflush(stderr);
+			/* this sensor is critical, exit on failed init */
+			errno = ENOSYS;
+			return ERROR;
 
-	} else {
-		printf("[sensors]   L3GD20 configuration ok\n");
+		} else {
+			printf("[sensors]   L3GD20 configuration ok\n");
+		}
 	}
 
 	/* XXX Add IOCTL configuration of remaining sensors */
@@ -326,7 +371,7 @@ int sensors_thread_main(int argc, char *argv[])
 		fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n");
 		/* Clean up */
 		close(fd_gyro);
-		close(fd_accelerometer);
+		close(fd_bma180);
 		close(fd_magnetometer);
 		close(fd_barometer);
 		close(fd_adc);
@@ -382,6 +427,7 @@ int sensors_thread_main(int argc, char *argv[])
 
 	int16_t buf_gyro[3];
 	int16_t buf_accelerometer[3];
+	struct accel_report buf_accel_report;
 	int16_t buf_magnetometer[7];
 	float	buf_barometer[3];
 
@@ -425,8 +471,9 @@ int sensors_thread_main(int argc, char *argv[])
 
 	/* Empty sensor buffers, avoid junk values */
 	/* Read first two values of each sensor into void */
-	(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
-	(void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
+	if (fd_gyro > 0)(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
+	if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, 6);
+	if (fd_accelerometer > 0)(void)read(fd_accelerometer, buf_accelerometer, 12);
 	(void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
 
 	if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer));
@@ -466,7 +513,7 @@ int sensors_thread_main(int argc, char *argv[])
 						   .yaw = 0.0f,
 						   .throttle = 0.0f };
 
-	int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
+	orb_advert_t manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 
 	if (manual_control_pub < 0) {
 		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
@@ -475,7 +522,7 @@ int sensors_thread_main(int argc, char *argv[])
 	/* advertise the rc topic */
 	struct rc_channels_s rc;
 	memset(&rc, 0, sizeof(rc));
-	int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
+	orb_advert_t rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
 	if (rc_pub < 0) {
 		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n");
@@ -528,8 +575,13 @@ int sensors_thread_main(int argc, char *argv[])
 				if (vstatus.flag_hil_enabled && !hil_enabled) {
 					hil_enabled = true;
 					publishing = false;
-					int ret = close(sensor_pub);
-					printf("[sensors] Closing sensor pub: %i \n", ret);
+
+					int sens_ret = close(sensor_pub);
+					if (sens_ret == OK) {
+						printf("[sensors] Closing sensor pub OK\n");
+					} else {
+						printf("[sensors] FAILED Closing sensor pub, result: %i \n", sens_ret);
+					}
 
 					/* switching from HIL to non-HIL mode */
 
@@ -582,79 +634,122 @@ int sensors_thread_main(int argc, char *argv[])
 
 			paramcounter++;
 
-			/* try reading gyro */
-			uint64_t start_gyro = hrt_absolute_time();
-			ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
-			int gyrotime = hrt_absolute_time() - start_gyro;
+			if (fd_gyro > 0) {
+				/* try reading gyro */
+				uint64_t start_gyro = hrt_absolute_time();
+				ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
+				int gyrotime = hrt_absolute_time() - start_gyro;
 
-			if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
+				if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
 
-			/* GYROSCOPE */
-			if (ret_gyro != sizeof(buf_gyro)) {
-				gyro_fail_count++;
+				/* GYROSCOPE */
+				if (ret_gyro != sizeof(buf_gyro)) {
+					gyro_fail_count++;
 
-				if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
-					fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-				}
+					if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
+						fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+					}
 
-				if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
-					// global_data_send_subsystem_info(&gyro_present_enabled);
-					gyro_healthy = false;
-					gyro_success_count = 0;
-				}
+					if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
+						// global_data_send_subsystem_info(&gyro_present_enabled);
+						gyro_healthy = false;
+						gyro_success_count = 0;
+					}
 
-			} else {
-				gyro_success_count++;
+				} else {
+					gyro_success_count++;
+
+					if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
+						// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
+						gyro_healthy = true;
+						gyro_fail_count = 0;
 
-				if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
-					// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
-					gyro_healthy = true;
-					gyro_fail_count = 0;
+					}
 
+					gyro_updated = true;
 				}
 
-				gyro_updated = true;
+				gyrotime = hrt_absolute_time() - start_gyro;
+
+				if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
 			}
 
-			gyrotime = hrt_absolute_time() - start_gyro;
+			/* read MPU-6000 */
+			if (fd_accelerometer > 0) {
+				/* try reading acc */
+				uint64_t start_acc = hrt_absolute_time();
+				ret_accelerometer = read(fd_accelerometer, &buf_accel_report, sizeof(struct accel_report));
 
-			if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
+				/* ACCELEROMETER */
+				if (ret_accelerometer != sizeof(struct accel_report)) {
+					acc_fail_count++;
 
-			/* try reading acc */
-			uint64_t start_acc = hrt_absolute_time();
-			ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
+					if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+						fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+					}
 
-			/* ACCELEROMETER */
-			if (ret_accelerometer != sizeof(buf_accelerometer)) {
-				acc_fail_count++;
 
-				if (acc_fail_count & 0b111 || (acc_fail_count > 20 && acc_fail_count < 100)) {
-					fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-				}
+					if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
+						// global_data_send_subsystem_info(&acc_present_enabled);
+						gyro_healthy = false;
+						acc_success_count = 0;
+					}
 
-				if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
-					// global_data_send_subsystem_info(&acc_present_enabled);
-					gyro_healthy = false;
-					acc_success_count = 0;
-				}
+				} else {
+					acc_success_count++;
 
-			} else {
-				acc_success_count++;
+					if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
 
-				if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
+						// global_data_send_subsystem_info(&acc_present_enabled_healthy);
+						acc_healthy = true;
+						acc_fail_count = 0;
 
-					// global_data_send_subsystem_info(&acc_present_enabled_healthy);
-					acc_healthy = true;
-					acc_fail_count = 0;
+					}
 
+					acc_updated = true;
 				}
 
-				acc_updated = true;
+				int acctime = hrt_absolute_time() - start_acc;
+				if (acctime > 500) printf("ACC: %d us\n", acctime);
 			}
 
-			int acctime = hrt_absolute_time() - start_acc;
+			/* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */
+			if (fd_bma180 > 0) {
+				/* try reading acc */
+				uint64_t start_acc = hrt_absolute_time();
+				ret_accelerometer = read(fd_bma180, buf_accelerometer, 6);
 
-			if (acctime > 500) printf("ACC: %d us\n", acctime);
+				/* ACCELEROMETER */
+				if (ret_accelerometer != 6) {
+					acc_fail_count++;
+
+					if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+						fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+					}
+
+					if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
+						// global_data_send_subsystem_info(&acc_present_enabled);
+						gyro_healthy = false;
+						acc_success_count = 0;
+					}
+
+				} else {
+					acc_success_count++;
+
+					if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
+
+						// global_data_send_subsystem_info(&acc_present_enabled_healthy);
+						acc_healthy = true;
+						acc_fail_count = 0;
+
+					}
+
+					acc_updated = true;
+				}
+
+				int acctime = hrt_absolute_time() - start_acc;
+				if (acctime > 500) printf("ACC: %d us\n", acctime);
+			}
 
 			/* MAGNETOMETER */
 			if (magcounter == 4) { /* 120 Hz */
@@ -681,7 +776,8 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_magnetometer != sizeof(buf_magnetometer)) {
 					mag_fail_count++;
 
-					if (mag_fail_count & 0b111 || (mag_fail_count > 20 && mag_fail_count < 100)) {
+
+					if ((mag_fail_count % 20) == 0 || (mag_fail_count > 20 && mag_fail_count < 100)) {
 						fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
@@ -724,7 +820,7 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_barometer != sizeof(buf_barometer)) {
 					baro_fail_count++;
 
-					if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
+					if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
 						fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
@@ -761,10 +857,10 @@ int sensors_thread_main(int argc, char *argv[])
 				ret_adc = read(fd_adc, &buf_adc, adc_readsize);
 				nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
 
-				if (ret_adc  < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) {
+				if (ret_adc  < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) {
 					adc_fail_count++;
 
-					if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
+					if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
 						fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
@@ -804,7 +900,7 @@ int sensors_thread_main(int argc, char *argv[])
 				 */
 				if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
 					/* Read out values from HRT */
-					for (int i = 0; i < ppm_decoded_channels; i++) {
+					for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
 						rc.chan[i].raw = ppm_buffer[i];
 						/* Set the range to +-, then scale up */
 						rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000;
@@ -833,7 +929,7 @@ int sensors_thread_main(int argc, char *argv[])
 					if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
 					
 					/* throttle input */
-					manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f;
+					manual_control.throttle = (rc.chan[rc.function[THROTTLE]].scaled+1.0f)/2.0f;
 					if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
 					if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
 
@@ -874,19 +970,56 @@ int sensors_thread_main(int argc, char *argv[])
 			if (acc_updated) {
 				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
 
-				/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-				raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
-				raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32768 : buf_accelerometer[0]; // y on the board is x of the sensor
-				raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : buf_accelerometer[2]; // z of the board is z of the sensor
-
-				// XXX read range from sensor
-				float range_g = 4.0f;
-				/* scale from 14 bit to m/s2 */
-				raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) / 8192.0f) * range_g) * 9.81f;
-				raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) / 8192.0f) * range_g) * 9.81f;
-				raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) / 8192.0f) * range_g) * 9.81f;
-
-				raw.accelerometer_raw_counter++;
+				if (fd_accelerometer > 0) {
+					/* MPU-6000 values */
+
+					/* scale from 14 bit to m/s2 */
+					raw.accelerometer_m_s2[0] = buf_accel_report.x;
+					raw.accelerometer_m_s2[1] = buf_accel_report.y;
+					raw.accelerometer_m_s2[2] = buf_accel_report.z;
+
+					/* assign negated value, except for -SHORT_MAX, as it would wrap there */
+					raw.accelerometer_raw[0] = buf_accel_report.x*1000; // x of the board is -y of the sensor
+					raw.accelerometer_raw[1] = buf_accel_report.y*1000; // y on the board is x of the sensor
+					raw.accelerometer_raw[2] = buf_accel_report.z*1000; // z of the board is z of the sensor
+
+					raw.accelerometer_raw_counter++;
+				} else if (fd_bma180 > 0) {
+
+					/* assign negated value, except for -SHORT_MAX, as it would wrap there */
+					raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
+					raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
+					raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
+
+
+					// XXX read range from sensor
+					float range_g = 4.0f;
+					/* scale from 14 bit to m/s2 */
+					raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
+
+					raw.accelerometer_raw_counter++;
+				}
+
+				/* L3GD20 is not available, use MPU-6000 */
+				if (fd_accelerometer > 0 && fd_gyro < 0) {
+					raw.gyro_raw[0] = ((buf_accelerometer[3] == -32768) ? -32767 : buf_accelerometer[3]); // x of the board is y of the sensor
+					/* assign negated value, except for -SHORT_MAX, as it would wrap there */
+					raw.gyro_raw[1] = ((buf_accelerometer[4] == -32768) ? 32767 : -buf_accelerometer[4]); // y on the board is -x of the sensor
+					raw.gyro_raw[2] = ((buf_accelerometer[5] == -32768) ? -32767 : buf_accelerometer[5]); // z of the board is -z of the sensor
+
+					/* scale measurements */
+					// XXX request scaling from driver instead of hardcoding it
+					/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
+					raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f;
+					raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
+					raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
+
+					raw.gyro_raw_counter++;
+					/* mark as updated */
+					gyro_updated = true;
+				}
 			}
 
 			/* MAGNETOMETER */
@@ -988,7 +1121,7 @@ int sensors_thread_main(int argc, char *argv[])
 	printf("[sensors] sensor readout stopped\n");
 
 	close(fd_gyro);
-	close(fd_accelerometer);
+	close(fd_bma180);
 	close(fd_magnetometer);
 	close(fd_barometer);
 	close(fd_adc);
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index 05bae007b..6b92e61ef 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/apps/systemlib/bson/tinybson.h
@@ -34,7 +34,7 @@
  /**
  * @file tinybson.h
  *
- * A simple subset SAX-style BSON parser and generator.
+ * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
  *
  * Some types and defines taken from the standalone BSON parser/generator 
  * in the Mongo C connector.
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index 7d0c75670..acd5674ab 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -91,6 +91,9 @@ UT_icd		param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
 /** parameter update topic */
 ORB_DEFINE(parameter_update, struct parameter_update_s);
 
+/** parameter update topic handle */
+static orb_advert_t param_topic = -1;
+
 /** lock the parameter store */
 static void
 param_lock(void)
@@ -390,13 +393,15 @@ out:
 		struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
 
 		/*
-		 * Because we're a library, we can't keep a persistent advertisement
-		 * around, so if we succeed in updating the topic, we have to toss
-		 * the descriptor straight away.
+		 * If we don't have a handle to our topic, create one now; otherwise
+		 * just publish.
 		 */
-		int param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
-		if (param_topic != -1)
-			close(param_topic);
+		if (param_topic == -1) {
+			param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
+		} else {
+			orb_publish(ORB_ID(parameter_update), param_topic, &pup);
+		}
+
 	}
 
 	return result;
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index d6b566b6a..d6c00f996 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -114,3 +114,9 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
+
+#include "topics/actuator_outputs.h"
+ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
+ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
+ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
+ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h
new file mode 100644
index 000000000..202f72b54
--- /dev/null
+++ b/apps/uORB/topics/actuator_outputs.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_outputs.h
+ *
+ * Actuator output values.
+ *
+ * Values published to these topics are the outputs of the control mixing
+ * system as sent to the actuators (servos, motors, etc.) that operate
+ * the vehicle.
+ *
+ * Each topic can be published by a single output driver.
+ */
+
+#ifndef TOPIC_ACTUATOR_OUTPUTS_H
+#define TOPIC_ACTUATOR_OUTPUTS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATOR_OUTPUTS		16
+#define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
+
+struct actuator_outputs_s {
+	float	output[NUM_ACTUATOR_OUTPUTS];
+};
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(actuator_outputs_0);
+ORB_DECLARE(actuator_outputs_1);
+ORB_DECLARE(actuator_outputs_2);
+ORB_DECLARE(actuator_outputs_3);
+
+/* output sets with pre-defined applications */
+#define ORB_ID_VEHICLE_CONTROLS		ORB_ID(actuator_outputs_0)
+
+#endif
\ No newline at end of file
diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp
index 1e7cdc8db..c4434e4ed 100644
--- a/apps/uORB/uORB.cpp
+++ b/apps/uORB/uORB.cpp
@@ -112,6 +112,8 @@ public:
 	virtual ssize_t		write(struct file *filp, const char *buffer, size_t buflen);
 	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	static ssize_t		publish(const orb_metadata *meta, orb_advert_t handle, const void *data);
+
 protected:
 	virtual pollevent_t	poll_state(struct file *filp);
 	virtual void		poll_notify_one(struct pollfd *fds, pollevent_t events);
@@ -298,6 +300,8 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
 	 *
 	 * Writes outside interrupt context will allocate the object
 	 * if it has not yet been allocated.
+	 *
+	 * Note that filp will usually be NULL.
 	 */
 	if (nullptr == _data) {
 		if (!up_interrupt_context()) {
@@ -353,12 +357,42 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		sd->update_interval = arg;
 		return OK;
 
+	case ORBIOCGADVERTISER:
+		*(uintptr_t *)arg = (uintptr_t)this;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
 	}
 }
 
+ssize_t
+ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
+{
+	ORBDevNode *devnode = (ORBDevNode *)handle;
+	int ret;
+
+	/* this is a bit risky, since we are trusting the handle in order to deref it */
+	if (devnode->_meta != meta) {
+		errno = EINVAL;
+		return ERROR;
+	}
+
+	/* call the devnode write method with no file pointer */
+	ret = devnode->write(nullptr, (const char *)data, meta->o_size);
+
+	if (ret < 0)
+		return ERROR;
+
+	if (ret != (int)meta->o_size) {
+		errno = EIO;
+		return ERROR;
+	}
+
+	return OK;
+}
+
 pollevent_t
 ORBDevNode::poll_state(struct file *filp)
 {
@@ -614,7 +648,7 @@ test()
 	if (pfd < 0)
 		return test_fail("advertise failed: %d", errno);
 
-	test_note("publish fd %d", pfd);
+	test_note("publish handle 0x%08x", pfd);
 	sfd = orb_subscribe(ORB_ID(orb_test));
 
 	if (sfd < 0)
@@ -877,29 +911,35 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 		return ERROR;
 	}
 
-	/* the advertiser must perform an initial publish to initialise the object */
-	if (advertiser) {
-		ret = orb_publish(meta, fd, data);
-
-		if (ret != OK) {
-			/* save errno across the close */
-			ret = errno;
-			close(fd);
-			errno = ret;
-			return ERROR;
-		}
-	}
-
 	/* everything has been OK, we can return the handle now */
 	return fd;
 }
 
 } // namespace
 
-int
+orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
 {
-	return node_open(PUBSUB, meta, data, true);
+	int result, fd;
+	orb_advert_t advertiser;
+
+	/* open the node as an advertiser */
+	fd = node_open(PUBSUB, meta, data, true);
+	if (fd == ERROR)
+		return ERROR;
+
+	/* get the advertiser handle and close the node */
+	result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	close(fd);
+	if (result == ERROR)
+		return ERROR;
+
+	/* the advertiser must perform an initial publish to initialise the object */
+	result= orb_publish(meta, advertiser, data);
+	if (result == ERROR)
+		return ERROR;
+
+	return advertiser;
 }
 
 int
@@ -915,21 +955,9 @@ orb_unsubscribe(int handle)
 }
 
 int
-orb_publish(const struct orb_metadata *meta, int handle, const void *data)
+orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
-	int ret;
-
-	ret = write(handle, data, meta->o_size);
-
-	if (ret < 0)
-		return ERROR;
-
-	if (ret != (int)meta->o_size) {
-		errno = EIO;
-		return ERROR;
-	}
-
-	return OK;
+	return ORBDevNode::publish(meta, handle, data);
 }
 
 int
diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h
index c36d0044f..eb068d2b7 100644
--- a/apps/uORB/uORB.h
+++ b/apps/uORB/uORB.h
@@ -106,11 +106,26 @@ struct orb_metadata {
 __BEGIN_DECLS
 
 /**
+ * ORB topic advertiser handle.
+ *
+ * Advertiser handles are global; once obtained they can be shared freely
+ * and do not need to be closed or released.
+ *
+ * This permits publication from interrupt context and other contexts where
+ * a file-descriptor-based handle would not otherwise be in scope for the
+ * publisher.
+ */
+typedef intptr_t	orb_advert_t;
+
+/**
  * Advertise as the publisher of a topic.
  *
  * This performs the initial advertisement of a topic; it creates the topic
  * node in /obj if required and publishes the initial data.
  *
+ * Any number of advertisers may publish to a topic; publications are atomic
+ * but co-ordination between publishers is not provided by the ORB. 
+ *
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
  * @param data		A pointer to the initial data to be published.
@@ -122,7 +137,7 @@ __BEGIN_DECLS
  *			ORB_DEFINE with no corresponding ORB_DECLARE)
  *			this function will return -1 and set errno to ENOENT.
  */
-extern int	orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
+extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
 /**
  * Publish new data to a topic.
@@ -131,13 +146,13 @@ extern int	orb_advertise(const struct orb_metadata *meta, const void *data) __EX
  * will be notified.  Subscribers that are not waiting can check the topic
  * for updates using orb_check and/or orb_stat.
  *
- * @handle		The handle returned from orb_advertise.
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
+ * @handle		The handle returned from orb_advertise.
  * @param data		A pointer to the data to be published.
  * @return		OK on success, ERROR otherwise with errno set accordingly.
  */
-extern int	orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT;
+extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
 
 /**
  * Subscribe to a topic.