meas_airspeed: don't use stale/bad data in airspeed reading

also fixed handling of perf counters on error

1 files changed, 8 insertions, 1 deletions

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 666bd30e6..b1cb2b3d8 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -162,6 +162,8 @@ MEASAirspeed::collect()
 
 	if (ret < 0) {
 		log("error reading from sensor: %d", ret);
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
 		return ret;
 	}
 
@@ -169,9 +171,14 @@ MEASAirspeed::collect()
 
 	if (status == 2) {
 		log("err: stale data");
-
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
+		return ret;
 	} else if (status == 3) {
 		log("err: fault");
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
+		return ret;                
 	}
 
 	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);