AP_HAL: added register_io_process()

this is used to register a low priority IO task. Used for file IO in
DataFlash_File.cpp

libraries/AP_HAL/Scheduler.h

@@ -18,7 +18,13 @@ public:
     virtual void     register_delay_callback(AP_HAL::Proc,
                         uint16_t min_time_ms) = 0;
 
+    // register a high priority timer task
     virtual void     register_timer_process(AP_HAL::TimedProc) = 0;
+
+    // register a low priority IO task
+    virtual void     register_io_process(AP_HAL::TimedProc) = 0;
+
+    // suspend and resume both timer and IO processes
     virtual void     suspend_timer_procs() = 0;
     virtual void     resume_timer_procs() = 0;
 

libraries/AP_HAL_AVR/Scheduler.cpp

@@ -108,6 +108,11 @@ void AVRScheduler::register_timer_process(AP_HAL::TimedProc proc) {
 
 }
 
+void AVRScheduler::register_io_process(AP_HAL::TimedProc proc) 
+{
+    // IO processes not supported on AVR
+}
+
 void AVRScheduler::register_timer_failsafe(
         AP_HAL::TimedProc failsafe, uint32_t period_us) {
     /* XXX Assert period_us == 1000 */

libraries/AP_HAL_AVR/Scheduler.h

@@ -32,6 +32,7 @@ public:
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
 
     void     register_timer_process(AP_HAL::TimedProc);
+    void     register_io_process(AP_HAL::TimedProc);
     void     suspend_timer_procs();
     void     resume_timer_procs();
 

libraries/AP_HAL_AVR_SITL/Scheduler.cpp

@@ -16,9 +16,15 @@ extern const AP_HAL::HAL& hal;
 AP_HAL::TimedProc SITLScheduler::_failsafe = NULL;
 volatile bool SITLScheduler::_timer_suspended = false;
 volatile bool SITLScheduler::_timer_event_missed = false;
+
 AP_HAL::TimedProc SITLScheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
 uint8_t SITLScheduler::_num_timer_procs = 0;
 bool SITLScheduler::_in_timer_proc = false;
+
+AP_HAL::TimedProc SITLScheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
+uint8_t SITLScheduler::_num_io_procs = 0;
+bool SITLScheduler::_in_io_proc = false;
+
 struct timeval SITLScheduler::_sketch_start_time;
 
 SITLScheduler::SITLScheduler()
@@ -100,6 +106,21 @@ void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc)
 
 }
 
+void SITLScheduler::register_io_process(AP_HAL::TimedProc proc) 
+{
+    for (uint8_t i = 0; i < _num_io_procs; i++) {
+        if (_io_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_io_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) {
+        _io_proc[_num_io_procs] = proc;
+        _num_io_procs++;
+    }
+
+}
+
 void SITLScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) 
 {
     _failsafe = failsafe;
@@ -118,7 +139,7 @@ void SITLScheduler::resume_timer_procs() {
 }
 
 bool SITLScheduler::in_timerprocess() {
-    return _in_timer_proc;
+    return _in_timer_proc || _in_io_proc;
 }
 
 bool SITLScheduler::system_initializing() {
@@ -185,6 +206,28 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
     _in_timer_proc = false;
 }
 
+void SITLScheduler::_run_io_procs(bool called_from_isr) 
+{
+    uint32_t tnow = _micros();
+    if (_in_io_proc) {
+        return;
+    }
+    _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] != NULL) {
+                _io_proc[i](tnow);
+            }
+        }
+    } else if (called_from_isr) {
+        _timer_event_missed = true;
+    }
+
+    _in_io_proc = false;
+}
+
 void SITLScheduler::panic(const prog_char_t *errormsg) {
     hal.console->println_P(errormsg);
     for(;;);

libraries/AP_HAL_AVR_SITL/Scheduler.h

@@ -23,6 +23,7 @@ public:
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
 
     void     register_timer_process(AP_HAL::TimedProc);
+    void     register_io_process(AP_HAL::TimedProc);
     void     suspend_timer_procs();
     void     resume_timer_procs();
 
@@ -43,7 +44,7 @@ public:
 
     // callable from interrupt handler
     static uint32_t _micros();
-    static void timer_event() { _run_timer_procs(true); }
+    static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
 
 private:
     uint8_t _nested_atomic_ctr;
@@ -53,12 +54,16 @@ private:
     static AP_HAL::TimedProc _failsafe;
 
     static void _run_timer_procs(bool called_from_isr);
+    static void _run_io_procs(bool called_from_isr);
 
     static volatile bool _timer_suspended;
     static volatile bool _timer_event_missed;
     static AP_HAL::TimedProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
+    static AP_HAL::TimedProc _io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
     static uint8_t _num_timer_procs;
+    static uint8_t _num_io_procs;
     static bool    _in_timer_proc;
+    static bool    _in_io_proc;
 
     bool _initialized;
 

libraries/AP_HAL_PX4/Scheduler.cpp

@@ -29,6 +29,7 @@ extern bool _px4_thread_should_exit;
 
 PX4Scheduler::PX4Scheduler() :
     _perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")),
+    _perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")),
 	_perf_delay(perf_alloc(PC_ELAPSED, "APM_delay"))
 {}
 
@@ -132,6 +133,22 @@ void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc)
     }
 }
 
+void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc) 
+{
+    for (uint8_t i = 0; i < _num_io_procs; i++) {
+        if (_io_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_io_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
+        _io_proc[_num_io_procs] = proc;
+        _num_io_procs++;
+    } else {
+        hal.console->printf("Out of IO processes\n");
+    }
+}
+
 void PX4Scheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) 
 {
     _failsafe = failsafe;
@@ -202,6 +219,26 @@ void *PX4Scheduler::_timer_thread(void)
     return NULL;
 }
 
+void PX4Scheduler::_run_io(void)
+{
+    uint32_t tnow = micros();
+    if (_in_io_proc) {
+        return;
+    }
+    _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] != NULL) {
+                _io_proc[i](tnow);
+            }
+        }
+    }
+
+    _in_io_proc = false;
+}
+
 void *PX4Scheduler::_io_thread(void)
 {
     while (!_px4_thread_should_exit) {
@@ -214,6 +251,11 @@ void *PX4Scheduler::_io_thread(void)
 
         // process any pending storage writes
         ((PX4Storage *)hal.storage)->_timer_tick();
+
+        // run registered IO processes
+        perf_begin(_perf_io_timers);
+        _run_io();
+        perf_end(_perf_io_timers);
     }
     return NULL;
 }

libraries/AP_HAL_PX4/Scheduler.h

@@ -31,6 +31,7 @@ public:
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
     void     register_timer_process(AP_HAL::TimedProc);
+    void     register_io_process(AP_HAL::TimedProc);
     void     register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
     void     suspend_timer_procs();
     void     resume_timer_procs();
@@ -50,9 +51,15 @@ private:
     uint64_t _sketch_start_time;
 
     volatile bool _timer_suspended;
+
     AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
     uint8_t _num_timer_procs;
     volatile bool _in_timer_proc;
+
+    AP_HAL::TimedProc _io_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_io_procs;
+    volatile bool _in_io_proc;
+
     volatile bool _timer_event_missed;
 
     pthread_t _timer_thread_ctx;
@@ -62,8 +69,10 @@ private:
     void *_io_thread(void);
 
     void _run_timers(bool called_from_timer_thread);
+    void _run_io(void);
 
     perf_counter_t  _perf_timers;
+    perf_counter_t  _perf_io_timers;
     perf_counter_t  _perf_delay;
 };
 #endif