From b23af6108772f8049ca94dfd8c648e1014917062 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:47:35 +0100
Subject: [PATCH 01/17] System disables all driver publications it can get hold
 of once entering HIL

---
 .../commander/state_machine_helper.cpp        | 31 +++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 490fc8fc6b..44e3aa787f 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,34 @@ 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) {
+
+						bool blocked = false;
+						int sensfd = ::open(direntry->d_name, 0);
+						::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						close(sensfd);
+
+						printf("Disabling %s\n: %s", direntry->d_name, (blocked) ? "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;

From c6e196edca7acae0b548a85c94d0c8f37df3c7aa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:52:19 +0100
Subject: [PATCH 02/17] Support disabling GPS output via IOCTL, general cleanup
 of author and copyright code style

---
 src/drivers/gps/gps.cpp        | 21 +++++++++++++--------
 src/drivers/gps/gps_helper.cpp |  7 ++++---
 src/drivers/gps/gps_helper.h   |  6 +++---
 src/drivers/gps/module.mk      |  2 +-
 src/drivers/gps/mtk.cpp        | 11 +++++++----
 src/drivers/gps/mtk.h          | 11 +++++++----
 src/drivers/gps/ubx.cpp        | 10 ++++++----
 src/drivers/gps/ubx.h          | 17 ++++++++++++-----
 8 files changed, 53 insertions(+), 32 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6b72d560fa..a736cbdf6b 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 2e2cbc8ddf..2360ff39bd 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 73d4b889cb..cfb9e0d43d 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 097db2abfc..82c67d40a3 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 56b702ea6c..456a9645bf 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 mkt.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 b5cfbf0a6d..9032e45b03 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 mkt.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 86291901cb..8a2afecb7e 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 76ef873a36..79a904f4a4 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_

From 9bf512cac82815e690774ddfc2fdeda29c22f4a0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:52:41 +0100
Subject: [PATCH 03/17] Framework to support disabling publications via IOCTL

---
 src/drivers/device/cdev.cpp   | 11 ++++++++++-
 src/drivers/device/device.cpp |  2 +-
 src/drivers/device/device.h   |  4 +++-
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 7954ce5ab5..65a9705f55 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,13 @@ 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);
+		break;
+	case DEVIOCGPUBBLOCK:
+		return _pub_blocked;
+		break;
 	}
 
 	return -ENOTTY;
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index c3ee77b1ca..6834551493 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 0235f62844..d99f229220 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;
 

From d72c82f66bf6dac8e6d10bf1024641908d3b854c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:53:15 +0100
Subject: [PATCH 04/17] Airspeed does not publish if disabled

---
 src/drivers/airspeed/airspeed.cpp           | 12 +++++++-----
 src/drivers/airspeed/airspeed.h             |  4 +++-
 src/drivers/ets_airspeed/ets_airspeed.cpp   | 15 +++++++++++++--
 src/drivers/meas_airspeed/meas_airspeed.cpp | 15 +++++++++++++--
 4 files changed, 36 insertions(+), 10 deletions(-)

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5e45cc936c..215d3792e8 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,10 +130,8 @@ 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);
 
 	if (_airspeed_pub < 0)
 		warnx("failed to create airspeed sensor object. Did you start uOrb?");
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index c341aa2c6d..c27b1bcd80 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/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index de371bf324..cdc70ac37c 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
@@ -185,7 +185,18 @@ ETSAirspeed::collect()
 	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 (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_airspeed_pub > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+		} else {
+			_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report);
+
+			if (_airspeed_pub < 0)
+				debug("failed to create differential_pressure publication");
+		}
+	}
 
 	new_report(report);
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index a95c4576b0..fee13f139c 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
@@ -217,7 +217,18 @@ MEASAirspeed::collect()
 	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 (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_airspeed_pub > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+		} else {
+			_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report);
+
+			if (_airspeed_pub < 0)
+				debug("failed to create differential_pressure publication");
+		}
+	}
 
 	new_report(report);
 

From c7e2841baa98bb985b402c89bef85e56f765ec11 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:53:31 +0100
Subject: [PATCH 05/17] BMA180 does not publish if disabled

---
 src/drivers/bma180/bma180.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 1590cc182b..df4e8f9989 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
@@ -723,7 +723,8 @@ BMA180::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
+	if !(_pub_blocked)
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);

