AP_Baro: use WITH_SEMAPHORE()

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

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -66,9 +66,7 @@ bool AP_Baro_BMP085::_init()
     AP_HAL::Semaphore *sem = _dev->get_semaphore();
 
     // take i2c bus sempahore
-    if (!sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    WITH_SEMAPHORE(sem);
-        AP_HAL::panic("BMP085: unable to get semaphore");
-    }
 
     if (BMP085_EOC >= 0) {
         _eoc = hal.gpio->channel(BMP085_EOC);
@@ -79,7 +77,6 @@ bool AP_Baro_BMP085::_init()
     uint8_t id;
 
     if (!_dev->read_registers(0xD0, &id, 1)) {
-        sem->give();
         return false;
     }
 
@@ -106,7 +103,6 @@ bool AP_Baro_BMP085::_init()
         }
     }
     if (!prom_ok) {
-        sem->give();
         return false;
     }
 
@@ -141,8 +137,6 @@ bool AP_Baro_BMP085::_init()
 
     _instance = _frontend.register_sensor();
 
-    sem->give();
-
     _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, void));
     return true;
 }
@@ -203,18 +197,16 @@ void AP_Baro_BMP085::_timer(void)
  */
 void AP_Baro_BMP085::update(void)
 {
-    if (_sem.take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-        if (!_has_sample) {
-            _sem.give();
-            return;
-        }
-
-        float temperature = 0.1f * _temp;
-        float pressure = _pressure_filter.getf();
 
-        _copy_to_frontend(_instance, pressure, temperature);
+    if (!_has_sample) {
-        _sem.give();
+        return;
     }
+
+    float temperature = 0.1f * _temp;
+    float pressure = _pressure_filter.getf();
+
+    _copy_to_frontend(_instance, pressure, temperature);
 }
 
 // Send command to Read Pressure

libraries/AP_Baro/AP_Baro_BMP280.cpp

@@ -66,9 +66,10 @@ AP_Baro_Backend *AP_Baro_BMP280::probe(AP_Baro &baro,
 
 bool AP_Baro_BMP280::_init()
 {
-    if (!_dev | !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev) {
         return false;
     }
+    WITH_SEMAPHORE(_dev->get_semaphore());
 
     _has_sample = false;
 
@@ -78,7 +79,6 @@ bool AP_Baro_BMP280::_init()
     if (!_dev->read_registers(BMP280_REG_ID, &whoami, 1)  ||
         whoami != BMP280_ID) {
         // not a BMP280
-        _dev->get_semaphore()->give();
         return false;
     }
 
@@ -114,8 +114,6 @@ bool AP_Baro_BMP280::_init()
 
     _instance = _frontend.register_sensor();
 
-    _dev->get_semaphore()->give();
-
     // request 50Hz update
     _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP280::_timer, void));
 
@@ -140,16 +138,14 @@ void AP_Baro_BMP280::_timer(void)
 // transfer data to the frontend
 void AP_Baro_BMP280::update(void)
 {
-    if (_sem.take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-        if (!_has_sample) {
+
-            _sem.give();
+    if (!_has_sample) {
-            return;
+        return;
-        }
-
-        _copy_to_frontend(_instance, _pressure, _temperature);
-        _has_sample = false;
-        _sem.give();
     }
+
+    _copy_to_frontend(_instance, _pressure, _temperature);
+    _has_sample = false;
 }
 
 // calculate temperature

libraries/AP_Baro/AP_Baro_Backend.cpp

@@ -14,9 +14,7 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
     if (instance >= _frontend._num_sensors) {
         return;
     }
-    if (!_sem.take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-        return;
-    }
 
     // consider a sensor as healthy if it has had an update in the
     // last 0.5 seconds and values are non-zero and have changed within the last 2 seconds
@@ -25,8 +23,6 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
         (now - _frontend.sensors[instance].last_update_ms < BARO_TIMEOUT_MS) &&
         (now - _frontend.sensors[instance].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) &&
         !is_zero(_frontend.sensors[instance].pressure);
-
-    _sem.give();
 }
 
 void AP_Baro_Backend::backend_update(uint8_t instance)

libraries/AP_Baro/AP_Baro_Backend.h

@@ -31,7 +31,7 @@ protected:
     void _copy_to_frontend(uint8_t instance, float pressure, float temperature);
 
     // semaphore for access to shared frontend data
-    HAL_Semaphore _sem;    
+    HAL_Semaphore_Recursive _sem;
 
     virtual void update_healthy_flag(uint8_t instance);
 

libraries/AP_Baro/AP_Baro_DPS280.cpp

@@ -210,28 +210,24 @@ void AP_Baro_DPS280::timer(void)
         return;
     }
 
