I am a bit too lazy to make a commit. Once I prepared a scheduler commit and it was not accepted because I replaced a macro with a template. However, I still think the scheduler could be simplified. Is anything speaking against a loop like this? Was just 5 mins of work.
for (uint8_t i=0; i<_num_tasks; i++) {
const bool perf_cntr_enabled = _debug > 1 && _perf_counters && _perf_counters[i];
const uint16_t dt = _tick_counter - _last_run[i];
const uint16_t rate_ratio = _loop_rate_hz / _tasks[i].rate_hz;
const uint16_t interval_ticks = rate_ratio > 0 ? rate_ratio : 1;
if(dt < interval_ticks) continue;
if (dt > interval_ticks*2) {
// we've slipped a whole run of this task!
if (_debug > 4) {
::printf("Scheduler slip task[%u-%s] (%u/%u/%u)\n",
(unsigned)i,
_tasks[i].name,
(unsigned)dt,
(unsigned)interval_ticks,
(unsigned)_tasks[i].max_time_micros);
}
}
// this task is due to run. Do we have enough time to run it?
_task_time_allowed = _tasks[i].max_time_micros;
if(_task_time_allowed > time_available) continue;
_task_time_started = AP_HAL::micros();
// run it
current_task = i;
if (perf_cntr_enabled) {
hal.util->perf_begin(_perf_counters[i]);
}
_tasks[i].function();
if (perf_cntr_enabled) {
hal.util->perf_end(_perf_counters[i]);
}
current_task = -1;
// work out how long the event actually took
const uint32_t time_taken = AP_HAL::micros() - _task_time_started;
// record the tick counter when we ran. This drives
// when we next run the event
_last_run[i] = _tick_counter;
const bool time_exceeded = time_taken > _task_time_allowed;
if (time_exceeded && _debug > 4) {
::printf("Scheduler overrun task[%u-%s] (%u/%u)\n",
(unsigned)i,
_tasks[i].name,
(unsigned)time_taken,
(unsigned)_task_time_allowed);
}
if (time_taken > time_available) {
_spare_ticks++;
if (_spare_ticks == 32) {
_spare_ticks /= 2;
_spare_micros /= 2;
}
return;
}
time_available -= time_taken;
}