From b2cc992e0c8132f1009be697dfcfeec866543074 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Aug 2018 12:53:17 +1000 Subject: [PATCH] AP_Baro: convert to use WITH_SEMAPHORE() --- libraries/AP_Baro/AP_Baro_BMP085.cpp | 15 ++++---- libraries/AP_Baro/AP_Baro_BMP280.cpp | 23 ++++++------ libraries/AP_Baro/AP_Baro_Backend.cpp | 5 ++- libraries/AP_Baro/AP_Baro_Backend.h | 3 +- libraries/AP_Baro/AP_Baro_DPS280.cpp | 10 +++--- libraries/AP_Baro/AP_Baro_FBM320.cpp | 10 +++--- libraries/AP_Baro/AP_Baro_ICM20789.cpp | 41 +++++++++++---------- libraries/AP_Baro/AP_Baro_KellerLD.cpp | 37 +++++++++---------- libraries/AP_Baro/AP_Baro_LPS2XH.cpp | 15 ++++---- libraries/AP_Baro/AP_Baro_MS5611.cpp | 49 ++++++++++++-------------- libraries/AP_Baro/AP_Baro_SITL.cpp | 6 ++-- libraries/AP_Baro/AP_Baro_UAVCAN.cpp | 21 +++++------ libraries/AP_Baro/AP_Baro_UAVCAN.h | 2 +- 13 files changed, 111 insertions(+), 126 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 12b0fb77e2..e667b38fd0 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -203,9 +203,9 @@ void AP_Baro_BMP085::_timer(void) */ void AP_Baro_BMP085::update(void) { - if (_sem->take_nonblocking()) { + if (_sem.take_nonblocking()) { if (!_has_sample) { - _sem->give(); + _sem.give(); return; } @@ -213,7 +213,7 @@ void AP_Baro_BMP085::update(void) float pressure = _pressure_filter.getf(); _copy_to_frontend(_instance, pressure, temperature); - _sem->give(); + _sem.give(); } } @@ -312,11 +312,10 @@ void AP_Baro_BMP085::_calculate() return; } - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _pressure_filter.apply(p); - _has_sample = true; - _sem->give(); - } + WITH_SEMAPHORE(_sem); + + _pressure_filter.apply(p); + _has_sample = true; } bool AP_Baro_BMP085::_data_ready() diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp index a1f2ee91e5..29bae0ff5e 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -140,15 +140,15 @@ void AP_Baro_BMP280::_timer(void) // transfer data to the frontend void AP_Baro_BMP280::update(void) { - if (_sem->take_nonblocking()) { + if (_sem.take_nonblocking()) { if (!_has_sample) { - _sem->give(); + _sem.give(); return; } _copy_to_frontend(_instance, _pressure, _temperature); _has_sample = false; - _sem->give(); + _sem.give(); } } @@ -165,10 +165,9 @@ void AP_Baro_BMP280::_update_temperature(int32_t temp_raw) const float temp = ((float)t) / 100.0f; - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _temperature = temp; - _sem->give(); - } + WITH_SEMAPHORE(_sem); + + _temperature = temp; } // calculate pressure @@ -199,9 +198,9 @@ void AP_Baro_BMP280::_update_pressure(int32_t press_raw) if (!pressure_ok(press)) { return; } - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _pressure = press; - _has_sample = true; - _sem->give(); - } + + WITH_SEMAPHORE(_sem); + + _pressure = press; + _has_sample = true; } diff --git a/libraries/AP_Baro/AP_Baro_Backend.cpp b/libraries/AP_Baro/AP_Baro_Backend.cpp index 8d0071d4a1..e32d1107ec 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.cpp +++ b/libraries/AP_Baro/AP_Baro_Backend.cpp @@ -7,7 +7,6 @@ extern const AP_HAL::HAL& hal; AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) : _frontend(baro) { - _sem = hal.util->new_semaphore(); } void AP_Baro_Backend::update_healthy_flag(uint8_t instance) @@ -15,7 +14,7 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance) if (instance >= _frontend._num_sensors) { return; } - if (!_sem->take_nonblocking()) { + if (!_sem.take_nonblocking()) { return; } @@ -27,7 +26,7 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance) (now - _frontend.sensors[instance].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) && !is_zero(_frontend.sensors[instance].pressure); - _sem->give(); + _sem.give(); } void AP_Baro_Backend::backend_update(uint8_t instance) diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 75998a2ab7..6d13ab5095 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -1,6 +1,7 @@ #pragma once #include "AP_Baro.h" +#include class AP_Baro_Backend { @@ -33,7 +34,7 @@ protected: void _copy_to_frontend(uint8_t instance, float pressure, float temperature); // semaphore for access to shared frontend data - AP_HAL::Semaphore *_sem; + HAL_Semaphore _sem; virtual void update_healthy_flag(uint8_t instance); diff --git a/libraries/AP_Baro/AP_Baro_DPS280.cpp b/libraries/AP_Baro/AP_Baro_DPS280.cpp index 400034fedb..3aede3a911 100644 --- a/libraries/AP_Baro/AP_Baro_DPS280.cpp +++ b/libraries/AP_Baro/AP_Baro_DPS280.cpp @@ -210,20 +210,20 @@ void AP_Baro_DPS280::timer(void) return; } - if (_sem->take_nonblocking()) { + if (_sem.take_nonblocking()) { pressure_sum += pressure; temperature_sum += temperature; count++; - _sem->give(); + _sem.give(); } } // transfer data to the frontend void AP_Baro_DPS280::update(void) { - if (count != 0 && _sem->take_nonblocking()) { + if (count != 0 && _sem.take_nonblocking()) { if (count == 0) { - _sem->give(); + _sem.give(); return; } @@ -232,6 +232,6 @@ void AP_Baro_DPS280::update(void) temperature_sum = 0; count=0; - _sem->give(); + _sem.give(); } } diff --git a/libraries/AP_Baro/AP_Baro_FBM320.cpp b/libraries/AP_Baro/AP_Baro_FBM320.cpp index d977df9a8b..b5d68a20ff 100644 --- a/libraries/AP_Baro/AP_Baro_FBM320.cpp +++ b/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) && _sem.take_nonblocking()) { pressure_sum += pressure; // sum and convert to degrees temperature_sum += temperature*0.01; count++; - _sem->give(); + _sem.give(); } } @@ -207,9 +207,9 @@ 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 && _sem.take_nonblocking()) { if (count == 0) { - _sem->give(); + _sem.give(); return; } @@ -218,6 +218,6 @@ void AP_Baro_FBM320::update(void) temperature_sum = 0; count=0; - _sem->give(); + _sem.give(); } } diff --git a/libraries/AP_Baro/AP_Baro_ICM20789.cpp b/libraries/AP_Baro/AP_Baro_ICM20789.cpp index 7b118370b2..bf3ec4c4da 100644 --- a/libraries/AP_Baro/AP_Baro_ICM20789.cpp +++ b/libraries/AP_Baro/AP_Baro_ICM20789.cpp @@ -305,19 +305,18 @@ void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw) return; } - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + WITH_SEMAPHORE(_sem); + #if BARO_ICM20789_DEBUG - dd.Praw = Praw; - dd.Traw = Traw; - dd.P = P; - dd.T = T; + dd.Praw = Praw; + dd.Traw = Traw; + dd.P = P; + dd.T = T; #endif - accum.psum += P; - accum.tsum += T; - accum.count++; - _sem->give(); - } + accum.psum += P; + accum.tsum += T; + accum.count++; } void AP_Baro_ICM20789::timer(void) @@ -342,19 +341,19 @@ void AP_Baro_ICM20789::timer(void) void AP_Baro_ICM20789::update() { - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - if (accum.count > 0) { - _copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count); - accum.psum = accum.tsum = 0; - accum.count = 0; - } - _sem->give(); #if BARO_ICM20789_DEBUG - // useful for debugging - DataFlash_Class::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff", - AP_HAL::micros64(), - dd.Traw, dd.Praw, dd.P, dd.T); + // useful for debugging + DataFlash_Class::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff", + AP_HAL::micros64(), + dd.Traw, dd.Praw, dd.P, dd.T); #endif + + WITH_SEMAPHORE(_sem); + + if (accum.count > 0) { + _copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count); + accum.psum = accum.tsum = 0; + accum.count = 0; } } diff --git a/libraries/AP_Baro/AP_Baro_KellerLD.cpp b/libraries/AP_Baro/AP_Baro_KellerLD.cpp index 687aa299f9..62d9bdcc3f 100644 --- a/libraries/AP_Baro/AP_Baro_KellerLD.cpp +++ b/libraries/AP_Baro/AP_Baro_KellerLD.cpp @@ -191,12 +191,12 @@ bool AP_Baro_KellerLD::_read() if (!pressure_ok(pressure_raw)) { return false; } - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128); - _sem->give(); - return true; - } - return false; + + WITH_SEMAPHORE(_sem); + + _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128); + + return true; } // Periodic callback, regular update at 50Hz @@ -230,22 +230,19 @@ void AP_Baro_KellerLD::update() float sum_pressure, sum_temperature; float num_samples; - if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return; + { + WITH_SEMAPHORE(_sem); + + if (_accum.num_samples == 0) { + return; + } + + sum_pressure = _accum.sum_pressure; + sum_temperature = _accum.sum_temperature; + num_samples = _accum.num_samples; + memset(&_accum, 0, sizeof(_accum)); } - if (_accum.num_samples == 0) { - _sem->give(); - return; - } - - sum_pressure = _accum.sum_pressure; - sum_temperature = _accum.sum_temperature; - num_samples = _accum.num_samples; - memset(&_accum, 0, sizeof(_accum)); - - _sem->give(); - uint16_t raw_pressure_avg = sum_pressure / num_samples; uint16_t raw_temperature_avg = sum_temperature / num_samples; diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index b03966f5a7..fe140695f5 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -205,15 +205,15 @@ void AP_Baro_LPS2XH::_timer(void) // transfer data to the frontend void AP_Baro_LPS2XH::update(void) { - if (_sem->take_nonblocking()) { + if (_sem.take_nonblocking()) { if (!_has_sample) { - _sem->give(); + _sem.give(); return; } _copy_to_frontend(_instance, _pressure, _temperature); _has_sample = false; - _sem->give(); + _sem.give(); } } @@ -223,14 +223,15 @@ void AP_Baro_LPS2XH::_update_temperature(void) uint8_t pu8[2]; _dev->read_registers(TEMP_OUT_ADDR, pu8, 2); int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; - if (_sem->take_nonblocking()) { + + if (_sem.take_nonblocking()) { if(_lps2xh_type == BARO_LPS25H){ _temperature=((float)(Temp_Reg_s16/480)+42.5); } if(_lps2xh_type == BARO_LPS22H){ _temperature=(float)(Temp_Reg_s16/100); } - _sem->give(); + _sem.give(); } } @@ -241,8 +242,8 @@ 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()) { + if (_sem.take_nonblocking()) { _pressure=Pressure_mb; - _sem->give(); + _sem.give(); } } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 61669a5ada..fd2ebfd3e7 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -304,19 +304,17 @@ void AP_Baro_MS56XX::_timer(void) return; } - if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - if (_state == 0) { - _update_and_wrap_accumulator(&_accum.s_D2, adc_val, - &_accum.d2_count, 32); - } else { - if (pressure_ok(adc_val)) { - _update_and_wrap_accumulator(&_accum.s_D1, adc_val, - &_accum.d1_count, 128); - } - } - _sem->give(); - _state = next_state; + WITH_SEMAPHORE(_sem); + + if (_state == 0) { + _update_and_wrap_accumulator(&_accum.s_D2, adc_val, + &_accum.d2_count, 32); + } else if (pressure_ok(adc_val)) { + _update_and_wrap_accumulator(&_accum.s_D1, adc_val, + &_accum.d1_count, 128); } + + _state = next_state; } void AP_Baro_MS56XX::_update_and_wrap_accumulator(uint32_t *accum, uint32_t val, @@ -335,23 +333,20 @@ void AP_Baro_MS56XX::update() uint32_t sD1, sD2; uint8_t d1count, d2count; - if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return; + { + WITH_SEMAPHORE(_sem); + + if (_accum.d1_count == 0) { + return; + } + + sD1 = _accum.s_D1; + sD2 = _accum.s_D2; + d1count = _accum.d1_count; + d2count = _accum.d2_count; + memset(&_accum, 0, sizeof(_accum)); } - if (_accum.d1_count == 0) { - _sem->give(); - return; - } - - sD1 = _accum.s_D1; - sD2 = _accum.s_D2; - d1count = _accum.d1_count; - d2count = _accum.d2_count; - memset(&_accum, 0, sizeof(_accum)); - - _sem->give(); - if (d1count != 0) { _D1 = ((float)sD1) / d1count; } diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index d980a764f8..44f9e26e89 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -121,15 +121,15 @@ void AP_Baro_SITL::_timer() // Read the sensor void AP_Baro_SITL::update(void) { - if (_sem->take_nonblocking()) { + if (_sem.take_nonblocking()) { if (!_has_sample) { - _sem->give(); + _sem.give(); return; } _copy_to_frontend(_instance, _recent_press, _recent_temp); _has_sample = false; - _sem->give(); + _sem.give(); } } diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp index b976006e5a..7f871d05fa 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -16,7 +16,6 @@ extern const AP_HAL::HAL& hal; AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) : AP_Baro_Backend(baro) { - _sem_baro = hal.util->new_semaphore(); } AP_Baro_UAVCAN::~AP_Baro_UAVCAN() @@ -31,7 +30,6 @@ AP_Baro_UAVCAN::~AP_Baro_UAVCAN() } ap_uavcan->remove_baro_listener(this); - delete _sem_baro; debug_baro_uavcan(2, _manager, "AP_Baro_UAVCAN destructed\n\r"); } @@ -66,22 +64,19 @@ AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro) // Read the sensor void AP_Baro_UAVCAN::update(void) { - if (_sem_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _copy_to_frontend(_instance, _pressure, _temperature); + WITH_SEMAPHORE(_sem_baro); + _copy_to_frontend(_instance, _pressure, _temperature); - _frontend.set_external_temperature(_temperature); - _sem_baro->give(); - } + _frontend.set_external_temperature(_temperature); } void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature) { - if (_sem_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _pressure = pressure; - _temperature = temperature - C_TO_KELVIN; - _last_timestamp = AP_HAL::micros64(); - _sem_baro->give(); - } + WITH_SEMAPHORE(_sem_baro); + + _pressure = pressure; + _temperature = temperature - C_TO_KELVIN; + _last_timestamp = AP_HAL::micros64(); } bool AP_Baro_UAVCAN::register_uavcan_baro(uint8_t mgr, uint8_t node) diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.h b/libraries/AP_Baro/AP_Baro_UAVCAN.h index 15c20d6702..efb5adbdf2 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.h +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.h @@ -26,5 +26,5 @@ private: bool _initialized; - AP_HAL::Semaphore *_sem_baro; + HAL_Semaphore _sem_baro; };