diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp index 176b222ea7..168e621428 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp @@ -68,7 +68,6 @@ VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) : _sum_value(0), _sum_ratiometric(0) { - _semaphore = hal.util->new_semaphore(); } void VRBRAINAnalogSource::set_stop_pin(uint8_t p) @@ -78,19 +77,17 @@ void VRBRAINAnalogSource::set_stop_pin(uint8_t p) float VRBRAINAnalogSource::read_average() { - if (_semaphore->take(1)) { - if (_sum_count == 0) { - _semaphore->give(); - return _value; - } - _value = _sum_value / _sum_count; - _value_ratiometric = _sum_ratiometric / _sum_count; - _sum_value = 0; - _sum_ratiometric = 0; - _sum_count = 0; - _semaphore->give(); + WITH_SEMAPHORE(_semaphore); + + if (_sum_count == 0) { return _value; } + _value = _sum_value / _sum_count; + _value_ratiometric = _sum_ratiometric / _sum_count; + _sum_value = 0; + _sum_ratiometric = 0; + _sum_count = 0; + return _value; } @@ -146,16 +143,16 @@ void VRBRAINAnalogSource::set_pin(uint8_t pin) if (_pin == pin) { return; } - if (_semaphore->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _pin = pin; - _sum_value = 0; - _sum_ratiometric = 0; - _sum_count = 0; - _latest_value = 0; - _value = 0; - _value_ratiometric = 0; - _semaphore->give(); - } + + WITH_SEMAPHORE(_semaphore); + + _pin = pin; + _sum_value = 0; + _sum_ratiometric = 0; + _sum_count = 0; + _latest_value = 0; + _value = 0; + _value_ratiometric = 0; } /* @@ -163,23 +160,22 @@ void VRBRAINAnalogSource::set_pin(uint8_t pin) */ void VRBRAINAnalogSource::_add_value(float v, float vcc5V) { - if (_semaphore->take(1)) { - _latest_value = v; - _sum_value += v; - if (vcc5V < 3.0f) { - _sum_ratiometric += v; - } else { - // this compensates for changes in the 5V rail relative to the - // 3.3V reference used by the ADC. - _sum_ratiometric += v * 5.0f / vcc5V; - } - _sum_count++; - if (_sum_count == 254) { - _sum_value /= 2; - _sum_ratiometric /= 2; - _sum_count /= 2; - } - _semaphore->give(); + WITH_SEMAPHORE(_semaphore); + + _latest_value = v; + _sum_value += v; + if (vcc5V < 3.0f) { + _sum_ratiometric += v; + } else { + // this compensates for changes in the 5V rail relative to the + // 3.3V reference used by the ADC. + _sum_ratiometric += v * 5.0f / vcc5V; + } + _sum_count++; + if (_sum_count == 254) { + _sum_value /= 2; + _sum_ratiometric /= 2; + _sum_count /= 2; } } diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.h b/libraries/AP_HAL_VRBRAIN/AnalogIn.h index 849f604a53..3d46af7a9b 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.h +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.h @@ -48,7 +48,7 @@ private: float _sum_ratiometric; void _add_value(float v, float vcc5V); float _pin_scaler(); - AP_HAL::Semaphore *_semaphore; + HAL_Semaphore _semaphore; }; class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn { diff --git a/libraries/AP_HAL_VRBRAIN/Util.h b/libraries/AP_HAL_VRBRAIN/Util.h index e723bea837..3325ecc9ca 100644 --- a/libraries/AP_HAL_VRBRAIN/Util.h +++ b/libraries/AP_HAL_VRBRAIN/Util.h @@ -50,9 +50,6 @@ public: void perf_end(perf_counter_t) override; void perf_count(perf_counter_t) override; - // create a new semaphore - AP_HAL::Semaphore *new_semaphore(void) override { return new VRBRAIN::Semaphore; } - void set_imu_temp(float current) override; void set_imu_target_temp(int8_t *target) override;