diff --git a/libraries/AP_HAL_PX4/AnalogIn.cpp b/libraries/AP_HAL_PX4/AnalogIn.cpp index 343d88454e..597441ff34 100644 --- a/libraries/AP_HAL_PX4/AnalogIn.cpp +++ b/libraries/AP_HAL_PX4/AnalogIn.cpp @@ -79,6 +79,7 @@ PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value) : _sum_value(0), _sum_ratiometric(0) { + _semaphore = hal.util->new_semaphore(); #ifdef PX4_ANALOG_VCC_5V_PIN if (_pin == ANALOG_INPUT_BOARD_VCC) { _pin = PX4_ANALOG_VCC_5V_PIN; @@ -93,16 +94,19 @@ void PX4AnalogSource::set_stop_pin(uint8_t p) float PX4AnalogSource::read_average() { - if (_sum_count == 0) { + 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(); return _value; } - hal.scheduler->suspend_timer_procs(); - _value = _sum_value / _sum_count; - _value_ratiometric = _sum_ratiometric / _sum_count; - _sum_value = 0; - _sum_ratiometric = 0; - _sum_count = 0; - hal.scheduler->resume_timer_procs(); return _value; } @@ -158,15 +162,16 @@ void PX4AnalogSource::set_pin(uint8_t pin) if (_pin == pin) { return; } - hal.scheduler->suspend_timer_procs(); - _pin = pin; - _sum_value = 0; - _sum_ratiometric = 0; - _sum_count = 0; - _latest_value = 0; - _value = 0; - _value_ratiometric = 0; - hal.scheduler->resume_timer_procs(); + 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(); + } } /* @@ -174,20 +179,23 @@ void PX4AnalogSource::set_pin(uint8_t pin) */ void PX4AnalogSource::_add_value(float v, float vcc5V) { - _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; + 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(); } } diff --git a/libraries/AP_HAL_PX4/AnalogIn.h b/libraries/AP_HAL_PX4/AnalogIn.h index 9da33f6981..47f7be5127 100644 --- a/libraries/AP_HAL_PX4/AnalogIn.h +++ b/libraries/AP_HAL_PX4/AnalogIn.h @@ -47,6 +47,7 @@ private: float _sum_ratiometric; void _add_value(float v, float vcc5V); float _pin_scaler(); + AP_HAL::Semaphore *_semaphore; }; class PX4::PX4AnalogIn : public AP_HAL::AnalogIn { diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index 96adc4cb5f..1f6cea051d 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -230,20 +230,6 @@ void PX4Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t perio _failsafe = failsafe; } -void PX4Scheduler::suspend_timer_procs() -{ - _timer_suspended = true; -} - -void PX4Scheduler::resume_timer_procs() -{ - _timer_suspended = false; - if (_timer_event_missed == true) { - _run_timers(false); - _timer_event_missed = false; - } -} - void PX4Scheduler::reboot(bool hold_in_bootloader) { // disarm motors to ensure they are off during a bootloader upload @@ -256,22 +242,18 @@ void PX4Scheduler::reboot(bool hold_in_bootloader) px4_systemreset(hold_in_bootloader); } -void PX4Scheduler::_run_timers(bool called_from_timer_thread) +void PX4Scheduler::_run_timers() { if (_in_timer_proc) { return; } _in_timer_proc = true; - if (!_timer_suspended) { - // now call the timer based drivers - for (int i = 0; i < _num_timer_procs; i++) { - if (_timer_proc[i]) { - _timer_proc[i](); - } + // now call the timer based drivers + for (int i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i]) { + _timer_proc[i](); } - } else if (called_from_timer_thread) { - _timer_event_missed = true; } // and the failsafe, if one is setup @@ -302,7 +284,7 @@ void *PX4Scheduler::_timer_thread(void *arg) // run registered timers perf_begin(sched->_perf_timers); - sched->_run_timers(true); + sched->_run_timers(); perf_end(sched->_perf_timers); // process any pending RC output requests @@ -329,12 +311,10 @@ void PX4Scheduler::_run_io(void) } _in_io_proc = true; - if (!_timer_suspended) { - // now call the IO based drivers - for (int i = 0; i < _num_io_procs; i++) { - if (_io_proc[i]) { - _io_proc[i](); - } + // now call the IO based drivers + for (int i = 0; i < _num_io_procs; i++) { + if (_io_proc[i]) { + _io_proc[i](); } } diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index a9b4f59df9..e9c0352763 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -53,8 +53,6 @@ public: void register_timer_process(AP_HAL::MemberProc); void register_io_process(AP_HAL::MemberProc); void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); - void suspend_timer_procs(); - void resume_timer_procs(); void reboot(bool hold_in_bootloader); bool in_main_thread() const override; @@ -82,8 +80,6 @@ private: uint16_t _min_delay_cb_ms; AP_HAL::Proc _failsafe; - volatile bool _timer_suspended; - AP_HAL::MemberProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS]; uint8_t _num_timer_procs; volatile bool _in_timer_proc; @@ -92,8 +88,6 @@ private: uint8_t _num_io_procs; volatile bool _in_io_proc; - volatile bool _timer_event_missed; - pid_t _main_task_pid; pthread_t _timer_thread_ctx; pthread_t _io_thread_ctx; @@ -112,7 +106,7 @@ private: static void *_uart_thread(void *arg); static void *_uavcan_thread(void *arg); - void _run_timers(bool called_from_timer_thread); + void _run_timers(); void _run_io(void); void delay_microseconds_semaphore(uint16_t us);