From af7381e9e0c308444bcaa44c33c9c29f326146eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 17 Apr 2013 21:33:50 +1000 Subject: [PATCH] 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 | 6 ++++ libraries/AP_HAL_AVR/Scheduler.cpp | 5 +++ libraries/AP_HAL_AVR/Scheduler.h | 1 + libraries/AP_HAL_AVR_SITL/Scheduler.cpp | 45 ++++++++++++++++++++++++- libraries/AP_HAL_AVR_SITL/Scheduler.h | 7 +++- libraries/AP_HAL_PX4/Scheduler.cpp | 42 +++++++++++++++++++++++ libraries/AP_HAL_PX4/Scheduler.h | 9 +++++ 7 files changed, 113 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index d4057f529a..7f7dfec347 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/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; diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index b2a7d93956..726e02c498 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/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 */ diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index 52fa5920af..58a41c8f3e 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/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(); diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp index 03e60febcd..edafb7c5ab 100644 --- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp +++ b/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(;;); diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h index 3efbf33a81..6419addfb1 100644 --- a/libraries/AP_HAL_AVR_SITL/Scheduler.h +++ b/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; diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index 673d1d0aad..9aa1701868 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/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; } diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index 9b7f469b0f..776f7178af 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/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