Merged master into beta

23 files changed, 458 insertions, 508 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index f73a3ef01..771f2128f 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	_collect_phase(false),
 	_diff_pres_offset(0.0f),
 	_airspeed_pub(-1),
+	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
@@ -102,6 +103,9 @@ Airspeed::~Airspeed()
 	/* make sure we are truly inactive */
 	stop();
 
+	if (_class_instance != -1)
+		unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance);
+
 	/* free any existing reports */
 	if (_reports != nullptr)
 		delete _reports;
@@ -126,13 +130,23 @@ Airspeed::init()
 	if (_reports == nullptr)
 		goto out;
 
-	/* get a publish handle on the airspeed topic */
-	differential_pressure_s zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
+
+	/* publication init */
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct differential_pressure_s arp;
+		measure();
+		_reports->get(&arp);
 
-	if (_airspeed_pub < 0)
-		warnx("failed to create airspeed sensor object. Did you start uOrb?");
+		/* measurement will have generated a report, publish */
+		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
+
+		if (_airspeed_pub < 0)
+			warnx("failed to create airspeed sensor object. uORB started?");
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index c341aa2c6..c27b1bcd8 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -127,6 +127,8 @@ protected:
 
 	orb_advert_t		_airspeed_pub;
 
+	int			_class_instance;
+
 	unsigned		_conversion_interval;
 
 	perf_counter_t		_sample_perf;
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 1590cc182..e43a34805 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -153,6 +153,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	int			_class_instance;
 
 	unsigned		_current_lowpass;
 	unsigned		_current_range;
@@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_class_instance(-1),
 	_current_lowpass(0),
 	_current_range(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
@@ -282,11 +284,6 @@ BMA180::init()
 	if (_reports == nullptr)
 		goto out;
 
-	/* advertise sensor topic */
-	struct accel_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
-
 	/* perform soft reset (p48) */
 	write_reg(ADDR_RESET, SOFT_RESET);
 
@@ -322,6 +319,19 @@ BMA180::init()
 		ret = ERROR;
 	}
 
+	_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
+	/* advertise sensor topic, measure manually to initialize valid report */
+	measure();
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		struct accel_report arp;
+		_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+	}
+
 out:
 	return ret;
 }
@@ -723,7 +733,8 @@ BMA180::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
+	if (_accel_topic > 0 && !(_pub_blocked))
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 7954ce5ab..b157b3f18 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +38,7 @@
  */
 
 #include "device.h"
+#include "drivers/drv_device.h"
 
 #include <sys/ioctl.h>
 #include <arch/irq.h>
@@ -93,6 +94,7 @@ CDev::CDev(const char *name,
 	Device(name, irq),
 	// public
 	// protected
+	_pub_blocked(false),
 	// private
 	_devname(devname),
 	_registered(false),
@@ -256,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case DIOC_GETPRIV:
 		*(void **)(uintptr_t)arg = (void *)this;
 		return OK;
+		break;
+	case DEVIOCSPUBBLOCK:
+		_pub_blocked = (arg != 0);
+		return OK;
+		break;
+	case DEVIOCGPUBBLOCK:
+		return _pub_blocked;
+		break;
 	}
 
 	return -ENOTTY;
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index c3ee77b1c..683455149 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 0235f6284..d99f22922 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -415,6 +415,8 @@ protected:
 	 */
 	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
 
+	bool		_pub_blocked;		/**< true if publishing should be blocked */
+
 private:
 	static const unsigned _max_pollwaiters = 8;
 
diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h
new file mode 100644
index 000000000..b310beb74
--- /dev/null
+++ b/src/drivers/drv_device.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_device.h
+ *
+ * Generic device / sensor interface.
+ */
+
+#ifndef _DRV_DEVICE_H
+#define _DRV_DEVICE_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _DEVICEIOCBASE		(0x100)
+#define _DEVICEIOC(_n)		(_IOC(_DEVICEIOCBASE, _n))
+
+/** ask device to stop publishing */
+#define DEVIOCSPUBBLOCK	_DEVICEIOC(0)
+
+/** check publication block status */
+#define DEVIOCGPUBBLOCK	_DEVICEIOC(1)
+
+#endif /* _DRV_DEVICE_H */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index de371bf32..8bbef5cfa 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -184,8 +184,10 @@ ETSAirspeed::collect()
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+	if (_airspeed_pub > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+	}
 
 	new_report(report);
 
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6b72d560f..a736cbdf6 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -289,11 +289,13 @@ GPS::task_main()
 
 			//no time and satellite information simulated
 
