Airspeed does not publish if disabled

4 files changed, 36 insertions, 10 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5e45cc936..215d3792e 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 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/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index de371bf32..cdc70ac37 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 a95c4576b..fee13f139 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);