AP_HAL_VRBrain: Remove timer process suspension interface

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();
     }
 }
 

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 {

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]();
         }
     }
 

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);