-			if (_report_pub > 0) {
-				orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+			if (!(_pub_blocked)) {
+				if (_report_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
 
-			} else {
-				_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+				} else {
+					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+				}
 			}
 
 			usleep(2e5);
@@ -330,11 +332,14 @@ GPS::task_main()
 				while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
 	//				lock();
 					/* opportunistic publishing - else invalid data would end up on the bus */
-					if (_report_pub > 0) {
-						orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
 
-					} else {
-						_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+					if (!(_pub_blocked)) {
+						if (_report_pub > 0) {
+							orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
+						} else {
+							_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+						}
 					}
 
 					last_rate_count++;
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 2e2cbc8dd..2360ff39b 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,6 +39,9 @@
 
 /**
  * @file gps_helper.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 float
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index 73d4b889c..cfb9e0d43 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +33,8 @@
 
 /**
  * @file gps_helper.h
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef GPS_HELPER_H
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index 097db2abf..82c67d40a 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 56b702ea6..c90ecbe28 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,7 +31,12 @@
  *
  ****************************************************************************/
 
-/* @file mkt.cpp */
+/**
+ * @file mtk.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
 
 #include <unistd.h>
 #include <stdio.h>
diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
index b5cfbf0a6..a2d5e27bb 100644
--- a/src/drivers/gps/mtk.h
+++ b/src/drivers/gps/mtk.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,7 +31,12 @@
  *
  ****************************************************************************/
 
-/* @file mtk.h */
+/**
+ * @file mtk.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
 
 #ifndef MTK_H_
 #define MTK_H_
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 86291901c..8a2afecb7 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,8 +37,13 @@
  * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
  * including Prototol Specification.
  *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
  */
+
 #include <assert.h>
 #include <math.h>
 #include <poll.h>
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 76ef873a3..79a904f4a 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,7 +31,17 @@
  *
  ****************************************************************************/
 
-/* @file U-Blox protocol definitions */
+/**
+ * @file ubx.h
+ *
+ * U-Blox protocol definition. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ */
 
 #ifndef UBX_H_
 #define UBX_H_
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 9b9c11af2..4c85c0cda 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -381,16 +381,6 @@ HMC5883::init()
 	reset();
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the mag topic if we are
-		 * the primary mag */
-		struct mag_report zero_report;
-		memset(&zero_report, 0, sizeof(zero_report));
-		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
-
-		if (_mag_topic < 0)
-			debug("failed to create sensor_mag object");
-	}
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -867,9 +857,17 @@ HMC5883::collect()
 	/* z remains z */
 	new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 
-	if (_mag_topic != -1) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_mag_topic != -1) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+		} else {
+			_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
+
+			if (_mag_topic < 0)
+				debug("failed to create sensor_mag publication");
+		}
 	}
 
 	/* post a report to the ring */
@@ -1140,10 +1138,12 @@ int HMC5883::check_calibration()
 			SUBSYSTEM_TYPE_MAG};
 		static orb_advert_t pub = -1;
 
-		if (pub > 0) {
-			orb_publish(ORB_ID(subsystem_info), pub, &info);
-		} else {
-			pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		if (!(_pub_blocked)) {
+			if (pub > 0) {
+				orb_publish(ORB_ID(subsystem_info), pub, &info);
+			} else {
+				pub = orb_advertise(ORB_ID(subsystem_info), &info);
+			}
 		}
 	}
 
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 670e51b97..90c3db9ae 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -379,15 +379,24 @@ L3GD20::init()
 		goto out;
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
-        if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* advertise sensor topic */
-		struct gyro_report zero_report;
-		memset(&zero_report, 0, sizeof(zero_report));
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
-        }
 
 	reset();
 