From 28a3dc726f8e4f3696736798a1d92bf7d9de6100 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:53:56 +0100
Subject: [PATCH 06/17] Support for publication blocking: HMC5883

---
 src/drivers/hmc5883/hmc5883.cpp | 37 +++++++++++++++++----------------
 1 file changed, 19 insertions(+), 18 deletions(-)

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index d3b99ae66e..49b72cf794 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 */
@@ -885,9 +875,18 @@ HMC5883::collect()
 	}
 #endif
 
-	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 */
@@ -1134,10 +1133,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);
+			}
 		}
 	}
 

From 3c7766db6c58dea67b66263d2a7c01bb57177db5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:54:10 +0100
Subject: [PATCH 07/17] Support for publication blocking: L3GD20(H)

---
 src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 670e51b979..e885b1bf9e 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,12 +379,6 @@ 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();
 
@@ -894,8 +888,19 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_gyro_topic > 0)
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_gyro_topic > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		} else {
+			_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &report);
+
+			if (_gyro_topic < 0)
+				debug("failed to create sensor_gyro publication");
+		}
+
+	}
 
 	_read++;
 

From a34a14ce862c270192283514d8a1914fbe43bd48 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:54:25 +0100
Subject: [PATCH 08/17] Support for publication blocking: LSM303D, cleaned up
 device start

---
 src/drivers/lsm303d/lsm303d.cpp | 64 +++++++++++++++++----------------
 1 file changed, 34 insertions(+), 30 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 969b5e25f9..3bd7a66f6b 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,14 +597,7 @@ 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);
-	}
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
 out:
 	return ret;
@@ -727,7 +720,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,9 +1510,18 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_topic != -1) {
-		/* publish for subscribers */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_accel_topic > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		} else {
+			_accel_topic = orb_advertise(ORB_ID(sensor_accel), &accel_report);
+
+			if (_accel_topic < 0)
+				debug("failed to create sensor_accel publication");
+		}
+
 	}
 
 	_accel_read++;
@@ -1591,9 +1593,18 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_topic != -1) {
-		/* publish for subscribers */
-		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_mag->_mag_topic > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+		} else {
+			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mag_report);
+
+			if (_mag->_mag_topic < 0)
+				debug("failed to create sensor_mag publication");
+		}
+
 	}
 
 	_mag_read++;
@@ -1707,13 +1718,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;

From 7af62bbe9e4baaa846a37176d6942e1893e42715 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:54:38 +0100
Subject: [PATCH 09/17] Support for publication blocking: MPU6000, cleaned up
 device start

---
 src/drivers/mpu6000/mpu6000.cpp | 46 +++++++++++++++++++--------------
 1 file changed, 26 insertions(+), 20 deletions(-)

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index bbc595af46..02fe6df4a1 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,7 @@ MPU6000::init()
 		return ret;
 	}
 
-	/* fetch an initial set of measurements for advertisement */
-	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);
-	}		
 
 out:
 	return ret;
@@ -1307,11 +1297,32 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_topic != -1) {
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_accel_topic > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+		} else {
+			_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb);
+
+			if (_accel_topic < 0)
+				debug("failed to create sensor_accel publication");
+		}
+
 	}
-	if (_gyro->_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
+
+	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+		if (_gyro->_gyro_topic > 0) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
+		} else {
+			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb);
+
+			if (_gyro->_gyro_topic < 0)
+				debug("failed to create sensor_gyro publication");
+		}
+
 	}
 
 	/* stop measuring */
@@ -1356,11 +1367,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;

From e6a67b1deb0e7d5b315d3c570ff010de4d54b65f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:54:55 +0100
Subject: [PATCH 10/17] Support for publication blocking: MS5611, cleaned up
 device start

---
 src/drivers/ms5611/ms5611.cpp | 33 +++++++++++++++++++++------------
 1 file changed, 21 insertions(+), 12 deletions(-)

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 6326cf7fcb..0893315667 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
@@ -132,6 +132,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;
@@ -204,6 +206,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 +221,9 @@ MS5611::~MS5611()
 	/* make sure we are truly inactive */
 	stop_cycle();
 
+	if (_class_instance != -1)
+		unregister_class_devname(BARO_DEVICE_PATH, _class_instance);
+
 	/* free any existing reports */
 	if (_reports != nullptr)
 		delete _reports;
@@ -251,16 +257,8 @@ 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);
-
-	if (_baro_topic < 0) {
-		debug("failed to create sensor_baro object");
-		ret = -ENOSPC;
-		goto out;
-	}
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(BARO_DEVICE_PATH);
 
 	ret = OK;
 out:
