Airspeed does not publish if disabled

1 files changed, 13 insertions, 2 deletions

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);