+	measure();
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct gyro_report grp;
+		_reports->get(&grp);
+
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+
+		if (_gyro_topic < 0)
+			debug("failed to create sensor_gyro publication");
+
+	}
+
 	ret = OK;
 out:
 	return ret;
@@ -894,8 +903,10 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_gyro_topic > 0)
+	if (_gyro_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
 		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+	}
 
 	_read++;
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 969b5e25f..4dee7649b 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -277,15 +277,15 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
-	int			_class_instance;
+	int			_accel_class_instance;
 
 	unsigned		_accel_read;
 	unsigned		_mag_read;
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
-	perf_counter_t		_reg7_resets;
 	perf_counter_t		_reg1_resets;
+	perf_counter_t		_reg7_resets;
 	perf_counter_t		_extreme_values;
 	perf_counter_t		_accel_reschedules;
 
@@ -295,8 +295,8 @@ private:
 
 	// expceted values of reg1 and reg7 to catch in-flight
 	// brownouts of the sensor
-	uint8_t			_reg7_expected;
 	uint8_t			_reg1_expected;
+	uint8_t			_reg7_expected;
 
 	// accel logging
 	int			_accel_log_fd;
@@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
-	_class_instance(-1),
+	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
@@ -551,8 +551,8 @@ LSM303D::~LSM303D()
 	if (_mag_reports != nullptr)
 		delete _mag_reports;
 
-	if (_class_instance != -1)
-		unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
+	if (_accel_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
 
 	delete _mag;
 
@@ -562,13 +562,13 @@ LSM303D::~LSM303D()
 	perf_free(_reg1_resets);
 	perf_free(_reg7_resets);
 	perf_free(_extreme_values);
+	perf_free(_accel_reschedules);
 }
 
 int
 LSM303D::init()
 {
 	int ret = ERROR;
-	int mag_ret;
 
 	/* do SPI init (and probe) first */
 	if (SPI::init() != OK) {
@@ -597,13 +597,37 @@ LSM303D::init()
 		goto out;
 	}
 
-	_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		// we are the primary accel device, so advertise to
-		// the ORB
-		struct accel_report zero_report;
-		memset(&zero_report, 0, sizeof(zero_report));
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
+	/* fill report structures */
+	measure();
+
+	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct mag_report mrp;
+		_mag_reports->get(&mrp);
+
+		/* measurement will have generated a report, publish */
+		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
+
+		if (_mag->_mag_topic < 0)
+			debug("failed to create sensor_mag publication");
+
+	}
+
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct accel_report arp;
+		_accel_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+
+		if (_accel_topic < 0)
+			debug("failed to create sensor_accel publication");
+
 	}
 
 out:
@@ -727,7 +751,7 @@ LSM303D::check_extremes(const accel_report *arb)
 		_last_log_us = now;
 		::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
 			  (unsigned long long)arb->timestamp, 
-			  arb->x, arb->y, arb->z,
+			  (double)arb->x, (double)arb->y, (double)arb->z,
 			  (int)arb->x_raw,
 			  (int)arb->y_raw,
 			  (int)arb->z_raw,
@@ -1517,8 +1541,8 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_topic != -1) {
-		/* publish for subscribers */
+	if (_accel_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 	}
 
@@ -1591,8 +1615,8 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_topic != -1) {
-		/* publish for subscribers */
+	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
 		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
 	}
 
@@ -1707,13 +1731,6 @@ LSM303D_mag::init()
 		goto out;
 
 	_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
-	if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
-		// we are the primary mag device, so advertise to
-		// the ORB
-		struct mag_report zero_report;
-		memset(&zero_report, 0, sizeof(zero_report));
-		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
-	}
 
 out:
 	return ret;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 9251cff7b..05ae21c1f 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -208,8 +208,10 @@ MEASAirspeed::collect()
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+	if (_airspeed_pub > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+	}
 
 	new_report(report);
 
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 30d6069b3..ec5f77d74 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
  *   Author: Marco Bauer <marco@wtns.de>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -65,7 +65,6 @@
 
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 
@@ -93,16 +92,12 @@
 #define MOTOR_SPINUP_COUNTER			30
 #define ESC_UORB_PUBLISH_DELAY		500000
 
