From fe064a2d4e254d19241a8ac69f847b9a4c656ab0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 12 Oct 2018 10:35:03 +1100 Subject: [PATCH] AP_Airspeed: use WITH_SEMAPHORE() and removed usage of hal.util->new_semaphore() --- libraries/AP_Airspeed/AP_Airspeed_Backend.cpp | 2 - libraries/AP_Airspeed/AP_Airspeed_Backend.h | 2 +- libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp | 25 +++++---- libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp | 51 +++++++++---------- libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp | 45 ++++++++-------- libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp | 36 ++++++------- libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp | 10 ++-- libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h | 2 +- 8 files changed, 85 insertions(+), 88 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp index 9f1f9935eb..f6d1806039 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp +++ b/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; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 560d76350b..b7c2f9b44c 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/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); diff --git a/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp b/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp index 30262a95e2..abbd57931d 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp +++ b/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; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp index b1941b32a8..396a1a2272 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp +++ b/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; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp index 2c112b3307..135911e27a 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp +++ b/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; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp index aa26b2b2f1..9da4fbc02e 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp +++ b/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; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp index fa9675e3bf..01bcf40b38 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp +++ b/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) diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h index dfa5d5e12b..3a0a9e10d8 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h +++ b/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; };