diff --git a/libraries/AP_HAL_FLYMAPLE/AnalogIn.cpp b/libraries/AP_HAL_FLYMAPLE/AnalogIn.cpp index 025076910c..5c8f819dbc 100644 --- a/libraries/AP_HAL_FLYMAPLE/AnalogIn.cpp +++ b/libraries/AP_HAL_FLYMAPLE/AnalogIn.cpp @@ -44,7 +44,7 @@ FLYMAPLEAnalogIn::FLYMAPLEAnalogIn() : void FLYMAPLEAnalogIn::init(void* machtnichts) { /* Register FLYMAPLEAnalogIn::_timer_event with the scheduler. */ - hal.scheduler->register_timer_process(_timer_event); + hal.scheduler->register_timer_process(reinterpret_cast(&FLYMAPLEAnalogIn::_timer_event), this); /* Register each private channel with FLYMAPLEAnalogIn. */ _register_channel(&_vcc); } diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp index d4e4f40c61..40f6b984da 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp @@ -41,6 +41,7 @@ volatile bool FLYMAPLEScheduler::_timer_suspended = false; volatile bool FLYMAPLEScheduler::_timer_event_missed = false; volatile bool FLYMAPLEScheduler::_in_timer_proc = false; AP_HAL::TimedProc FLYMAPLEScheduler::_timer_proc[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; +void * FLYMAPLEScheduler::_timer_arg[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; uint8_t FLYMAPLEScheduler::_num_timer_procs = 0; FLYMAPLEScheduler::FLYMAPLEScheduler() : @@ -108,7 +109,7 @@ void FLYMAPLEScheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_ _min_delay_cb_ms = min_time_ms; } -void FLYMAPLEScheduler::register_timer_process(AP_HAL::TimedProc proc) +void FLYMAPLEScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg) { for (int i = 0; i < _num_timer_procs; i++) { if (_timer_proc[i] == proc) { @@ -121,6 +122,7 @@ void FLYMAPLEScheduler::register_timer_process(AP_HAL::TimedProc proc) * because that memory won't be used until _num_timer_procs is * incremented. */ _timer_proc[_num_timer_procs] = proc; + _timer_arg[_num_timer_procs] = arg; /* _num_timer_procs is used from interrupt, and multiple bytes long. */ noInterrupts(); _num_timer_procs++; @@ -128,7 +130,7 @@ void FLYMAPLEScheduler::register_timer_process(AP_HAL::TimedProc proc) } } -void FLYMAPLEScheduler::register_io_process(AP_HAL::TimedProc k) +void FLYMAPLEScheduler::register_io_process(AP_HAL::TimedProc k, void *arg) { // IO processes not supported on FLYMAPLE } @@ -166,7 +168,7 @@ void FLYMAPLEScheduler::_failsafe_timer_event() { // run the failsafe, if one is setup if (_failsafe != NULL) - _failsafe(libmaple_micros()); + _failsafe(NULL); } void FLYMAPLEScheduler::begin_atomic() @@ -181,14 +183,13 @@ void FLYMAPLEScheduler::end_atomic() void FLYMAPLEScheduler::_run_timer_procs(bool called_from_isr) { - uint32_t tnow = libmaple_micros(); _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] != NULL) { - _timer_proc[i](tnow); + _timer_proc[i](_timer_arg[i]); } } } else if (called_from_isr) { diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.h b/libraries/AP_HAL_FLYMAPLE/Scheduler.h index 3a1f972005..32733dfecd 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.h +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.h @@ -34,9 +34,9 @@ public: void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); - void register_timer_process(AP_HAL::TimedProc); + void register_timer_process(AP_HAL::TimedProc, void *); - void register_io_process(AP_HAL::TimedProc); + void register_io_process(AP_HAL::TimedProc, void *); void suspend_timer_procs(); void resume_timer_procs(); @@ -73,6 +73,7 @@ private: static volatile bool _timer_suspended; static volatile bool _timer_event_missed; static AP_HAL::TimedProc _timer_proc[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS]; + static void * _timer_arg[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS]; static uint8_t _num_timer_procs; };