+
+
+
 class MK : public device::I2C
 {
 public:
-	enum Mode {
-		MODE_NONE,
-		MODE_2PWM,
-		MODE_4PWM,
-		MODE_6PWM,
-	};
-
 	enum MappingMode {
 		MAPPING_MK = 0,
 		MAPPING_PX4,
@@ -120,8 +115,7 @@ public:
 	virtual int	init(unsigned motors);
 	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
 
-	int		set_mode(Mode mode);
-	int		set_pwm_rate(unsigned rate);
+	int		set_update_rate(unsigned rate);
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
@@ -133,7 +127,6 @@ private:
 	static const unsigned _max_actuators = MAX_MOTORS;
 	static const bool	showDebug = false;
 
-	Mode		_mode;
 	int 		_update_rate;
 	int 		_current_update_rate;
 	int		_task;
@@ -180,33 +173,15 @@ private:
 	static const GPIOConfig	_gpio_tab[];
 	static const unsigned	_ngpio;
 
-	void		gpio_reset(void);
-	void		gpio_set_function(uint32_t gpios, int function);
-	void		gpio_write(uint32_t gpios, int function);
-	uint32_t	gpio_read(void);
-	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 	int			mk_servo_arm(bool status);
-
 	int 		mk_servo_set(unsigned int chan, short val);
 	int 		mk_servo_set_value(unsigned int chan, short val);
 	int 		mk_servo_test(unsigned int chan);
 	short		scaling(float val, float inMin, float inMax, float outMin, float outMax);
-
-
 };
 
-const MK::GPIOConfig MK::_gpio_tab[] = {
-	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
-	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
-	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
-	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
-	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
-	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
-	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
-	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
-};
 
-const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
 
 const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad + configuration
 const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 };	// Addresstranslator for Hexa + configuration
@@ -247,8 +222,7 @@ MK	*g_mk;
 
 MK::MK(int bus, const char *_device_path) :
 	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
-	_mode(MODE_NONE),
-	_update_rate(50),
+	_update_rate(400),
 	_task(-1),
 	_t_actuators(-1),
 	_t_actuator_armed(-1),
@@ -317,26 +291,23 @@ MK::init(unsigned motors)
 
 	usleep(500000);
 
-   if (sizeof(_device) > 0) {
-	   ret = register_driver(_device, &fops, 0666, (void *)this);
+	if (sizeof(_device) > 0) {
+		ret = register_driver(_device, &fops, 0666, (void *)this);
 
-	   if (ret == OK) {
+		if (ret == OK) {
 			log("creating alternate output device");
 			_primary_pwm_device = true;
 		}
 
-   }
-
-	/* reset GPIOs */
-	gpio_reset();
+	}
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("mkblctrl",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_MAX - 20,
-			   2048,
-			   (main_t)&MK::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX - 20,
+			       2048,
+			       (main_t)&MK::task_main_trampoline,
+			       nullptr);
 
 
 	if (_task < 0) {
@@ -354,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[])
 }
 
 int
-MK::set_mode(Mode mode)
-{
-	/*
-	 * Configure for PWM output.
-	 *
-	 * Note that regardless of the configured mode, the task is always
-	 * listening and mixing; the mode just selects which of the channels
-	 * are presented on the output pins.
-	 */
-	switch (mode) {
-	case MODE_2PWM:
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;	/* default output rate */
-		break;
-
-	case MODE_4PWM:
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;	/* default output rate */
-		break;
-
-	case MODE_NONE:
-		debug("MODE_NONE");
-		/* disable servo outputs and set a very low update rate */
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	_mode = mode;
-	return OK;
-}
-
-int
-MK::set_pwm_rate(unsigned rate)
+MK::set_update_rate(unsigned rate)
 {
 	if ((rate > 500) || (rate < 10))
 		return -EINVAL;
@@ -621,11 +556,13 @@ MK::task_main()
 						}
 					}
 
