4 files changed, 53 insertions, 22 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 524151c90..109c50287 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
 	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
 	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
+	_last_published_sensor_ok(true), /* initialize differently to force publication */
 	_measure_ticks(0),
 	_collect_phase(false),
 	_diff_pres_offset(0.0f),
 	_airspeed_pub(-1),
+	_subsys_pub(-1),
 	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@@ -344,22 +346,6 @@ Airspeed::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		SUBSYSTEM_TYPE_DIFFPRESSURE
-	};
-	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);
-	}
 }
 
 void
@@ -369,11 +355,34 @@ Airspeed::stop()
 }
 
 void
+Airspeed::update_status()
+{
+	if (_sensor_ok != _last_published_sensor_ok) {
+		/* notify about state change */
+		struct subsystem_info_s info = {
+			true,
+			true,
+			_sensor_ok,
+			SUBSYSTEM_TYPE_DIFFPRESSURE
+		};
+
+		if (_subsys_pub > 0) {
+			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
+		} else {
+			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		}
+
+		_last_published_sensor_ok = _sensor_ok;
+	}
+}
+
+void
 Airspeed::cycle_trampoline(void *arg)
 {
 	Airspeed *dev = (Airspeed *)arg;
 
 	dev->cycle();
+	dev->update_status();
 }
 
 void
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 186602eda..0b8e949c9 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -118,14 +118,21 @@ protected:
 	virtual int	measure() = 0;
 	virtual int	collect() = 0;
 
+	/**
+	 * Update the subsystem status
+	 */
+	void update_status();
+
 	work_s			_work;
 	float			_max_differential_pressure_pa;
 	bool			_sensor_ok;
+	bool			_last_published_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
 	float			_diff_pres_offset;
 
 	orb_advert_t		_airspeed_pub;
+	orb_advert_t		_subsys_pub;
 
 	int			_class_instance;
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index d873a1132..146a06e7c 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -207,14 +207,18 @@ ETSAirspeed::collect()
 void
 ETSAirspeed::cycle()
 {
+	int ret;
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		ret = collect();
+		if (OK != ret) {
 			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
+			_sensor_ok = false;
 			return;
 		}
 
@@ -238,8 +242,12 @@ ETSAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
-		log("measure error");
+	ret = measure();
+	if (OK != ret) {
+		debug("measure error");
+	}
+
+	_sensor_ok = (ret == OK);
 
 	/* next phase is collection */
 	_collect_phase = true;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 1ad383ee0..c0f3c28e0 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -288,13 +288,17 @@ MEASAirspeed::collect()
 void
 MEASAirspeed::cycle()
 {
+	int ret;
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		ret = collect();
+		if (OK != ret) {
 			/* restart the measurement state machine */
 			start();
+			_sensor_ok = false;
 			return;
 		}
 
@@ -318,10 +322,13 @@ MEASAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure()) {
-		log("measure error");
+	ret = measure();
+	if (OK != ret) {
+		debug("measure error");
 	}
 
+	_sensor_ok = (ret == OK);
+
 	/* next phase is collection */
 	_collect_phase = true;