Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * main loop scheduler for ArduPilot
18 : * Author: Andrew Tridgell, January 2013
19 : *
20 : */
21 :
22 : #include "AP_Scheduler_config.h"
23 :
24 : #if AP_SCHEDULER_ENABLED
25 :
26 : #include "AP_Scheduler.h"
27 :
28 : #include <AP_HAL/AP_HAL.h>
29 : #include <AP_Param/AP_Param.h>
30 : #include <AP_Vehicle/AP_Vehicle.h>
31 : #include <AP_Logger/AP_Logger.h>
32 : #include <AP_InertialSensor/AP_InertialSensor.h>
33 : #include <AP_InternalError/AP_InternalError.h>
34 : #include <AP_Common/ExpandingString.h>
35 : #include <AP_HAL/SIMState.h>
36 : #include <AP_Vehicle/AP_Vehicle_Type.h>
37 :
38 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
39 : #include <SITL/SITL.h>
40 : #endif
41 : #include <stdio.h>
42 :
43 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduSub)
44 : #define SCHEDULER_DEFAULT_LOOP_RATE 400
45 : #else
46 : #define SCHEDULER_DEFAULT_LOOP_RATE 50
47 : #endif
48 :
49 : #define debug(level, fmt, args...) do { if ((level) <= _debug.get()) { hal.console->printf(fmt, ##args); }} while (0)
50 :
51 : extern const AP_HAL::HAL& hal;
52 :
53 : const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
54 : // @Param: DEBUG
55 : // @DisplayName: Scheduler debug level
56 : // @Description: Set to non-zero to enable scheduler debug messages. When set to show "Slips" the scheduler will display a message whenever a scheduled task is delayed due to too much CPU load. When set to ShowOverruns the scheduled will display a message whenever a task takes longer than the limit promised in the task table.
57 : // @Values: 0:Disabled,2:ShowSlips,3:ShowOverruns
58 : // @User: Advanced
59 : AP_GROUPINFO("DEBUG", 0, AP_Scheduler, _debug, 0),
60 :
61 : // @Param: LOOP_RATE
62 : // @DisplayName: Scheduling main loop rate
63 : // @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart. Values over 400 are considered highly experimental.
64 : // @Range: 50 400
65 : // @RebootRequired: True
66 : // @Units: Hz
67 : // @User: Advanced
68 : AP_GROUPINFO("LOOP_RATE", 1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE),
69 :
70 : // @Param: OPTIONS
71 : // @DisplayName: Scheduling options
72 : // @Description: This controls optional aspects of the scheduler.
73 : // @Bitmask: 0:Enable per-task perf info
74 : // @User: Advanced
75 : AP_GROUPINFO("OPTIONS", 2, AP_Scheduler, _options, 0),
76 :
77 : AP_GROUPEND
78 : };
79 :
80 : // constructor
81 2098 : AP_Scheduler::AP_Scheduler()
82 : {
83 2098 : if (_singleton) {
84 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
85 0 : AP_HAL::panic("Too many schedulers");
86 : #endif
87 0 : return;
88 : }
89 2098 : _singleton = this;
90 :
91 2098 : AP_Param::setup_object_defaults(this, var_info);
92 : }
93 :
94 : /*
95 : * Get the AP_Scheduler singleton
96 : */
97 : AP_Scheduler *AP_Scheduler::_singleton;
98 811726726 : AP_Scheduler *AP_Scheduler::get_singleton()
99 : {
100 811726726 : return _singleton;
101 : }
102 :
103 : // initialise the scheduler
104 2094 : void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit)
105 : {
106 : // grab the semaphore before we start anything
107 2094 : _rsem.take_blocking();
108 :
109 : // only allow 50 to 2000 Hz
110 2094 : if (_loop_rate_hz < 50) {
111 0 : _loop_rate_hz.set(50);
112 2094 : } else if (_loop_rate_hz > 2000) {
113 0 : _loop_rate_hz.set(2000);
114 : }
115 2094 : _last_loop_time_s = 1.0 / _loop_rate_hz;
116 :
117 : // These variables are initialized only here in scheduler::init()
118 : // These are also defensively initialized in the getter functions to catch initialization order issues.
119 2094 : _loop_period_us = 1000000UL / _loop_rate_hz;
120 2094 : _loop_period_s = 1.0f / _loop_rate_hz;
121 2094 : _active_loop_rate_hz = _loop_rate_hz;
122 :
123 2094 : _vehicle_tasks = tasks;
124 2094 : _num_vehicle_tasks = num_tasks;
125 :
126 : #if AP_VEHICLE_ENABLED
127 2094 : AP_Vehicle* vehicle = AP::vehicle();
128 2094 : if (vehicle != nullptr) {
129 2094 : vehicle->get_common_scheduler_tasks(_common_tasks, _num_common_tasks);
130 : }
131 : #endif
132 :
133 2094 : _num_tasks = _num_vehicle_tasks + _num_common_tasks;
134 :
135 2094 : _last_run = NEW_NOTHROW uint16_t[_num_tasks];
136 2094 : _tick_counter = 0;
137 :
138 : // setup initial performance counters
139 2094 : perf_info.set_loop_rate(get_loop_rate_hz());
140 2094 : perf_info.reset();
141 :
142 2094 : if (_options & uint8_t(Options::RECORD_TASK_INFO)) {
143 0 : perf_info.allocate_task_info(_num_tasks);
144 : }
145 :
146 2094 : _log_performance_bit = log_performance_bit;
147 :
148 : // sanity check the task lists to ensure the priorities are
149 : // never decrease
150 2094 : uint8_t old = 0;
151 63913 : for (uint8_t i=0; i<_num_common_tasks; i++) {
152 61819 : if (_common_tasks[i].priority < old){
153 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
154 0 : break;
155 : }
156 61819 : old = _common_tasks[i].priority;
157 : }
158 2094 : old = 0;
159 104016 : for (uint8_t i=0; i<_num_vehicle_tasks; i++) {
160 101922 : if (_vehicle_tasks[i].priority < old) {
161 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
162 0 : break;
163 : }
164 101922 : old = _vehicle_tasks[i].priority;
165 : }
166 2094 : }
167 :
168 : // one tick has passed
169 41686253 : void AP_Scheduler::tick(void)
170 : {
171 41686253 : _tick_counter++;
172 41686253 : _tick_counter32++;
173 41686253 : }
174 :
175 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
176 : /*
177 : fill stack with NaN so we can catch use of uninitialised stack
178 : variables in SITL
179 : */
180 1133018659 : static void fill_nanf_stack(void)
181 : {
182 : float v[1024];
183 1133018659 : fill_nanf(v, ARRAY_SIZE(v));
184 1133018659 : }
185 : #endif
186 :
187 : /*
188 : run one tick
189 : this will run as many scheduler tasks as we can in the specified time
190 : */
191 41686253 : void AP_Scheduler::run(uint32_t time_available)
192 : {
193 41686253 : uint32_t run_started_usec = AP_HAL::micros();
194 41686253 : uint32_t now = run_started_usec;
195 :
196 41686253 : uint8_t vehicle_tasks_offset = 0;
197 41686253 : uint8_t common_tasks_offset = 0;
198 :
199 3460287610 : for (uint8_t i=0; i<_num_tasks; i++) {
200 : // determine which of the common task / vehicle task to run
201 3418602988 : bool run_vehicle_task = false;
202 3418602988 : if (vehicle_tasks_offset < _num_vehicle_tasks &&
203 2495953403 : common_tasks_offset < _num_common_tasks) {
204 : // still have entries on both lists; compare the
205 : // priorities. In case of a tie the vehicle-specific
206 : // entry wins.
207 2495953403 : const Task &vehicle_task = _vehicle_tasks[vehicle_tasks_offset];
208 2495953403 : const Task &common_task = _common_tasks[common_tasks_offset];
209 2495953403 : if (vehicle_task.priority <= common_task.priority) {
210 2198469542 : run_vehicle_task = true;
211 : }
212 3418602988 : } else if (vehicle_tasks_offset < _num_vehicle_tasks) {
213 : // out of common tasks to run
214 0 : run_vehicle_task = true;
215 922649585 : } else if (common_tasks_offset < _num_common_tasks) {
216 : // out of vehicle tasks to run
217 922649585 : run_vehicle_task = false;
218 : } else {
219 : // this is an error; the outside loop should have terminated
220 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
221 0 : break;
222 : }
223 :
224 3418602988 : const AP_Scheduler::Task &task = run_vehicle_task ? _vehicle_tasks[vehicle_tasks_offset] : _common_tasks[common_tasks_offset];
225 3418602988 : if (run_vehicle_task) {
226 2198469542 : vehicle_tasks_offset++;
227 : } else {
228 1220133446 : common_tasks_offset++;
229 : }
230 :
231 3418602988 : if (task.priority > MAX_FAST_TASK_PRIORITIES) {
232 2887601052 : const uint16_t dt = _tick_counter - _last_run[i];
233 : // we allow 0 to mean loop rate
234 2887601052 : uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz);
235 2887601052 : if (interval_ticks < 1) {
236 58891883 : interval_ticks = 1;
237 : }
238 2887601052 : if (dt < interval_ticks) {
239 : // this task is not yet scheduled to run again
240 2285581515 : continue;
241 : }
242 : // this task is due to run. Do we have enough time to run it?
243 602019537 : _task_time_allowed = task.max_time_micros;
244 :
245 602019537 : if (dt >= interval_ticks*2) {
246 2158 : perf_info.task_slipped(i);
247 : }
248 :
249 602019537 : if (dt >= interval_ticks*max_task_slowdown) {
250 : // we are going beyond the maximum slowdown factor for a
251 : // task. This will trigger increasing the time budget
252 0 : task_not_achieved++;
253 : }
254 :
255 602019537 : if (_task_time_allowed > time_available) {
256 : // not enough time to run this task. Continue loop -
257 : // maybe another task will fit into time remaining
258 2814 : continue;
259 : }
260 : } else {
261 531001936 : _task_time_allowed = get_loop_period_us();
262 : }
263 :
264 : // run it
265 1133018659 : _task_time_started = now;
266 1133018659 : hal.util->persistent_data.scheduler_task = i;
267 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
268 1133018659 : fill_nanf_stack();
269 : #endif
270 1133018659 : task.function();
271 1133017028 : hal.util->persistent_data.scheduler_task = -1;
272 :
273 : // record the tick counter when we ran. This drives
274 : // when we next run the event
275 1133017028 : _last_run[i] = _tick_counter;
276 :
277 : // work out how long the event actually took
278 1133017028 : now = AP_HAL::micros();
279 1133017028 : uint32_t time_taken = now - _task_time_started;
280 1133017028 : bool overrun = false;
281 1133017028 : if (time_taken > _task_time_allowed) {
282 223 : overrun = true;
283 : // the event overran!
284 223 : debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n",
285 : (unsigned)i,
286 : task.name,
287 : (unsigned)time_taken,
288 : (unsigned)_task_time_allowed);
289 : }
290 :
291 1133017028 : perf_info.update_task_info(i, time_taken, overrun);
292 :
293 1133017028 : if (time_taken >= time_available) {
294 : /*
295 : we are out of time, but we need to keep walking the task
296 : table in case there is another fast loop task after this
297 : task, plus we need to update the accouting so we can
298 : work out if we need to allocate extra time for the loop
299 : (lower the loop rate)
300 : Just set time_available to zero, which means we will
301 : only run fast tasks after this one
302 : */
303 214 : time_available = 0;
304 : } else {
305 1133016814 : time_available -= time_taken;
306 : }
307 : }
308 :
309 : // update number of spare microseconds
310 41684622 : _spare_micros += time_available;
311 :
312 41684622 : _spare_ticks++;
313 41684622 : if (_spare_ticks == 32) {
314 2602209 : _spare_ticks /= 2;
315 2602209 : _spare_micros /= 2;
316 : }
317 41684622 : }
318 :
319 : /*
320 : return number of micros until the current task reaches its deadline
321 : */
322 186924818 : uint16_t AP_Scheduler::time_available_usec(void) const
323 : {
324 186924818 : uint32_t dt = AP_HAL::micros() - _task_time_started;
325 186924818 : if (dt > _task_time_allowed) {
326 99 : return 0;
327 : }
328 186924719 : return _task_time_allowed - dt;
329 : }
330 :
331 : /*
332 : calculate load average as a number from 0 to 1
333 : */
334 1135604 : float AP_Scheduler::load_average()
335 : {
336 : // return 1 if filtered main loop rate is 5% below the configured rate
337 1135604 : if (get_filtered_loop_rate_hz() < get_loop_rate_hz() * 0.95) {
338 222 : return 1.0;
339 : }
340 1135382 : if (_spare_ticks == 0) {
341 717 : return 0.0f;
342 : }
343 1134665 : const uint32_t loop_us = get_loop_period_us();
344 1134665 : const uint32_t used_time = loop_us - (_spare_micros/_spare_ticks);
345 1134665 : return constrain_float(used_time / (float)loop_us, 0, 1);
346 : }
347 :
348 41686254 : void AP_Scheduler::loop()
349 : {
350 : // wait for an INS sample
351 41686254 : hal.util->persistent_data.scheduler_task = -3;
352 41686254 : _rsem.give();
353 41686254 : AP::ins().wait_for_sample();
354 41686253 : _rsem.take_blocking();
355 41686253 : hal.util->persistent_data.scheduler_task = -1;
356 :
357 41686253 : _loop_sample_time_us = AP_HAL::micros64();
358 41686253 : const uint32_t sample_time_us = uint32_t(_loop_sample_time_us);
359 :
360 41686253 : if (_loop_timer_start_us == 0) {
361 2086 : _loop_timer_start_us = sample_time_us;
362 2086 : _last_loop_time_s = get_loop_period_s();
363 : } else {
364 41684167 : _last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6;
365 : }
366 :
367 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
368 : {
369 : /*
370 : for testing low CPU conditions we can add an optional delay in SITL
371 : */
372 41686253 : auto *sitl = AP::sitl();
373 41686253 : uint32_t loop_delay_us = sitl? sitl->loop_delay.get() : 1000U;
374 41686253 : hal.scheduler->delay_microseconds(loop_delay_us);
375 : }
376 : #endif
377 :
378 : // tell the scheduler one tick has passed
379 41686253 : tick();
380 :
381 : // run all the tasks that are due to run. Note that we only
382 : // have to call this once per loop, as the tasks are scheduled
383 : // in multiples of the main loop tick. So if they don't run on
384 : // the first call to the scheduler they won't run on a later
385 : // call until scheduler.tick() is called again
386 41686253 : const uint32_t loop_us = get_loop_period_us();
387 41686253 : uint32_t now = AP_HAL::micros();
388 41686253 : uint32_t time_available = 0;
389 41686253 : const uint32_t loop_tick_us = now - sample_time_us;
390 41686253 : if (loop_tick_us < loop_us) {
391 : // get remaining time available for this loop
392 41686253 : time_available = loop_us - loop_tick_us;
393 : }
394 :
395 : // add in extra loop time determined by not achieving scheduler tasks
396 41686253 : time_available += extra_loop_us;
397 :
398 : // run the tasks
399 41686253 : run(time_available);
400 :
401 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
402 : // move result of AP_HAL::micros() forward:
403 41684622 : hal.scheduler->delay_microseconds(1);
404 : #endif
405 :
406 41684622 : if (task_not_achieved > 0) {
407 : // add some extra time to the budget
408 0 : extra_loop_us = MIN(extra_loop_us+100U, 5000U);
409 0 : task_not_achieved = 0;
410 0 : task_all_achieved = 0;
411 41684622 : } else if (extra_loop_us > 0) {
412 0 : task_all_achieved++;
413 0 : if (task_all_achieved > 50) {
414 : // we have gone through 50 loops without a task taking too
415 : // long. CPU pressure has eased, so drop the extra time we're
416 : // giving each loop
417 0 : task_all_achieved = 0;
418 : // we are achieving all tasks, slowly lower the extra loop time
419 0 : extra_loop_us = MAX(0U, extra_loop_us-50U);
420 : }
421 : }
422 :
423 : // check loop time
424 41684622 : perf_info.check_loop_time(sample_time_us - _loop_timer_start_us);
425 :
426 41684622 : _loop_timer_start_us = sample_time_us;
427 :
428 : #if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
429 : hal.simstate->update();
430 : #endif
431 41684622 : }
432 :
433 : #if HAL_LOGGING_ENABLED
434 19414 : void AP_Scheduler::update_logging()
435 : {
436 19414 : if (debug_flags()) {
437 0 : perf_info.update_logging();
438 : }
439 38828 : if (_log_performance_bit != (uint32_t)-1 &&
440 19414 : AP::logger().should_log(_log_performance_bit)) {
441 18657 : Log_Write_Performance();
442 : }
443 19414 : perf_info.set_loop_rate(get_loop_rate_hz());
444 19414 : perf_info.reset();
445 : // dynamically update the per-task perf counter
446 19414 : if (!(_options & uint8_t(Options::RECORD_TASK_INFO)) && perf_info.has_task_info()) {
447 1 : perf_info.free_task_info();
448 19413 : } else if ((_options & uint8_t(Options::RECORD_TASK_INFO)) && !perf_info.has_task_info()) {
449 1 : perf_info.allocate_task_info(_num_tasks);
450 : }
451 19414 : }
452 :
453 : // Write a performance monitoring packet
454 18657 : void AP_Scheduler::Log_Write_Performance()
455 : {
456 18657 : uint64_t rtc = 0;
457 : #if AP_RTC_ENABLED
458 18657 : UNUSED_RESULT(AP::rtc().get_utc_usec(rtc));
459 : #endif
460 :
461 18657 : const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
462 18657 : struct log_Performance pkt = {
463 : LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
464 18657 : time_us : AP_HAL::micros64(),
465 37314 : loop_rate : (uint16_t)(get_filtered_loop_rate_hz() + 0.5f),
466 18657 : num_long_running : perf_info.get_num_long_running(),
467 18657 : num_loops : perf_info.get_num_loops(),
468 18657 : max_time : perf_info.get_max_time(),
469 18657 : mem_avail : hal.util->available_memory(),
470 37314 : load : (uint16_t)(load_average() * 1000),
471 18657 : internal_error_last_line : AP::internalerror().last_error_line(),
472 18657 : internal_errors : AP::internalerror().errors(),
473 18657 : internal_error_count : AP::internalerror().count(),
474 18657 : spi_count : pd.spi_count,
475 18657 : i2c_count : pd.i2c_count,
476 18657 : i2c_isr_count : pd.i2c_isr_count,
477 18657 : extra_loop_us : extra_loop_us,
478 : rtc : rtc,
479 55971 : };
480 18657 : AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
481 18657 : }
482 : #endif // HAL_LOGGING_ENABLED
483 :
484 : // display task statistics as text buffer for @SYS/tasks.txt
485 2 : void AP_Scheduler::task_info(ExpandingString &str)
486 : {
487 : // a header to allow for machine parsers to determine format
488 2 : str.printf("TasksV2\n");
489 :
490 : // dynamically enable statistics collection
491 2 : if (!(_options & uint8_t(Options::RECORD_TASK_INFO))) {
492 0 : _options.set(_options | uint8_t(Options::RECORD_TASK_INFO));
493 0 : return;
494 : }
495 :
496 2 : if (perf_info.get_task_info(0) == nullptr) {
497 0 : return;
498 : }
499 :
500 : // baseline the total time taken by all tasks
501 2 : float total_time = 1.0f;
502 174 : for (uint8_t i = 0; i < _num_tasks + 1; i++) {
503 172 : const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i);
504 172 : if (ti != nullptr && ti->tick_count > 0) {
505 170 : total_time += ti->elapsed_time_us;
506 : }
507 : }
508 :
509 2 : uint8_t vehicle_tasks_offset = 0;
510 2 : uint8_t common_tasks_offset = 0;
511 :
512 172 : for (uint8_t i = 0; i < _num_tasks; i++) {
513 170 : const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i);
514 : const char *task_name;
515 :
516 : // determine which of the common task / vehicle task to run
517 170 : bool run_vehicle_task = false;
518 170 : if (vehicle_tasks_offset < _num_vehicle_tasks &&
519 128 : common_tasks_offset < _num_common_tasks) {
520 : // still have entries on both lists; compare the
521 : // priorities. In case of a tie the vehicle-specific
522 : // entry wins.
523 128 : const Task &vehicle_task = _vehicle_tasks[vehicle_tasks_offset];
524 128 : const Task &common_task = _common_tasks[common_tasks_offset];
525 128 : if (vehicle_task.priority <= common_task.priority) {
526 112 : run_vehicle_task = true;
527 : }
528 170 : } else if (vehicle_tasks_offset < _num_vehicle_tasks) {
529 : // out of common tasks to run
530 0 : run_vehicle_task = true;
531 42 : } else if (common_tasks_offset < _num_common_tasks) {
532 : // out of vehicle tasks to run
533 42 : run_vehicle_task = false;
534 : } else {
535 : // this is an error; the outside loop should have terminated
536 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
537 0 : return;
538 : }
539 :
540 170 : if (run_vehicle_task) {
541 112 : task_name = _vehicle_tasks[vehicle_tasks_offset++].name;
542 : } else {
543 58 : task_name = _common_tasks[common_tasks_offset++].name;
544 : }
545 :
546 170 : ti->print(task_name, total_time, str);
547 : }
548 : }
549 :
550 : namespace AP {
551 :
552 624451872 : AP_Scheduler &scheduler()
553 : {
554 624451872 : return *AP_Scheduler::get_singleton();
555 : }
556 :
557 : };
558 :
559 : #endif // AP_SCHEDULER_ENABLED
|
