AP_Airspeed: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()

libraries/AP_Airspeed/AP_Airspeed_Backend.cpp

@@ -28,12 +28,10 @@ AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend, uint8_t _instan
     frontend(_frontend),
     instance(_instance)
 {
-    sem = hal.util->new_semaphore();
 }
 
 AP_Airspeed_Backend::~AP_Airspeed_Backend(void)
 {
-    delete sem;
 }
  
 

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -47,7 +47,7 @@ protected:
     }
     
     // semaphore for access to shared frontend data
-    AP_HAL::Semaphore *sem;
+    HAL_Semaphore sem;
 
     float get_airspeed_ratio(void) const {
         return frontend.get_airspeed_ratio(instance);

libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp

@@ -82,13 +82,13 @@ void AP_Airspeed_DLVR::timer()
     float press_h2o = 1.25f * 2.0f * DLVR_FSS * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);
     float temp = temp_raw * (200.0f / 2047.0f) - 50.0f;
 
-    sem->take_blocking();
+    WITH_SEMAPHORE(sem);
+
     pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;
     temperature_sum += temp;
     press_count++;
     temp_count++;
     last_sample_time_ms = AP_HAL::millis();
-    sem->give();
 }
 
 // return the current differential_pressure in Pascal
@@ -97,14 +97,16 @@ bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+
+    {
+        WITH_SEMAPHORE(sem);
         if (press_count > 0) {
             pressure = pressure_sum / press_count;
             press_count = 0;
             pressure_sum = 0;
         }
-        sem->give();
     }
+
     _pressure = pressure;
     return true;
 }
@@ -115,14 +117,15 @@ bool AP_Airspeed_DLVR::get_temperature(float &_temperature)
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (temp_count > 0) {
-            temperature = temperature_sum / temp_count;
-            temp_count = 0;
-            temperature_sum = 0;
-        }
-        sem->give();
+
+    WITH_SEMAPHORE(sem);
+
+    if (temp_count > 0) {
+        temperature = temperature_sum / temp_count;
+        temp_count = 0;
+        temperature_sum = 0;
     }
+
     _temperature = temperature;
     return true;
 }

libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp

@@ -51,9 +51,7 @@ bool AP_Airspeed_MS4525::init()
         if (!_dev) {
             continue;
         }
-        if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-            continue;
-        }
+        WITH_SEMAPHORE(_dev->get_semaphore());
 
         // lots of retries during probe
         _dev->set_retries(10);
@@ -62,8 +60,6 @@ bool AP_Airspeed_MS4525::init()
         hal.scheduler->delay(10);
         _collect();
 
-        _dev->get_semaphore()->give();
-
         found = (_last_sample_time_ms != 0);
         if (found) {
             printf("MS4525: Found sensor on bus %u address 0x%02x\n", addresses[i].bus, addresses[i].addr);
@@ -176,14 +172,13 @@ void AP_Airspeed_MS4525::_collect()
     _voltage_correction(press, temp);
     _voltage_correction(press2, temp2);
 
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _press_sum += press + press2;
-        _temp_sum += temp + temp2;
-        _press_count += 2;
-        _temp_count += 2;
-        sem->give();
-    }
-    
+    WITH_SEMAPHORE(sem);
+
+    _press_sum += press + press2;
+    _temp_sum += temp + temp2;
+    _press_count += 2;
+    _temp_count += 2;
+
     _last_sample_time_ms = AP_HAL::millis();
 }
 
@@ -230,14 +225,15 @@ bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (_press_count > 0) {
-            _pressure = _press_sum / _press_count;
-            _press_count = 0;
-            _press_sum = 0;
-        }
-        sem->give();
+
+    WITH_SEMAPHORE(sem);
+
+    if (_press_count > 0) {
+        _pressure = _press_sum / _press_count;
+        _press_count = 0;
+        _press_sum = 0;
     }
+
     pressure = _pressure;
     return true;
 }
@@ -248,14 +244,15 @@ bool AP_Airspeed_MS4525::get_temperature(float &temperature)
     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (_temp_count > 0) {
-            _temperature = _temp_sum / _temp_count;
-            _temp_count = 0;
-            _temp_sum = 0;
-        }
-        sem->give();
+
+    WITH_SEMAPHORE(sem);
+
+    if (_temp_count > 0) {
+        _temperature = _temp_sum / _temp_count;
+        _temp_count = 0;
+        _temp_sum = 0;
     }
+
     temperature = _temperature;
     return true;
 }

libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp

