AP_HAL_PX4: Remove timer process suspension interface

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 (_semaphore->take(1)) {
         if (_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();
+        _semaphore->give();
+        return _value;
+    }
     return _value;
 }
 
@@ -158,7 +162,7 @@ void PX4AnalogSource::set_pin(uint8_t pin)
     if (_pin == pin) {
         return;
     }
-    hal.scheduler->suspend_timer_procs();
+    if (_semaphore->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         _pin = pin;
         _sum_value = 0;
         _sum_ratiometric = 0;
@@ -166,7 +170,8 @@ void PX4AnalogSource::set_pin(uint8_t pin)
         _latest_value = 0;
         _value = 0;
         _value_ratiometric = 0;
-    hal.scheduler->resume_timer_procs();
+        _semaphore->give();
+    }
 }
 
 /*
@@ -174,6 +179,7 @@ void PX4AnalogSource::set_pin(uint8_t pin)
  */
 void PX4AnalogSource::_add_value(float v, float vcc5V)
 {
+    if (_semaphore->take(1)) {
         _latest_value = v;
         _sum_value += v;
         if (vcc5V < 3.0f) {
@@ -189,6 +195,8 @@ void PX4AnalogSource::_add_value(float v, float vcc5V)
             _sum_ratiometric /= 2;
             _sum_count /= 2;
         }
+        _semaphore->give();
+    }
 }
 
 

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 {

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,23 +242,19 @@ 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]();
         }
     }
-    } else if (called_from_timer_thread) {
-        _timer_event_missed = true;
-    }
 
     // and the failsafe, if one is setup
     if (_failsafe != nullptr) {
@@ -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,14 +311,12 @@ 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]();
         }
     }
-    }
 
     _in_io_proc = false;
 }

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