From bae7e4b88eb45cb8c355a3f9822e0bb8bfc73519 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Fri, 26 Oct 2012 18:02:52 -0700 Subject: [PATCH] AP_HAL Scheduler: simplify register_timer_process, min time for delay cb --- libraries/AP_HAL/Scheduler.h | 6 ++-- libraries/AP_HAL_AVR/AnalogIn_APM1.cpp | 2 +- libraries/AP_HAL_AVR/AnalogIn_APM2.cpp | 2 +- libraries/AP_HAL_AVR/Scheduler.cpp | 29 ++++++++++--------- libraries/AP_HAL_AVR/Scheduler.h | 6 ++-- .../examples/Scheduler/Scheduler.pde | 8 ++--- 6 files changed, 27 insertions(+), 26 deletions(-) diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index f0e8d065f7..09cb23cdd1 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/libraries/AP_HAL/Scheduler.h @@ -14,9 +14,9 @@ public: virtual uint32_t millis() = 0; virtual uint32_t micros() = 0; virtual void delay_microseconds(uint16_t us) = 0; - virtual void register_delay_callback(AP_HAL::Proc) = 0; - virtual void register_timer_process(AP_HAL::TimedProc, - uint32_t period_us, uint16_t phase) = 0; + virtual void register_delay_callback(AP_HAL::Proc, + uint16_t min_time_ms) = 0; + virtual void register_timer_process(AP_HAL::TimedProc) = 0; virtual bool defer_timer_process(AP_HAL::TimedProc) = 0; virtual void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us) = 0; diff --git a/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp b/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp index 6510dee6c9..9f05e78c01 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp @@ -14,7 +14,7 @@ APM1AnalogIn::APM1AnalogIn () : void APM1AnalogIn::init(void* machtnichts) { /* Register AVRAnalogIn::_timer_event with the scheduler. */ - hal.scheduler->register_timer_process(_timer_event, 1000, 0); + hal.scheduler->register_timer_process(_timer_event); /* Register each private channel with AVRAnalogIn. */ _register_channel(&_bat_voltage); _register_channel(&_bat_current); diff --git a/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp b/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp index 206f7a40ba..b9b6243aba 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp @@ -17,7 +17,7 @@ APM2AnalogIn::APM2AnalogIn () : void APM2AnalogIn::init(void * machtichts) { /* Register AVRAnalogIn::_timer_event with the scheduler. */ - hal.scheduler->register_timer_process(_timer_event, 1000, 0); + hal.scheduler->register_timer_process(_timer_event); /* Register each private channel with AVRAnalogIn. */ _register_channel(&_pitot); _register_channel(&_bat_voltage); diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index 64dd49372f..8dbd829cbe 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -173,16 +173,18 @@ uint32_t ArduinoScheduler::_micros() { void ArduinoScheduler::delay(uint32_t ms) { uint16_t start = (uint16_t)micros(); - - while (ms > 0) { - if (((uint16_t)micros() - start) >= 1000) { - ms--; - start += 1000; - if (_delay_cb) { - _delay_cb(); + + while (ms > 0) { + if (((uint16_t)micros() - start) >= 1000) { + ms--; + start += 1000; + if (_min_delay_cb_ms >= ms) { + if (_delay_cb) { + _delay_cb(); + } } - } - } + } + } } /* Delay for the given number of microseconds. Assumes a 16 MHz clock. */ @@ -209,14 +211,13 @@ void ArduinoScheduler::delay_microseconds(uint16_t us) ); } -void ArduinoScheduler::register_delay_callback(AP_HAL::Proc proc) { +void ArduinoScheduler::register_delay_callback(AP_HAL::Proc proc, + uint16_t min_time_ms) { _delay_cb = proc; + _min_delay_cb_ms = min_time_ms; } -void ArduinoScheduler::register_timer_process( - AP_HAL::TimedProc proc, uint32_t period_us, uint16_t phase) { - /* XXX Assert period_us == 1000 */ - /* XXX phase meaningless */ +void ArduinoScheduler::register_timer_process(AP_HAL::TimedProc proc) { for (int i = 0; i < _num_timer_procs; i++) { if (_timer_proc[i] == proc) { return; diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index 26ccadfc5e..272edc41c7 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -19,9 +19,8 @@ public: uint32_t millis(); uint32_t micros(); void delay_microseconds(uint16_t us); - void register_delay_callback(AP_HAL::Proc); - void register_timer_process(AP_HAL::TimedProc, - uint32_t period_us, uint16_t phase); + void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); + void register_timer_process(AP_HAL::TimedProc); bool defer_timer_process(AP_HAL::TimedProc); void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us); void suspend_timer_procs(); @@ -39,6 +38,7 @@ private: static uint32_t _micros(); AP_HAL::Proc _delay_cb; + uint16_t _min_delay_cb_ms; static AP_HAL::TimedProc _failsafe; static volatile bool _timer_suspended; diff --git a/libraries/AP_HAL_AVR/examples/Scheduler/Scheduler.pde b/libraries/AP_HAL_AVR/examples/Scheduler/Scheduler.pde index 42db1da9c5..4d6e87d4dc 100644 --- a/libraries/AP_HAL_AVR/examples/Scheduler/Scheduler.pde +++ b/libraries/AP_HAL_AVR/examples/Scheduler/Scheduler.pde @@ -71,7 +71,7 @@ void setup (void) { "Pin %d should toggle at 1khz:\r\n"), (int) DELAY_TOGGLE_PIN); - hal.scheduler->register_delay_callback(delay_toggle); + hal.scheduler->register_delay_callback(delay_toggle,0); hal.scheduler->delay(100); @@ -89,8 +89,8 @@ void setup (void) { hal.console->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"), (int) SCHEDULED_TOGGLE_PIN_2); - hal.scheduler->register_timer_process(schedule_toggle_1, 1000, 0); - hal.scheduler->register_timer_process(schedule_toggle_2, 1000, 0); + hal.scheduler->register_timer_process(schedule_toggle_1); + hal.scheduler->register_timer_process(schedule_toggle_2); hal.scheduler->delay(100); @@ -99,7 +99,7 @@ void setup (void) { "dominates the processor.")); hal.console->printf_P(PSTR("Pin %d should toggle then go high forever.\r\n"), (int) SCHEDULED_TOGGLE_PIN_2); - hal.scheduler->register_timer_process(schedule_toggle_hang, 1000, 0); + hal.scheduler->register_timer_process(schedule_toggle_hang); } void loop (void) { }