@@ -670,7 +668,18 @@ 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 (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+
+			if (_baro_topic > 0) {
+				/* publish it */
+				orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+			} else {
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro), &report);
+
+				if (_baro_topic < 0)
+					debug("failed to create sensor_baro publication");
+			}
+		}
 
 		if (_reports->force(&report)) {
 			perf_count(_buffer_overflows);

From dbbe4ab1d587376bc2d530b3e80a001aa30b69c7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jan 2014 11:55:16 +0100
Subject: [PATCH 11/17] Header for publication disable

---
 src/drivers/drv_device.h | 62 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 62 insertions(+)
 create mode 100644 src/drivers/drv_device.h

diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h
new file mode 100644
index 0000000000..b310beb746
--- /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 */

From 47e0c926a6932b7a60ce85a5c748ce5bfcc102e7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jan 2014 14:02:16 +0100
Subject: [PATCH 12/17] Fixed two typos identified by kroimon

---
 src/drivers/gps/mtk.cpp | 2 +-
 src/drivers/gps/mtk.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 456a9645bf..c90ecbe281 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mkt.cpp
+ * @file mtk.cpp
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
index 9032e45b03..a2d5e27bbf 100644
--- a/src/drivers/gps/mtk.h
+++ b/src/drivers/gps/mtk.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mkt.cpp
+ * @file mtk.cpp
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>

From c4dc310ebda8f79ec13c68745408444661b32fe1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jan 2014 14:03:57 +0100
Subject: [PATCH 13/17] Fixed bogus return value of publication blocking
 disable

---
 src/modules/commander/state_machine_helper.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 44e3aa787f..7ae61d9ef2 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -505,12 +505,11 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 					while ((direntry = readdir(d)) != NULL) {
 
-						bool blocked = false;
 						int sensfd = ::open(direntry->d_name, 0);
-						::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
 						close(sensfd);
 
-						printf("Disabling %s\n: %s", direntry->d_name, (blocked) ? "OK" : "FAIL");
+						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
 					}
 
 					closedir(d);

From d19971065140bdfbbe5972f2a394597504abef9e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jan 2014 15:40:46 +0100
Subject: [PATCH 14/17] Fixed up init sequence of all sensors - we can publish
 in interrupt context, but not advertise! All advertisements now contain valid
 data

---
 src/drivers/airspeed/airspeed.cpp           | 16 ++++-
 src/drivers/bma180/bma180.cpp               | 22 ++++--
 src/drivers/ets_airspeed/ets_airspeed.cpp   | 15 +---
 src/drivers/l3gd20/l3gd20.cpp               | 30 ++++----
 src/drivers/lsm303d/lsm303d.cpp             | 61 +++++++++-------
 src/drivers/meas_airspeed/meas_airspeed.cpp | 15 +---
 src/drivers/mpu6000/mpu6000.cpp             | 59 ++++++++-------
 src/drivers/ms5611/ms5611.cpp               | 80 +++++++++++++++------
 8 files changed, 186 insertions(+), 112 deletions(-)

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 215d3792e8..71c0b70f01 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -133,8 +133,20 @@ Airspeed::init()
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
 
-	if (_airspeed_pub < 0)
-		warnx("failed to create airspeed sensor object. Did you start uOrb?");
+	/* 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);
+
+		/* 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/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index df4e8f9989..e43a348052 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -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,7 @@ BMA180::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if !(_pub_blocked)
+	if (_accel_topic > 0 && !(_pub_blocked))
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index cdc70ac37c..8bbef5cfa0 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -184,18 +184,9 @@ ETSAirspeed::collect()
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* announce the airspeed if needed, just publish else */
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_airspeed_pub > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
-		} else {
-			_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report);
-
-			if (_airspeed_pub < 0)
-				debug("failed to create differential_pressure publication");
-		}
+	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/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e885b1bf9e..90c3db9ae9 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -382,6 +382,21 @@ L3GD20::init()
 
 	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;
@@ -888,18 +903,9 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_gyro_topic > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
-		} else {
-			_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &report);
-
-			if (_gyro_topic < 0)
-				debug("failed to create sensor_gyro publication");
-		}
-
+	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 3bd7a66f6b..4dee7649bc 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -597,8 +597,39 @@ LSM303D::init()
 		goto out;
 	}
 