-					if(!_overrideSecurityChecks) {
+					if (!_overrideSecurityChecks) {
 						/* don't go under BLCTRL_MIN_VALUE */
+
 						if (outputs.output[i] < BLCTRL_MIN_VALUE) {
 							outputs.output[i] = BLCTRL_MIN_VALUE;
 						}
+
 					}
 
 					/* output to BLCtrl's */
@@ -675,21 +612,24 @@ MK::task_main()
 				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
 				esc.esc[i].esc_rpm = (uint16_t) 0;
 				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
+
 				if (Motor[i].Version == 1) {
 					// BLCtrl 2.0 (11Bit)
-					esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
+					esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits;
+
 				} else {
 					// BLCtrl < 2.0 (8Bit)
 					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
 				}
+
 				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
 
-   			// if motortest is requested - do it...
-   			if (_motortest == true) {
-	   		   mk_servo_test(i);
-			   }
+				// if motortest is requested - do it...
+				if (_motortest == true) {
+					mk_servo_test(i);
+				}
 
 			}
 
@@ -728,7 +668,7 @@ MK::mk_servo_arm(bool status)
 unsigned int
 MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 {
-	if(initI2C) {
+	if (initI2C) {
 		I2C::init();
 	}
 
@@ -781,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
-		
-		if(!_overrideSecurityChecks) {
+
+		if (!_overrideSecurityChecks) {
 			if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
 				_task_should_exit = true;
 			}
@@ -811,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val)
 		tmpVal = 0;
 	}
 
-	Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
-	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
+	Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff;
+	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07;
 
 	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
@@ -1019,28 +959,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret;
 
-	// XXX disabled, confusing users
-
-	/* try it as a GPIO ioctl first */
-	ret = gpio_ioctl(filp, cmd, arg);
-
-	if (ret != -ENOTTY)
-		return ret;
-
-	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
-	/*
-	switch (_mode) {
-	case MODE_2PWM:
-	case MODE_4PWM:
-	case MODE_6PWM:
-		ret = pwm_ioctl(filp, cmd, arg);
-		break;
-
-	default:
-		debug("not in a PWM mode");
-		break;
-	}
-	*/
 	ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
@@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		ret = OK;
 		break;
 
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = _update_rate;
+		break;
+
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = OK;
 		break;
@@ -1084,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		if (arg < 2150) {
 			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
 			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
+
 		} else {
 			ret = -EINVAL;
 		}
@@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len)
 	return count * 2;
 }
 
