Scheduler loop simplification

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",
        // 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) {
        if (perf_cntr_enabled) {
        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",
        if (time_taken > time_available) {
            if (_spare_ticks == 32) {
                _spare_ticks /= 2;
                _spare_micros /= 2;
        time_available -= time_taken;

it gets rid of the goto ! I am in :stuck_out_tongue_winking_eye:
Could you do a PR after get merged ?

About templates, they will be more in ardupilot, just need the time for the core developper to learn to understand them !

Additionally, I wonder if OpenMP would be still a bad idea? On a RPi I was trying something like this and it helps to increase the loop rate. Most assigned functions in the scheduler look safe.