-    if (_sem.take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-        pressure_sum += pressure;
+
-        temperature_sum += temperature;
+    pressure_sum += pressure;
-        count++;
+    temperature_sum += temperature;
-        _sem.give();
+    count++;
-    }
 }
 
 // transfer data to the frontend
 void AP_Baro_DPS280::update(void)
 {
-    if (count != 0 && _sem.take_nonblocking()) {
+    if (count == 0) {
-        if (count == 0) {
+        return;
-            _sem.give();
-            return;
-        }
-
-        _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
-        pressure_sum = 0;
-        temperature_sum = 0;
-        count=0;
-
-        _sem.give();
     }
+
+    WITH_SEMAPHORE(_sem);
+
+    _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
+    pressure_sum = 0;
+    temperature_sum = 0;
+    count=0;
 }

libraries/AP_Baro/AP_Baro_FBM320.cpp

@@ -187,12 +187,12 @@ void AP_Baro_FBM320::timer(void)
     } else {
         int32_t pressure, temperature;
         calculate_PT(value_T, value, pressure, temperature);
-        if (pressure_ok(pressure) && _sem.take_nonblocking()) {
+        if (pressure_ok(pressure)) {
+            WITH_SEMAPHORE(_sem);
             pressure_sum += pressure;
             // sum and convert to degrees
             temperature_sum += temperature*0.01;
             count++;
-            _sem.give();
         }
     }
 
@@ -207,17 +207,13 @@ void AP_Baro_FBM320::timer(void)
 // transfer data to the frontend
 void AP_Baro_FBM320::update(void)
 {
-    if (count != 0 && _sem.take_nonblocking()) {
+    if (count == 0) {
-        if (count == 0) {
+        return;
-            _sem.give();
-            return;
-        }
-
-        _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
-        pressure_sum = 0;
-        temperature_sum = 0;
-        count=0;
-
-        _sem.give();
     }
+    WITH_SEMAPHORE(_sem);
+
+    _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
+    pressure_sum = 0;
+    temperature_sum = 0;
+    count=0;
 }

libraries/AP_Baro/AP_Baro_LPS2XH.cpp

@@ -205,16 +205,13 @@ void AP_Baro_LPS2XH::_timer(void)
 // transfer data to the frontend
 void AP_Baro_LPS2XH::update(void)
 {
-    if (_sem.take_nonblocking()) {
+    if (!_has_sample) {
-        if (!_has_sample) {
+        return;
-            _sem.give();
-            return;
-        }
-
-        _copy_to_frontend(_instance, _pressure, _temperature);
-        _has_sample = false;
-        _sem.give();
     }
+
+    WITH_SEMAPHORE(_sem);
+    _copy_to_frontend(_instance, _pressure, _temperature);
+    _has_sample = false;
 }
 
 // calculate temperature
@@ -224,14 +221,13 @@ void AP_Baro_LPS2XH::_update_temperature(void)
     _dev->read_registers(TEMP_OUT_ADDR, pu8, 2);
     int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0];
     
-    if (_sem.take_nonblocking()) {
+    WITH_SEMAPHORE(_sem);
-    	if(_lps2xh_type == BARO_LPS25H){
+
-    		_temperature=((float)(Temp_Reg_s16/480)+42.5);
+    if (_lps2xh_type == BARO_LPS25H) {
-    	}
+        _temperature=((float)(Temp_Reg_s16/480)+42.5);
-    	if(_lps2xh_type == BARO_LPS22H){
+    }
-    		_temperature=(float)(Temp_Reg_s16/100);
+    if (_lps2xh_type == BARO_LPS22H) {
-    	}
+        _temperature=(float)(Temp_Reg_s16/100);
-        _sem.give();
     }
 }
 
@@ -242,8 +238,7 @@ void AP_Baro_LPS2XH::_update_pressure(void)
     _dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3);
     int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0];
     int32_t Pressure_mb = Pressure_Reg_s32 * (100.0 / 4096); // scale for pa
-    if (_sem.take_nonblocking()) {
+
-        _pressure=Pressure_mb;
+    WITH_SEMAPHORE(_sem);
-        _sem.give();
+    _pressure = Pressure_mb;
-    }
 }

libraries/AP_Baro/AP_Baro_SITL.cpp

@@ -121,16 +121,13 @@ void AP_Baro_SITL::_timer()
 // Read the sensor
 void AP_Baro_SITL::update(void)
 {
-    if (_sem.take_nonblocking()) {
+    if (!_has_sample) {
-        if (!_has_sample) {
+        return;
-            _sem.give();
-            return;
-        }
-
-        _copy_to_frontend(_instance, _recent_press, _recent_temp);
-        _has_sample = false;
-        _sem.give();
     }
+
+    WITH_SEMAPHORE(_sem);
+    _copy_to_frontend(_instance, _recent_press, _recent_temp);
+    _has_sample = false;
 }
 
 #endif  // CONFIG_HAL_BOARD

libraries/AP_Baro/AP_Baro_UAVCAN.cpp

@@ -19,7 +19,7 @@ UC_REGISTRY_BINDER(PressureCb, uavcan::equipment::air_data::StaticPressure);
 UC_REGISTRY_BINDER(TemperatureCb, uavcan::equipment::air_data::StaticTemperature);
 
 AP_Baro_UAVCAN::DetectedModules AP_Baro_UAVCAN::_detected_modules[] = {0};
-AP_HAL::Semaphore* AP_Baro_UAVCAN::_sem_registry = nullptr;
+HAL_Semaphore AP_Baro_UAVCAN::_sem_registry;
 
 /*
   constructor - registers instance at top Baro driver
@@ -57,15 +57,12 @@ void AP_Baro_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
 
 bool AP_Baro_UAVCAN::take_registry()
 {
-    if (_sem_registry == nullptr) {
+    return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
-        _sem_registry = hal.util->new_semaphore();
-    }
-    return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
 }
 
 void AP_Baro_UAVCAN::give_registry()
 {
-    _sem_registry->give();
+    _sem_registry.give();
 }
 
 AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro)

libraries/AP_Baro/AP_Baro_UAVCAN.h

@@ -47,5 +47,5 @@ private:
         AP_Baro_UAVCAN* driver;
     } _detected_modules[BARO_MAX_DRIVERS];
 
-    static AP_HAL::Semaphore *_sem_registry;
+    static HAL_Semaphore _sem_registry;
 };