diff --git a/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp b/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp index e33747bb24..64f0996c1f 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp @@ -64,9 +64,7 @@ void AnalogIn_ADS1115::init() { _adc->init(); - hal.scheduler->suspend_timer_procs(); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AnalogIn_ADS1115::_update, void)); - hal.scheduler->resume_timer_procs(); } void AnalogIn_ADS1115::_update() diff --git a/libraries/AP_HAL_Linux/AnalogIn_IIO.cpp b/libraries/AP_HAL_Linux/AnalogIn_IIO.cpp index f81b0fecbb..475a6ac4ec 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_IIO.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_IIO.cpp @@ -23,6 +23,7 @@ AnalogSource_IIO::AnalogSource_IIO(int16_t pin, float initial_value, float volta _sum_count(0), _pin_fd(-1) { + _semaphore = hal.util->new_semaphore(); init_pins(); select_pin(); } @@ -50,14 +51,16 @@ void AnalogSource_IIO::select_pin(void) float AnalogSource_IIO::read_average() { read_latest(); - if (_sum_count == 0) { - return _value; + if (_semaphore->take(1)) { + if (_sum_count == 0) { + _semaphore->give(); + return _value; + } + _value = _sum_value / _sum_count; + _sum_value = 0; + _sum_count = 0; + _semaphore->give(); } - hal.scheduler->suspend_timer_procs(); - _value = _sum_value / _sum_count; - _sum_value = 0; - _sum_count = 0; - hal.scheduler->resume_timer_procs(); return _value; } @@ -75,9 +78,12 @@ float AnalogSource_IIO::read_latest() _latest = 0; return 0; } - _latest = atoi(sbuf) * _voltage_scaling; - _sum_value += _latest; - _sum_count++; + if (_semaphore->take(1)) { + _latest = atoi(sbuf) * _voltage_scaling; + _sum_value += _latest; + _sum_count++; + _semaphore->give(); + } return _latest; } @@ -100,14 +106,15 @@ void AnalogSource_IIO::set_pin(uint8_t pin) return; } - hal.scheduler->suspend_timer_procs(); - _pin = pin; - _sum_value = 0; - _sum_count = 0; - _latest = 0; - _value = 0; - select_pin(); - hal.scheduler->resume_timer_procs(); + if (_semaphore->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + _pin = pin; + _sum_value = 0; + _sum_count = 0; + _latest = 0; + _value = 0; + select_pin(); + _semaphore->give(); + } } void AnalogSource_IIO::set_stop_pin(uint8_t p) diff --git a/libraries/AP_HAL_Linux/AnalogIn_IIO.h b/libraries/AP_HAL_Linux/AnalogIn_IIO.h index 790dee6670..0bce2085d4 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_IIO.h +++ b/libraries/AP_HAL_Linux/AnalogIn_IIO.h @@ -45,6 +45,7 @@ private: int16_t _pin; int _pin_fd; int fd_analog_sources[IIO_ANALOG_IN_COUNT]; + AP_HAL::Semaphore *_semaphore; void init_pins(void); void select_pin(void); diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index f116d7fb03..2d64b46a5a 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -208,18 +208,6 @@ void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_u _failsafe = failsafe; } -void Scheduler::suspend_timer_procs() -{ - if (!_timer_semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - printf("Failed to take timer semaphore\n"); - } -} - -void Scheduler::resume_timer_procs() -{ - _timer_semaphore.give(); -} - void Scheduler::_timer_task() { int i; @@ -229,11 +217,6 @@ void Scheduler::_timer_task() } _in_timer_proc = true; - if (!_timer_semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - printf("Failed to take timer semaphore in %s\n", __PRETTY_FUNCTION__); - return; - } - // now call the timer based drivers for (i = 0; i < _num_timer_procs; i++) { if (_timer_proc[i]) { @@ -241,8 +224,6 @@ void Scheduler::_timer_task() } } - _timer_semaphore.give(); - // and the failsafe, if one is setup if (_failsafe != nullptr) { _failsafe(); diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index e340e3ded7..b44718c0f9 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -30,8 +30,6 @@ public: void register_timer_process(AP_HAL::MemberProc); void register_io_process(AP_HAL::MemberProc); - void suspend_timer_procs(); - void resume_timer_procs(); bool in_main_thread() const override; @@ -98,7 +96,6 @@ private: uint64_t _last_stack_debug_msec; pthread_t _main_ctx; - Semaphore _timer_semaphore; Semaphore _io_semaphore; };