+	/* 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:
 	return ret;
 }
@@ -1510,18 +1541,9 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_accel_topic > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
-		} else {
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel), &accel_report);
-
-			if (_accel_topic < 0)
-				debug("failed to create sensor_accel publication");
-		}
-
+	if (_accel_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1593,18 +1615,9 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_mag->_mag_topic > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
-		} else {
-			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mag_report);
-
-			if (_mag->_mag_topic < 0)
-				debug("failed to create sensor_mag publication");
-		}
-
+	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index fee13f139c..51a059e394 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -216,18 +216,9 @@ MEASAirspeed::collect()
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* announce the airspeed if needed, just publish else */
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_airspeed_pub > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
-		} else {
-			_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report);
-
-			if (_airspeed_pub < 0)
-				debug("failed to create differential_pressure publication");
-		}
+	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 02fe6df4a1..bf80c9cff2 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -489,6 +489,35 @@ MPU6000::init()
 
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
+	measure();
+
+	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");
+
+	}
+
+	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;
 }
@@ -1297,32 +1326,14 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_accel_topic > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
-		} else {
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb);
-
-			if (_accel_topic < 0)
-				debug("failed to create sensor_accel publication");
-		}
-
+	if (_accel_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
 
-	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_gyro->_gyro_topic > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
-		} else {
-			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb);
-
-			if (_gyro->_gyro_topic < 0)
-				debug("failed to create sensor_gyro publication");
-		}
-
+	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 0893315667..0ef056273f 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -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
 {
@@ -194,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),
@@ -222,7 +223,7 @@ MS5611::~MS5611()
 	stop_cycle();
 
 	if (_class_instance != -1)
-		unregister_class_devname(BARO_DEVICE_PATH, _class_instance);
+		unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
 
 	/* free any existing reports */
 	if (_reports != nullptr)
@@ -260,7 +261,54 @@ MS5611::init()
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(BARO_DEVICE_PATH);
 
-	ret = OK;
+	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);
+
 out:
 	return ret;
 }
@@ -668,17 +716,9 @@ MS5611::collect()
 		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-			if (_baro_topic > 0) {
-				/* publish it */
-				orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-			} else {
-				_baro_topic = orb_advertise(ORB_ID(sensor_baro), &report);
-
-				if (_baro_topic < 0)
-					debug("failed to create sensor_baro publication");
-			}
+		if (_baro_topic > 0 && !(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 		}
 
 		if (_reports->force(&report)) {
@@ -821,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;
@@ -855,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));
@@ -914,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 ");
@@ -953,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))

From 58d78e57b7b7de57a347ad94e70d2d19bb7230d0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jan 2014 15:41:12 +0100
Subject: [PATCH 15/17] Build the sensors as part of the test binary

---
 makefiles/config_px4fmu-v1_test.mk | 3 +++
 makefiles/config_px4fmu-v2_test.mk | 9 +++++++++
 2 files changed, 12 insertions(+)

diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk
index 41e8b95ff5..300afa3d5b 100644
--- a/makefiles/config_px4fmu-v1_test.mk
+++ b/makefiles/config_px4fmu-v1_test.mk
@@ -29,6 +29,9 @@ MODULES		+= systemcmds/nshterm
 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/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index f54a4d8257..ea52213df0 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
@@ -30,6 +36,9 @@ MODULES		+= systemcmds/mtd
 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.

From bb8956c84e83c22d143a99d4ca37491574200438 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jan 2014 16:04:12 +0100
Subject: [PATCH 16/17] Fixed return value

---
 src/drivers/device/cdev.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 65a9705f55..b157b3f181 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -261,6 +261,7 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 	case DEVIOCSPUBBLOCK:
 		_pub_blocked = (arg != 0);
+		return OK;
 		break;
 	case DEVIOCGPUBBLOCK:
 		return _pub_blocked;

From ac326beaaae7b38d65ad6d7d13f00dfeaa6ae520 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jan 2014 16:04:26 +0100
Subject: [PATCH 17/17] Improved config tool to also do device IOCTLs

---
 src/systemcmds/config/config.c | 56 +++++++++++++++++++++++++++++-----
 1 file changed, 49 insertions(+), 7 deletions(-)

diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 80689f20c8..476015f3e3 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,20 +74,60 @@ 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);
 		}
 	}
 	
 	errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
 }
 
+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[])
 {
@@ -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) {