-void
-MK::gpio_reset(void)
-{
-	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
-	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
-
-	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-	stm32_configgpio(GPIO_GPIO_DIR);
-}
-
-void
-MK::gpio_set_function(uint32_t gpios, int function)
-{
-	/*
-	 * GPIOs 0 and 1 must have the same direction as they are buffered
-	 * by a shared 2-port driver.  Any attempt to set either sets both.
-	 */
-	if (gpios & 3) {
-		gpios |= 3;
-
-		/* flip the buffer to output mode if required */
-		if (GPIO_SET_OUTPUT == function)
-			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
-	}
-
-	/* configure selected GPIOs as required */
-	for (unsigned i = 0; i < _ngpio; i++) {
-		if (gpios & (1 << i)) {
-			switch (function) {
-			case GPIO_SET_INPUT:
-				stm32_configgpio(_gpio_tab[i].input);
-				break;
-
-			case GPIO_SET_OUTPUT:
-				stm32_configgpio(_gpio_tab[i].output);
-				break;
-
-			case GPIO_SET_ALT_1:
-				if (_gpio_tab[i].alt != 0)
-					stm32_configgpio(_gpio_tab[i].alt);
-
-				break;
-			}
-		}
-	}
-
-	/* flip buffer to input mode if required */
-	if ((GPIO_SET_INPUT == function) && (gpios & 3))
-		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-}
-
-void
-MK::gpio_write(uint32_t gpios, int function)
-{
-	int value = (function == GPIO_SET) ? 1 : 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (gpios & (1 << i))
-			stm32_gpiowrite(_gpio_tab[i].output, value);
-}
-
-uint32_t
-MK::gpio_read(void)
-{
-	uint32_t bits = 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (stm32_gpioread(_gpio_tab[i].input))
-			bits |= (1 << i);
-
-	return bits;
-}
-
-int
-MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	ret = OK;
-
-	lock();
-
-	switch (cmd) {
-
-	case GPIO_RESET:
-		gpio_reset();
-		break;
-
-	case GPIO_SET_OUTPUT:
-	case GPIO_SET_INPUT:
-	case GPIO_SET_ALT_1:
-		gpio_set_function(arg, cmd);
-		break;
-
-	case GPIO_SET_ALT_2:
-	case GPIO_SET_ALT_3:
-	case GPIO_SET_ALT_4:
-		ret = -EINVAL;
-		break;
-
-	case GPIO_SET:
-	case GPIO_CLEAR:
-		gpio_write(arg, cmd);
-		break;
-
-	case GPIO_GET:
-		*(uint32_t *)arg = gpio_read();
-		break;
-
-	default:
-		ret = -ENOTTY;
-	}
-
-	unlock();
-
-	return ret;
-}
 
 namespace
 {
 
-enum PortMode {
-	PORT_MODE_UNSET = 0,
-	PORT_FULL_GPIO,
-	PORT_FULL_SERIAL,
-	PORT_FULL_PWM,
-	PORT_GPIO_AND_SERIAL,
-	PORT_PWM_AND_SERIAL,
-	PORT_PWM_AND_GPIO,
-};
-
 enum MappingMode {
 	MAPPING_MK = 0,
 	MAPPING_PX4,
@@ -1341,20 +1135,11 @@ enum FrameType {
 	FRAME_X,
 };
 
-PortMode g_port_mode;
 
 int
-mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
+mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
 {
-	uint32_t gpio_bits;
 	int shouldStop = 0;
-	MK::Mode servo_mode;
-
-	/* reset to all-inputs */
-	g_mk->ioctl(0, GPIO_RESET, 0);
-
-	gpio_bits = 0;
-	servo_mode = MK::MODE_NONE;
 
 	/* native PX4 addressing) */
 	g_mk->set_px4mode(px4mode);
@@ -1368,7 +1153,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	/* ovveride security checks if enabled */
 	g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
 
-
 	/* count used motors */
 	do {
 		if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
@@ -1383,86 +1167,54 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 
 	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
 
-	/* (re)set the PWM output mode */
-	g_mk->set_mode(servo_mode);
-
-
-	if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
-		g_mk->set_pwm_rate(update_rate);
+	g_mk->set_update_rate(update_rate);
 
 	return OK;
 }
 
 int
-mk_start(unsigned bus, unsigned motors, char *device_path)
-{
-	int ret = OK;
-
-	if (g_mk == nullptr) {
-
-		g_mk = new MK(bus, device_path);
-
-		if (g_mk == nullptr) {
-			ret = -ENOMEM;
-
-		} else {
-			ret = g_mk->init(motors);
-
-			if (ret != OK) {
-				delete g_mk;
-				g_mk = nullptr;
-			}
-		}
-	}
-
-	return ret;
-}
-
-
-int
-mk_check_for_i2c_esc_bus(char *device_path, int motors)
+mk_start(unsigned motors, char *device_path)
 {
 	int ret;
 
-	if (g_mk == nullptr) {
+	// try i2c3 first
+	g_mk = new MK(3, device_path);
 
-		g_mk = new MK(3, device_path);
+	if (!g_mk)
+		return -ENOMEM;
 
-		if (g_mk == nullptr) {
-			return -1;
-
-		} else {
-			ret = g_mk->mk_check_for_blctrl(8, false, true);
-			delete g_mk;
-			g_mk = nullptr;
-
-			if (ret > 0) {
-				return 3;
-			}
+	if (OK == g_mk->init(motors)) {
+		warnx("[mkblctrl] scanning i2c3...\n");
+		ret = g_mk->mk_check_for_blctrl(8, false, true);
 
+		if (ret > 0) {
+			return OK;
 		}
+	}
 
+	delete g_mk;
+	g_mk = nullptr;
 
-		g_mk = new MK(1, device_path);
-
-		if (g_mk == nullptr) {
-			return -1;
+	// fallback to bus 1
+	g_mk = new MK(1, device_path);
 
-		} else {
-			ret = g_mk->mk_check_for_blctrl(8, false, true);
-			delete g_mk;
-			g_mk = nullptr;
+	if (!g_mk)
+		return -ENOMEM;
 
-			if (ret > 0) {
-				return 1;
-			}
+	if (OK == g_mk->init(motors)) {
+		warnx("[mkblctrl] scanning i2c1...\n");
+		ret = g_mk->mk_check_for_blctrl(8, false, true);
 
+		if (ret > 0) {
+			return OK;
 		}
 	}
 
-	return -1;
-}
+	delete g_mk;
+	g_mk = nullptr;
 
+	return -ENXIO;
+}
 
 
 } // namespace
@@ -1472,10 +1224,8 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
 int
 mkblctrl_main(int argc, char *argv[])
 {
-	PortMode port_mode = PORT_FULL_PWM;
 	int pwm_update_rate_in_hz = UPDATE_RATE;
 	int motorcount = 8;
-	int bus = -1;
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
@@ -1489,18 +1239,6 @@ mkblctrl_main(int argc, char *argv[])
 	 */
 	for (int i = 1; i < argc; i++) {
 
-		/* look for the optional i2c bus parameter */
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				bus = atoi(argv[i + 1]);
-				newMode = true;
-
-			} else {
-				errx(1, "missing argument for i2c bus (-b)");
-				return 1;
-			}
-		}
-
 		/* look for the optional frame parameter */
 		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
 			if (argc > i + 1) {
@@ -1560,51 +1298,43 @@ mkblctrl_main(int argc, char *argv[])
 		fprintf(stderr, "mkblctrl: help:\n");
 		fprintf(stderr, "  [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
 		fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
-		fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
 		fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
 		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
 		fprintf(stderr, "\n");
 		fprintf(stderr, "Motortest:\n");
 		fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
 		fprintf(stderr, "mkblctrl -t\n");
-      fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+		fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
 		exit(1);
 	}
 
 
 	if (!motortest) {
-   	if (g_mk == nullptr) {
-      	if (bus == -1) {
-      		bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
-      	}
-
-      	if (bus != -1) {
-     			if (mk_start(bus, motorcount, devicepath) != OK) {
-     				errx(1, "failed to start the MK-BLCtrl driver");
-     			}
-      	} else {
-      		errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
-      	}
-
-      	/* parameter set ? */
-      	if (newMode) {
-      		/* switch parameter */
-      		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
-      	}
-
-      	exit(0);
-      } else {
-         errx(1, "MK-BLCtrl driver already running");
-      }
-
-   } else {
-      if (g_mk == nullptr) {
-         errx(1, "MK-BLCtrl driver not running. You have to start it first.");
-
-      } else {
-         g_mk->set_motor_test(motortest);
-         exit(0);
-
-      }
-   }
+		if (g_mk == nullptr) {
+			if (mk_start(motorcount, devicepath) != OK) {
+				errx(1, "failed to start the MK-BLCtrl driver");
+			}
+
+			/* parameter set ? */
+			if (newMode) {
+				/* switch parameter */
+				return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+			}
+
+			exit(0);
+
+		} else {
+			errx(1, "MK-BLCtrl driver already running");
+		}
+
+	} else {
+		if (g_mk == nullptr) {
+			errx(1, "MK-BLCtrl driver not running. You have to start it first.");
+
+		} else {
+			g_mk->set_motor_test(motortest);
+			exit(0);
+
+		}
+	}
 }
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index bbc595af4..bf80c9cff 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -443,7 +443,6 @@ int
 MPU6000::init()
 {
 	int ret;
-	int gyro_ret;
 
 	/* do SPI init (and probe) first */
 	ret = SPI::init();
@@ -488,16 +487,36 @@ MPU6000::init()
 		return ret;
 	}
 
-	/* fetch an initial set of measurements for advertisement */
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
 	measure();
 
-	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* advertise accel topic */
-		accel_report ar;
-		_accel_reports->get(&ar);
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
-	}		
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct accel_report arp;
+		_accel_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+
+		if (_accel_topic < 0)
+			debug("failed to create sensor_accel publication");
+
+	}
+
+	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct gyro_report grp;
+		_gyro_reports->get(&grp);
+
+		_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+
+		if (_gyro->_gyro_topic < 0)
+			debug("failed to create sensor_gyro publication");
+
+	}
 
 out:
 	return ret;
