From 82fe40224bfba26421c85838a8fb99eb29986001 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 30 Sep 2013 20:35:42 +1000 Subject: [PATCH] HAL_AVR_SITL: updates for AP_HAL::MemberProc --- libraries/AP_HAL_AVR_SITL/Scheduler.cpp | 24 ++++++++++-------------- libraries/AP_HAL_AVR_SITL/Scheduler.h | 14 ++++++-------- 2 files changed, 16 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp index cfced346ce..db0d172824 100644 --- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp @@ -24,17 +24,15 @@ using namespace AVR_SITL; extern const AP_HAL::HAL& hal; -AP_HAL::TimedProc SITLScheduler::_failsafe = NULL; +AP_HAL::Proc 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}; -void *SITLScheduler::_timer_arg[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; +AP_HAL::MemberProc 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}; -void *SITLScheduler::_io_arg[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; +AP_HAL::MemberProc SITLScheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; uint8_t SITLScheduler::_num_io_procs = 0; bool SITLScheduler::_in_io_proc = false; @@ -137,7 +135,7 @@ void SITLScheduler::register_delay_callback(AP_HAL::Proc proc, _min_delay_cb_ms = min_time_ms; } -void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg) +void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc) { for (uint8_t i = 0; i < _num_timer_procs; i++) { if (_timer_proc[i] == proc) { @@ -147,13 +145,12 @@ void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg) if (_num_timer_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) { _timer_proc[_num_timer_procs] = proc; - _timer_arg[_num_timer_procs] = arg; _num_timer_procs++; } } -void SITLScheduler::register_io_process(AP_HAL::TimedProc proc, void *arg) +void SITLScheduler::register_io_process(AP_HAL::MemberProc proc) { for (uint8_t i = 0; i < _num_io_procs; i++) { if (_io_proc[i] == proc) { @@ -163,13 +160,12 @@ void SITLScheduler::register_io_process(AP_HAL::TimedProc proc, void *arg) if (_num_io_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) { _io_proc[_num_io_procs] = proc; - _io_arg[_num_io_procs] = arg; _num_io_procs++; } } -void SITLScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) +void SITLScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) { _failsafe = failsafe; } @@ -228,7 +224,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr) // block. If it does then we will recurse and die when // we run out of stack if (_failsafe != NULL) { - _failsafe(NULL); + _failsafe(); } return; } @@ -238,7 +234,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr) // now call the timer based drivers for (int i = 0; i < _num_timer_procs; i++) { if (_timer_proc[i] != NULL) { - _timer_proc[i](_timer_arg[i]); + _timer_proc[i](); } } } else if (called_from_isr) { @@ -247,7 +243,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr) // and the failsafe, if one is setup if (_failsafe != NULL) { - _failsafe(NULL); + //_failsafe(NULL); } _in_timer_proc = false; @@ -264,7 +260,7 @@ void SITLScheduler::_run_io_procs(bool called_from_isr) // now call the IO based drivers for (int i = 0; i < _num_io_procs; i++) { if (_io_proc[i] != NULL) { - _io_proc[i](_io_arg[i]); + _io_proc[i](); } } } else if (called_from_isr) { diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h index 53c6069d21..bd6e33aba9 100644 --- a/libraries/AP_HAL_AVR_SITL/Scheduler.h +++ b/libraries/AP_HAL_AVR_SITL/Scheduler.h @@ -22,14 +22,14 @@ 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 *); - void register_io_process(AP_HAL::TimedProc, void *); + void register_timer_process(AP_HAL::MemberProc); + void register_io_process(AP_HAL::MemberProc); void suspend_timer_procs(); void resume_timer_procs(); bool in_timerprocess(); - void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us); + void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); bool system_initializing(); void system_initialized(); @@ -51,17 +51,15 @@ private: AP_HAL::Proc _delay_cb; uint16_t _min_delay_cb_ms; static struct timeval _sketch_start_time; - static AP_HAL::TimedProc _failsafe; + static AP_HAL::Proc _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 void *_timer_arg[SITL_SCHEDULER_MAX_TIMER_PROCS]; - static AP_HAL::TimedProc _io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS]; - static void *_io_arg[SITL_SCHEDULER_MAX_TIMER_PROCS]; + static AP_HAL::MemberProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS]; + static AP_HAL::MemberProc _io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS]; static uint8_t _num_timer_procs; static uint8_t _num_io_procs; static bool _in_timer_proc;