Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scheduler: improved the accuracy of the scheduling code

Browse Source 
prevent accumulated micros() errors in run()
This commit is contained in:
Andrew Tridgell 2013-10-10 14:13:44 +11:00
parent dd53677a3c
commit dd4b9275fd
2 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Scheduler/AP_Scheduler.cpp
View File

@ -58,6 +58,9 @@ void AP_Scheduler::tick(void)
*/ */
void AP_Scheduler::run(uint16_t time_available) void AP_Scheduler::run(uint16_t time_available)
{ {
uint32_t run_started_usec = hal.scheduler->micros();
uint32_t now = run_started_usec;
for (uint8_t i=0; i<_num_tasks; i++) { for (uint8_t i=0; i<_num_tasks; i++) {
uint16_t dt = _tick_counter - _last_run[i]; uint16_t dt = _tick_counter - _last_run[i];
uint16_t interval_ticks = pgm_read_word(&_tasks[i].interval_ticks); uint16_t interval_ticks = pgm_read_word(&_tasks[i].interval_ticks);
@ -75,9 +78,10 @@ void AP_Scheduler::run(uint16_t time_available)
(unsigned)_task_time_allowed); (unsigned)_task_time_allowed);
} }
} }
if (_task_time_allowed <= time_available) { if (_task_time_allowed <= time_available) {
// run it // run it
_task_time_started = hal.scheduler->micros(); _task_time_started = now;
task_fn_t func = (task_fn_t)pgm_read_pointer(&_tasks[i].function); task_fn_t func = (task_fn_t)pgm_read_pointer(&_tasks[i].function);
func(); func();
@ -86,7 +90,8 @@ void AP_Scheduler::run(uint16_t time_available)
_last_run[i] = _tick_counter; _last_run[i] = _tick_counter;
// work out how long the event actually took // work out how long the event actually took
uint32_t time_taken = hal.scheduler->micros() - _task_time_started; now = hal.scheduler->micros();
uint32_t time_taken = now - _task_time_started;
if (time_taken > _task_time_allowed) { if (time_taken > _task_time_allowed) {
// the event overran! // the event overran!

2
libraries/AP_Scheduler/AP_Scheduler.h
View File

@ -88,7 +88,7 @@ private:
uint16_t *_last_run; uint16_t *_last_run;
// number of microseconds allowed for the current task // number of microseconds allowed for the current task
uint16_t _task_time_allowed; uint32_t _task_time_allowed;
// the time in microseconds when the task started // the time in microseconds when the task started
uint32_t _task_time_started; uint32_t _task_time_started;