Merge branch 'master' into paul_estimator

41 files changed, 529 insertions, 366 deletions

diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
new file mode 100644
index 000000000..2dc83a517
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -0,0 +1,39 @@
+#!nsh
+#
+# UNTESTED UNTESTED!
+#
+# Generic 10” Hexa coaxial geometry
+#
+# Lorenz Meier <lm@inf.ethz.ch>
+#
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+	param set MC_ATT_P 7.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_D 0.0
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_D 0.004
+	param set MC_YAWPOS_P 2.0
+	param set MC_YAWPOS_I 0.0
+	param set MC_YAWPOS_D 0.0
+	param set MC_YAWRATE_P 0.3
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.005
+	
+	# TODO add default MPC parameters
+fi
+
+set VEHICLE_TYPE mc
+set MIXER hexa_cox
+
+set PWM_OUTPUTS 12345678
+set PWM_RATE 400
+# DJI ESC range
+set PWM_DISARMED 900
+set PWM_MIN 1200
+set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox
index 5f3cec4e0..a55fc5a30 100644
--- a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -2,7 +2,7 @@
 #
 # Generic 10” Octo coaxial geometry
 #
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lm@inf.ethz.ch>
 #
 
 if [ $DO_AUTOCONFIG == yes ]
@@ -29,7 +29,7 @@ fi
 set VEHICLE_TYPE mc
 set MIXER FMU_octo_cox
 
-set PWM_OUTPUTS 1234
+set PWM_OUTPUTS 12345678
 set PWM_RATE 400
 # DJI ESC range
 set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
index ddec8f36e..447796709 100644
--- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
@@ -29,7 +29,7 @@ fi
 set VEHICLE_TYPE mc
 set MIXER FMU_hexa_x
 
-set PWM_OUTPUTS 1234
+set PWM_OUTPUTS 12345678
 set PWM_RATE 400
 # DJI ESC range
 set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
index 106e0fb54..c4e9560d1 100644
--- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
+++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
@@ -29,7 +29,7 @@ fi
 set VEHICLE_TYPE mc
 set MIXER FMU_hexa_+
 
-set PWM_OUTPUTS 1234
+set PWM_OUTPUTS 12345678
 set PWM_RATE 400
 # DJI ESC range
 set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
index f0eea339b..ea56195d4 100644
--- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
+++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
@@ -29,7 +29,7 @@ fi
 set VEHICLE_TYPE mc
 set MIXER FMU_octo_x
 
-set PWM_OUTPUTS 1234
+set PWM_OUTPUTS 12345678
 set PWM_RATE 400
 # DJI ESC range
 set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
index 992a7aeba..f7693875c 100644
--- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
+++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
@@ -29,7 +29,7 @@ fi
 set VEHICLE_TYPE mc
 set MIXER FMU_octo_+
 
-set PWM_OUTPUTS 1234
+set PWM_OUTPUTS 12345678
 set PWM_RATE 400
 # DJI ESC range
 set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 00baed646..38b1cb57e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -186,10 +186,19 @@ then
 fi
 
 #
+# Hexa Coaxial
+#
+
+if param compare SYS_AUTOSTART 11001
+then
+	sh /etc/init.d/11001_hexa_cox
+fi
+
+#
 # Octo Coaxial
 #
 
 if param compare SYS_AUTOSTART 12001
 then
-	sh /etc/init.d/12001_octo_cox_pwm
+	sh /etc/init.d/12001_octo_cox
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 6f4e1f3b5..50ac9759a 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -467,19 +467,23 @@ then
 			# Use mixer to detect vehicle type
 			if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ]
 			then
-				param set MAV_TYPE 13
+				set MAV_TYPE 13
+			fi
+			if [ $MIXER == hexa_cox ]
+			then
+				set MAV_TYPE 13
 			fi
 			if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
 			then
-				param set MAV_TYPE 14
+				set MAV_TYPE 14
 			fi
 			if [ $MIXER == FMU_octo_cox ]
 			then
-				param set MAV_TYPE 14
+				set MAV_TYPE 14
 			fi
 		fi
 
-	    param set MAV_TYPE $MAV_TYPE
+		param set MAV_TYPE $MAV_TYPE
 		
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
index 610868354..80e3bac09 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
@@ -23,13 +23,13 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0  -3000  -5000      0 -10000  10000
-S: 0 1  -5000  -5000      0 -10000  10000
+S: 0 0  -8000  -8000      0 -10000  10000
+S: 0 1   6000   6000      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0  -5000  -3000      0 -10000  10000
-S: 0 1   5000   5000      0 -10000  10000
+S: 0 0  -8000  -8000      0 -10000  10000
+S: 0 1  -6000  -6000      0 -10000  10000
 
 Output 2
 --------
@@ -48,7 +48,7 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
-Gimbal / payload mixer for last four channels
+Gimbal / flaps / payload mixer for last four channels
 -----------------------------------------------------
 
 M: 1