@@ -218,14 +218,13 @@ void AP_Airspeed_MS5525::calculate(void)
     }
 #endif
     
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        pressure_sum += P_Pa;
-        temperature_sum += Temp_C;
-        press_count++;
-        temp_count++;
-        last_sample_time_ms = AP_HAL::millis();
-        sem->give();
-    }
+    WITH_SEMAPHORE(sem);
+
+    pressure_sum += P_Pa;
+    temperature_sum += Temp_C;
+    press_count++;
+    temp_count++;
+    last_sample_time_ms = AP_HAL::millis();
 }
 
 // 80Hz timer
@@ -287,15 +286,16 @@ bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (press_count > 0) {
-            pressure = pressure_sum / press_count;
-            press_count = 0;
-            pressure_sum = 0;
-        }
-        sem->give();
+
+    WITH_SEMAPHORE(sem);
+
+    if (press_count > 0) {
+        pressure = pressure_sum / press_count;
+        press_count = 0;
+        pressure_sum = 0;
     }
     _pressure = pressure;
+
     return true;
 }
 
@@ -305,14 +305,15 @@ bool AP_Airspeed_MS5525::get_temperature(float &_temperature)
     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (temp_count > 0) {
-            temperature = temperature_sum / temp_count;
-            temp_count = 0;
-            temperature_sum = 0;
-        }
-        sem->give();
+
+    WITH_SEMAPHORE(sem);
+
+    if (temp_count > 0) {
+        temperature = temperature_sum / temp_count;
+        temp_count = 0;
+        temperature_sum = 0;
     }
+
     _temperature = temperature;
     return true;
 }

libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp

@@ -180,14 +180,13 @@ void AP_Airspeed_SDP3X::_timer()
     float diff_press_pa = float(P) / float(_scale);
     float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE;
 
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _press_sum += diff_press_pa;
-        _temp_sum += temperature;
-        _press_count++;
-        _temp_count++;
-        _last_sample_time_ms = now;
-        sem->give();
-    }
+    WITH_SEMAPHORE(sem);
+
+    _press_sum += diff_press_pa;
+    _temp_sum += temperature;
+    _press_count++;
+    _temp_count++;
+    _last_sample_time_ms = now;
 }
 
 /*
@@ -279,14 +278,16 @@ bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure)
     if (now - _last_sample_time_ms > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+
+    {
+        WITH_SEMAPHORE(sem);
         if (_press_count > 0) {
             _press = _press_sum / _press_count;
             _press_count = 0;
             _press_sum = 0;
         }
-        sem->give();
     }
+
     pressure = _correct_pressure(_press);
     return true;
 }
@@ -297,14 +298,15 @@ bool AP_Airspeed_SDP3X::get_temperature(float &temperature)
     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
         return false;
     }
-    if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if (_temp_count > 0) {
-            _temp = _temp_sum / _temp_count;
-            _temp_count = 0;
-            _temp_sum = 0;
-        }
-        sem->give();
+
+    WITH_SEMAPHORE(sem);
+
+    if (_temp_count > 0) {
+        _temp = _temp_sum / _temp_count;
+        _temp_count = 0;
+        _temp_sum = 0;
     }
+
     temperature = _temp;
     return true;
 }

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp

@@ -17,7 +17,7 @@ extern const AP_HAL::HAL& hal;
 UC_REGISTRY_BINDER(AirspeedCb, uavcan::equipment::air_data::RawAirData);
 
 AP_Airspeed_UAVCAN::DetectedModules AP_Airspeed_UAVCAN::_detected_modules[] = {0};
-AP_HAL::Semaphore* AP_Airspeed_UAVCAN::_sem_registry = nullptr;
+HAL_Semaphore AP_Airspeed_UAVCAN::_sem_registry;
 
 // constructor
 AP_Airspeed_UAVCAN::AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance) :
@@ -43,16 +43,12 @@ void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
 
 bool AP_Airspeed_UAVCAN::take_registry()
 {
-    if (_sem_registry == nullptr) {
-        _sem_registry = hal.util->new_semaphore();
-    }
-
-    return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+    return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
 }
 
 void AP_Airspeed_UAVCAN::give_registry()
 {
-    _sem_registry->give();
+    _sem_registry.give();
 }
 
 AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance)

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h

@@ -44,5 +44,5 @@ private:
         AP_Airspeed_UAVCAN *driver;
     } _detected_modules[AIRSPEED_MAX_SENSORS];
 
-    static AP_HAL::Semaphore *_sem_registry;
+    static HAL_Semaphore _sem_registry;
 };