From bedee31f61a78381f3a17dfd711ed1fc6c7b0705 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Nov 2016 12:05:12 +1100 Subject: [PATCH] AP_Baro: fixed semaphore and thread usage in baro drivers --- libraries/AP_Baro/AP_Baro_BMP085.cpp | 42 +++++++++++++-------------- libraries/AP_Baro/AP_Baro_BMP085.h | 3 +- libraries/AP_Baro/AP_Baro_Backend.cpp | 4 ++- libraries/AP_Baro/AP_Baro_Backend.h | 3 ++ libraries/AP_Baro/AP_Baro_MS5611.cpp | 7 +---- libraries/AP_Baro/AP_Baro_MS5611.h | 6 ---- libraries/AP_Baro/AP_Baro_qflight.cpp | 12 ++++++-- 7 files changed, 39 insertions(+), 38 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index f809e13607..cf8a1c7344 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -80,22 +80,17 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr _state = 0; sem->give(); + + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, bool)); } /* This is a state machine. Acumulate a new sensor reading. */ -void AP_Baro_BMP085::accumulate(void) +bool AP_Baro_BMP085::_timer(void) { - AP_HAL::Semaphore *sem = _dev->get_semaphore(); - if (!_data_ready()) { - return; - } - - // take i2c bus sempahore - if (!sem->take(1)) { - return; + return true; } if (_state == 0) { @@ -111,8 +106,7 @@ void AP_Baro_BMP085::accumulate(void) } else { _cmd_read_pressure(); } - - sem->give(); + return true; } /* @@ -120,17 +114,18 @@ void AP_Baro_BMP085::accumulate(void) */ void AP_Baro_BMP085::update(void) { - if (!_has_sample && _data_ready()) { - accumulate(); - } - if (!_has_sample) { - return; - } + if (_sem->take_nonblocking()) { + if (!_has_sample) { + _sem->give(); + return; + } - float temperature = 0.1f * _temp; + float temperature = 0.1f * _temp; + float pressure = _pressure_filter.getf(); - float pressure = _pressure_filter.getf(); - _copy_to_frontend(_instance, pressure, temperature); + _copy_to_frontend(_instance, pressure, temperature); + _sem->give(); + } } // Send command to Read Pressure @@ -217,8 +212,11 @@ void AP_Baro_BMP085::_calculate() x2 = (-7357 * p) >> 16; p += ((x1 + x2 + 3791) >> 4); - _pressure_filter.apply(p); - _has_sample = true; + if (_sem->take(0)) { + _pressure_filter.apply(p); + _has_sample = true; + _sem->give(); + } } bool AP_Baro_BMP085::_data_ready() diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index f8162e1519..cdc3de3cdf 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -14,7 +14,6 @@ public: /* AP_Baro public interface: */ void update(); - void accumulate(void); private: void _cmd_read_pressure(); @@ -24,6 +23,8 @@ private: void _calculate(); bool _data_ready(); + bool _timer(void); + AP_HAL::OwnPtr _dev; AP_HAL::DigitalSource *_eoc; diff --git a/libraries/AP_Baro/AP_Baro_Backend.cpp b/libraries/AP_Baro/AP_Baro_Backend.cpp index 8552f8367c..760d8a75b8 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.cpp +++ b/libraries/AP_Baro/AP_Baro_Backend.cpp @@ -5,7 +5,9 @@ extern const AP_HAL::HAL& hal; // constructor AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) : _frontend(baro) -{} +{ + _sem = hal.util->new_semaphore(); +} /* copy latest data to the frontend from a backend diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 2f14a61823..758f771631 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -21,4 +21,7 @@ protected: AP_Baro &_frontend; void _copy_to_frontend(uint8_t instance, float pressure, float temperature); + + // semaphore for access to shared frontend data + AP_HAL::Semaphore *_sem; }; diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index f25a0a43c2..a9cee35160 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -69,11 +69,6 @@ void AP_Baro_MS56XX::_init() AP_HAL::panic("AP_Baro_MS56XX: failed to use device"); } - _sem = hal.util->new_semaphore(); - if (!_sem) { - AP_HAL::panic("AP_Baro_MS56XX: failed to create semaphore"); - } - _instance = _frontend.register_sensor(); if (!_dev->get_semaphore()->take(10)) { @@ -280,7 +275,7 @@ void AP_Baro_MS56XX::update() uint32_t sD1, sD2; uint8_t d1count, d2count; - if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!_sem->take_nonblocking()) { return; } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 02c586de9b..65b98ec1f0 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -33,12 +33,6 @@ protected: AP_HAL::OwnPtr _dev; - /* - * Synchronize access to _accum between thread sampling the HW and main - * thread using the values - */ - AP_HAL::Semaphore *_sem; - /* Shared values between thread sampling the HW and main thread */ struct { uint32_t s_D1; diff --git a/libraries/AP_Baro/AP_Baro_qflight.cpp b/libraries/AP_Baro/AP_Baro_qflight.cpp index 8c8459153b..65dc98acd9 100644 --- a/libraries/AP_Baro/AP_Baro_qflight.cpp +++ b/libraries/AP_Baro/AP_Baro_qflight.cpp @@ -40,27 +40,35 @@ void AP_Baro_QFLIGHT::timer_update(void) return; } + if (!_sem->take(0)) { + return; + } + for (uint16_t i=0; inum_samples; i++) { DSPBuffer::BARO::BUF &b = barobuf->buf[i]; pressure_sum += b.pressure_pa; temperature_sum += b.temperature_C; sum_count++; } + + _sem->give(); } // Read the sensor void AP_Baro_QFLIGHT::update(void) { + if (!_sem->take_nonblocking()) { + return; + } if (sum_count > 0) { - hal.scheduler->suspend_timer_procs(); _copy_to_frontend(instance, pressure_sum/sum_count, temperature_sum/sum_count); sum_count = 0; pressure_sum = 0; temperature_sum = 0; - hal.scheduler->resume_timer_procs(); } + _sem->give(); } #endif // CONFIG_HAL_BOARD_SUBTYPE