From a2dfca2fee6ff06eaf45028e010bf4a1973ba5ff Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sun, 13 May 2018 15:47:35 -0700 Subject: [PATCH] AP_HAL_VRBrain: Remove timer process suspension interface --- libraries/AP_HAL_VRBRAIN/AnalogIn.cpp | 75 +++++++++++++------------- libraries/AP_HAL_VRBRAIN/AnalogIn.h | 1 + libraries/AP_HAL_VRBRAIN/Scheduler.cpp | 40 ++++---------- libraries/AP_HAL_VRBRAIN/Scheduler.h | 8 +-- 4 files changed, 51 insertions(+), 73 deletions(-) diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp index ccf7e60add..176b222ea7 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp @@ -68,11 +68,7 @@ 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) @@ -82,16 +78,19 @@ void VRBRAINAnalogSource::set_stop_pin(uint8_t p) float VRBRAINAnalogSource::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; } @@ -147,15 +146,16 @@ void VRBRAINAnalogSource::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(); + } } /* @@ -163,20 +163,23 @@ void VRBRAINAnalogSource::set_pin(uint8_t pin) */ void VRBRAINAnalogSource::_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_VRBRAIN/AnalogIn.h b/libraries/AP_HAL_VRBRAIN/AnalogIn.h index 4d1d053190..849f604a53 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.h +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.h @@ -48,6 +48,7 @@ private: float _sum_ratiometric; void _add_value(float v, float vcc5V); float _pin_scaler(); + AP_HAL::Semaphore *_semaphore; }; class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn { diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp index e20be8f607..7b12f91061 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp @@ -230,20 +230,6 @@ void VRBRAINScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t p _failsafe = failsafe; } -void VRBRAINScheduler::suspend_timer_procs() -{ - _timer_suspended = true; -} - -void VRBRAINScheduler::resume_timer_procs() -{ - _timer_suspended = false; - if (_timer_event_missed == true) { - _run_timers(false); - _timer_event_missed = false; - } -} - void VRBRAINScheduler::reboot(bool hold_in_bootloader) { // disarm motors to ensure they are off during a bootloader upload @@ -256,22 +242,18 @@ void VRBRAINScheduler::reboot(bool hold_in_bootloader) px4_systemreset(hold_in_bootloader); } -void VRBRAINScheduler::_run_timers(bool called_from_timer_thread) +void VRBRAINScheduler::_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 *VRBRAINScheduler::_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 VRBRAINScheduler::_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_VRBRAIN/Scheduler.h b/libraries/AP_HAL_VRBRAIN/Scheduler.h index 94f8f8370f..b4a74372a4 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ b/libraries/AP_HAL_VRBRAIN/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; @@ -68,8 +66,6 @@ private: volatile bool _hal_initialized; AP_HAL::Proc _failsafe; - volatile bool _timer_suspended; - AP_HAL::MemberProc _timer_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS]; uint8_t _num_timer_procs; volatile bool _in_timer_proc; @@ -78,8 +74,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; @@ -98,7 +92,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);