@@ -66,4 +66,3 @@ S: 0 6  10000  10000      0 -10000  10000
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 7  10000  10000      0 -10000  10000
-
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
index f8f9f0e4d..e608b459f 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
@@ -1,18 +1,3 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a hexacopter in the + configuration. All controls
-are mixed 100%.
+# Hexa +
 
 R: 6+ 10000 10000 10000 0
-
-Gimbal / payload mixer for last two channels
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
index 26b40b9e9..16e6e22f9 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
@@ -1,18 +1,3 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a hexacopter in the X configuration.  All controls
-are mixed 100%.
+# Hexa X
 
 R: 6x 10000 10000 10000 0
-
-Gimbal / payload mixer for last two channels
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
index 51cebb785..f7845450d 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
@@ -1,7 +1,3 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a Coaxial Octocopter in the X configuration.  All controls 
-are mixed 100%.
+# Octo coaxial
 
 R: 8c 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
index edc71f013..c9a348aa4 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
@@ -1,7 +1,3 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a octocopter in the X configuration.  All controls
-are mixed 100%.
+# Octo X
 
 R: 8x 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README
deleted file mode 100644
index 6649c53c2..000000000
--- a/ROMFS/px4fmu_common/mixers/README
+++ /dev/null
@@ -1,154 +0,0 @@
-PX4 mixer definitions
-=====================
-
-Files in this directory implement example mixers that can be used as a basis
-for customisation, or for general testing purposes.
-
-Mixer basics
-------------
-
-Mixers combine control values from various sources (control tasks, user inputs,
-etc.) and produce output values suitable for controlling actuators; servos,
-motors, switches and so on.
-
-An actuator derives its value from the combination of one or more control
-values. Each of the control values is scaled according to the actuator's
-configuration and then combined to produce the actuator value, which may then be
-further scaled to suit the specific output type.
-
-Internally, all scaling is performed using floating point values. Inputs and
-outputs are clamped to the range -1.0 to 1.0.
-
-control    control   control
-   |          |         |
-   v          v         v
- scale      scale     scale
-   |          |         |
-   |          v         |
-   +-------> mix <------+
-              |
-            scale
-              |
-              v
-             out
-
-Scaling
--------
-
-Basic scalers provide linear scaling of the input to the output.
-
-Each scaler allows the input value to be scaled independently for inputs
-greater/less than zero. An offset can be applied to the output, and lower and
-upper boundary constraints can be applied. Negative scaling factors cause the
-output to be inverted (negative input produces positive output).
-
-Scaler pseudocode:
-
-if (input < 0)
-    output = (input * NEGATIVE_SCALE) + OFFSET
-else
-    output = (input * POSITIVE_SCALE) + OFFSET
-
-if (output < LOWER_LIMIT)
-    output = LOWER_LIMIT
-if (output > UPPER_LIMIT)
-    output = UPPER_LIMIT
-
-Syntax
-------
-
-Mixer definitions are text files; lines beginning with a single capital letter
-followed by a colon are significant. All other lines are ignored, meaning that
-explanatory text can be freely mixed with the definitions.
-
-Each file may define more than one mixer; the allocation of mixers to actuators
-is specific to the device reading the mixer definition, and the number of
-actuator outputs generated by a mixer is specific to the mixer.
-
-A mixer begins with a line of the form
-
-	<tag>: <mixer arguments>
-
-The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a 
-multirotor mixer, etc.
-
-Null Mixer
-..........
-
-A null mixer consumes no controls and generates a single actuator output whose
-value is always zero.  Typically a null mixer is used as a placeholder in a
-collection of mixers in order to achieve a specific pattern of actuator outputs.
-
-The null mixer definition has the form:
-
-  Z:
-
-Simple Mixer
-............
-
-A simple mixer combines zero or more control inputs into a single actuator
-output.  Inputs are scaled, and the mixing function sums the result before
-applying an output scaler.
-
-A simple mixer definition begins with:
-
-  M: <control count>
-  O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
-
-If <control count> is zero, the sum is effectively zero and the mixer will
-output a fixed value that is <offset> constrained by <lower limit> and <upper
-limit>.
-
-The second line defines the output scaler with scaler parameters as discussed
-above. Whilst the calculations are performed as floating-point operations, the
-values stored in the definition file are scaled by a factor of 10000; i.e. an
-offset of -0.5 is encoded as -5000.
-
-The definition continues with <control count> entries describing the control
-inputs and their scaling, in the form:
-
-  S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
-
-The <group> value identifies the control group from which the scaler will read,
-and the <index> value an offset within that group.  These values are specific to
-the device reading the mixer definition.
-
-When used to mix vehicle controls, mixer group zero is the vehicle attitude
-control group, and index values zero through three are normally roll, pitch,
-yaw and thrust respectively.
-
-The remaining fields on the line configure the control scaler with parameters as
-discussed above. Whilst the calculations are performed as floating-point
-operations, the values stored in the definition file are scaled by a factor of
-10000; i.e. an offset of -0.5 is encoded as -5000.
-
-Multirotor Mixer
-................
-
-The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
-into a set of actuator outputs intended to drive motor speed controllers.
-
-The mixer definition is a single line of the form:
-
-R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
-
-The supported geometries include:
-
-  4x - quadrotor in X configuration
-  4+ - quadrotor in + configuration
-  6x - hexcopter in X configuration
-  6+ - hexcopter in + configuration
-  8x - octocopter in X configuration
-  8+ - octocopter in + configuration
-  
-Each of the roll, pitch and yaw scale values determine scaling of the roll,
-pitch and yaw controls relative to the thrust control.  Whilst the calculations
-are performed as floating-point operations, the values stored in the definition
-file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
-
-Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
-thrust input ranges from 0.0 to 1.0.  Output for each actuator is in the 
-range -1.0 to 1.0.
-
-In the case where an actuator saturates, all actuator values are rescaled so that 
-the saturating actuator is limited to 1.0.
diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/hexa_cox.mix
new file mode 100644
index 000000000..497786feb
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/hexa_cox.mix
@@ -0,0 +1,3 @@
+# Hexa coaxial
+
+R: 6c 10000 10000 10000 0
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index d5a50cfae..4f997e96b 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -18,6 +18,12 @@ MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= drivers/px4io
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
@@ -31,6 +37,9 @@ MODULES		+= systemcmds/hw_ver
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
 
 #
 # Transitional support - add commands from the NuttX export archive.
diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
index e56b14ba4..850043ddf 100755
--- a/nuttx-configs/px4fmu-v2/include/board.h
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -209,8 +209,8 @@
 
 #define GPIO_USART3_RX	GPIO_USART3_RX_3
 #define GPIO_USART3_TX	GPIO_USART3_TX_3
-#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
-#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+#define GPIO_USART3_RTS	GPIO_USART3_RTS_2
+#define GPIO_USART3_CTS	GPIO_USART3_CTS_2
 
 #define GPIO_UART4_RX	GPIO_UART4_RX_1
 #define GPIO_UART4_TX	GPIO_UART4_TX_1
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/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))
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 490fc8fc6..7ae61d9ef 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -42,6 +42,8 @@
 #include <unistd.h>
 #include <stdint.h>
 #include <stdbool.h>
+#include <dirent.h>
+#include <fcntl.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -51,6 +53,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
@@ -491,6 +494,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 				current_control_mode->flag_system_hil_enabled = true;
 				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
 				valid_transition = true;
+
+				// Disable publication of all attached sensors
+
+				/* list directory */
+				DIR		*d;
+				struct dirent	*direntry;
+				d = opendir("/dev");
+				if (d) {
+
+					while ((direntry = readdir(d)) != NULL) {
+
+						int sensfd = ::open(direntry->d_name, 0);
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						close(sensfd);
+
+						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+					}
+
+					closedir(d);
+
+					warnx("directory listing ok (FS mounted and readable)");
+
+				} else {
+					/* failed opening dir */
+					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
+					return 1;
+				}
 			}
 
 			break;
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 80689f20c..476015f3e 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -56,6 +56,7 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
 
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
@@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]);
 static void	do_gyro(int argc, char *argv[]);
 static void	do_accel(int argc, char *argv[]);
 static void	do_mag(int argc, char *argv[]);
+static void	do_device(int argc, char *argv[]);
 
 int
 config_main(int argc, char *argv[])
@@ -72,14 +74,12 @@ config_main(int argc, char *argv[])
 	if (argc >= 2) {
 		if (!strcmp(argv[1], "gyro")) {
 			do_gyro(argc - 2, argv + 2);
-		}
-
-		if (!strcmp(argv[1], "accel")) {
+		} else if (!strcmp(argv[1], "accel")) {
 			do_accel(argc - 2, argv + 2);
-		}
-
-		if (!strcmp(argv[1], "mag")) {
+		} else if (!strcmp(argv[1], "mag")) {
 			do_mag(argc - 2, argv + 2);
+		} else {
+			do_device(argc - 1, argv + 1);
 		}
 	}
 	
@@ -87,6 +87,48 @@ config_main(int argc, char *argv[])
 }
 
 static void
+do_device(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "no device path provided and command provided.");
+	}
+
+	int	fd;
+	int	ret;
+
+	fd = open(argv[0], 0);
+
+	if (fd < 0) {
+		warn("%s", argv[0]);
+		errx(1, "FATAL: no device found");
+
+	} else {
+
+		if (argc == 2 && !strcmp(argv[1], "block")) {
+
+			/* disable the device publications */
+			ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
+
+			if (ret)
+				errx(ret,"uORB publications could not be blocked");
+
+		} else if (argc == 2 && !strcmp(argv[1], "unblock")) {
+
+			/* enable the device publications */
+			ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
+
+			if (ret)
+				errx(ret,"uORB publications could not be unblocked");
+
+		} else {
+			errx("no valid command: %s", argv[1]);
+		}
+	}
+
+	exit(0);
+}
+
+static void
 do_gyro(int argc, char *argv[])
 {
 	int	fd;
@@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[])
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+		} else if (argc == 1 && !strcmp(argv[0], "check")) {
 			ret = ioctl(fd, GYROIOCSELFTEST, 0);
 
 			if (ret) {