@@ -1307,10 +1326,13 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_topic != -1) {
+	if (_accel_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
-	if (_gyro->_gyro_topic != -1) {
+
+	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
 		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
@@ -1356,11 +1378,6 @@ MPU6000_gyro::init()
 	}
 
 	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
-	if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
-		gyro_report gr;
-		memset(&gr, 0, sizeof(gr));
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
-	}
 
 out:
 	return ret;
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 6326cf7fc..0ef056273 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -90,6 +90,7 @@ static const int ERROR = -1;
 /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
 #define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
 #define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+#define MS5611_BARO_DEVICE_PATH		"/dev/ms5611"
 
 class MS5611 : public device::CDev
 {
@@ -132,6 +133,8 @@ protected:
 
 	orb_advert_t		_baro_topic;
 
+	int			_class_instance;
+
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_measure_perf;
 	perf_counter_t		_comms_errors;
@@ -192,7 +195,7 @@ protected:
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
 MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
-	CDev("MS5611", BARO_DEVICE_PATH),
+	CDev("MS5611", MS5611_BARO_DEVICE_PATH),
 	_interface(interface),
 	_prom(prom_buf.s),
 	_measure_ticks(0),
@@ -204,6 +207,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
 	_SENS(0),
 	_msl_pressure(101325),
 	_baro_topic(-1),
+	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
 	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
@@ -218,6 +222,9 @@ MS5611::~MS5611()
 	/* make sure we are truly inactive */
 	stop_cycle();
 
+	if (_class_instance != -1)
+		unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
+
 	/* free any existing reports */
 	if (_reports != nullptr)
 		delete _reports;
@@ -251,18 +258,57 @@ MS5611::init()
 		goto out;
 	}
 
-	/* get a publish handle on the baro topic */
-	struct baro_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(BARO_DEVICE_PATH);
 
-	if (_baro_topic < 0) {
-		debug("failed to create sensor_baro object");
-		ret = -ENOSPC;
-		goto out;
-	}
+	struct baro_report brp;
+	/* do a first measurement cycle to populate reports with valid data */
+	_measure_phase = 0;
+	_reports->flush();
+
+	/* this do..while is goto without goto */
+	do {
+		/* do temperature first */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		_reports->get(&brp);
+
+		ret = OK;
+
+		if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+			_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
+
+			if (_baro_topic < 0)
+				debug("failed to create sensor_baro publication");
+		}
+
+	} while (0);
 
-	ret = OK;
 out:
 	return ret;
 }
@@ -670,7 +716,10 @@ MS5611::collect()
 		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+		if (_baro_topic > 0 && !(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+		}
 
 		if (_reports->force(&report)) {
 			perf_count(_buffer_overflows);
@@ -812,7 +861,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 	if (fd < 0) {
 		warnx("can't open baro device");
 		goto fail;
@@ -846,10 +895,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -905,7 +954,7 @@ test()
 void
 reset()
 {
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -944,10 +993,10 @@ calibrate(unsigned altitude)
 	float	pressure;
 	float	p1;
 
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
 
 	/* start the sensor polling at max */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))