From 194ea669121410d5674241b89892733b0f0b3ca8 Mon Sep 17 00:00:00 2001
From: liang <466175335@qq.com>
Date: Fri, 11 Oct 2019 23:19:33 -0700
Subject: [PATCH] AP_Airspeed: protect sample timestamp while get data

---
 libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp   | 17 ++++++++---------
 libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp |  8 ++++----
 libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp |  8 ++++----
 libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp  | 20 +++++++++-----------
 4 files changed, 25 insertions(+), 28 deletions(-)

diff --git a/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp b/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
index de35907ba8..aa3b7088f7 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
@@ -98,17 +98,16 @@ void AP_Airspeed_DLVR::timer()
 // return the current differential_pressure in Pascal
 bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
 
-    {
-        WITH_SEMAPHORE(sem);
-        if (press_count > 0) {
-            pressure = pressure_sum / press_count;
-            press_count = 0;
-            pressure_sum = 0;
-        }
+    if (press_count > 0) {
+        pressure = pressure_sum / press_count;
+        press_count = 0;
+        pressure_sum = 0;
     }
 
     _pressure = pressure;
@@ -118,12 +117,12 @@ bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)
 // return the current temperature in degrees C, if available
 bool AP_Airspeed_DLVR::get_temperature(float &_temperature)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
 
-    WITH_SEMAPHORE(sem);
-
     if (temp_count > 0) {
         temperature = temperature_sum / temp_count;
         temp_count = 0;
diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
index 9e9af06cc9..0405b246f8 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
@@ -223,12 +223,12 @@ void AP_Airspeed_MS4525::_timer()
 // return the current differential_pressure in Pascal
 bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
         return false;
     }
 
-    WITH_SEMAPHORE(sem);
-
     if (_press_count > 0) {
         _pressure = _press_sum / _press_count;
         _press_count = 0;
@@ -242,12 +242,12 @@ bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
 // return the current temperature in degrees C, if available
 bool AP_Airspeed_MS4525::get_temperature(float &temperature)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
         return false;
     }
 
-    WITH_SEMAPHORE(sem);
-
     if (_temp_count > 0) {
         _temperature = _temp_sum / _temp_count;
         _temp_count = 0;
diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
index e972899c76..9a979b45d4 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
@@ -264,12 +264,12 @@ void AP_Airspeed_MS5525::timer()
 // return the current differential_pressure in Pascal
 bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
 
-    WITH_SEMAPHORE(sem);
-
     if (press_count > 0) {
         pressure = pressure_sum / press_count;
         press_count = 0;
@@ -283,12 +283,12 @@ bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
 // return the current temperature in degrees C, if available
 bool AP_Airspeed_MS5525::get_temperature(float &_temperature)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
 
-    WITH_SEMAPHORE(sem);
-
     if (temp_count > 0) {
         temperature = temperature_sum / temp_count;
         temp_count = 0;
diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp
index 948f92a7ac..004f340681 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp
@@ -278,18 +278,16 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press)
 // return the current differential_pressure in Pascal
 bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure)
 {
-    uint32_t now = AP_HAL::millis();
-    if (now - _last_sample_time_ms > 100) {
+    WITH_SEMAPHORE(sem);
+
+    if (AP_HAL::millis() - _last_sample_time_ms > 100) {
         return false;
     }
 
-    {
-        WITH_SEMAPHORE(sem);
-        if (_press_count > 0) {
-            _press = _press_sum / _press_count;
-            _press_count = 0;
-            _press_sum = 0;
-        }
+    if (_press_count > 0) {
+        _press = _press_sum / _press_count;
+        _press_count = 0;
+        _press_sum = 0;
     }
 
     pressure = _correct_pressure(_press);
@@ -299,12 +297,12 @@ bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure)
 // return the current temperature in degrees C, if available
 bool AP_Airspeed_SDP3X::get_temperature(float &temperature)
 {
+    WITH_SEMAPHORE(sem);
+
     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
         return false;
     }
 
-    WITH_SEMAPHORE(sem);
-
     if (_temp_count > 0) {
         _temp = _temp_sum / _temp_count;
         _temp_count = 0;