AP_Scheduler: use fill_nanf() on each scheduler function

This commit is contained in:
Andrew Tridgell 2019-10-01 16:55:48 +10:00
parent cbe34320b5
commit ddde512b74

View File

@ -114,6 +114,18 @@ void AP_Scheduler::tick(void)
_tick_counter++; _tick_counter++;
} }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
/*
fill stack with NaN so we can catch use of uninitialised stack
variables in SITL
*/
static void fill_nanf_stack(void)
{
float v[1024];
fill_nanf(v, ARRAY_SIZE(v));
}
#endif
/* /*
run one tick run one tick
this will run as many scheduler tasks as we can in the specified time this will run as many scheduler tasks as we can in the specified time
@ -173,6 +185,9 @@ void AP_Scheduler::run(uint32_t time_available)
if (_debug > 1 && _perf_counters && _perf_counters[i]) { if (_debug > 1 && _perf_counters && _perf_counters[i]) {
hal.util->perf_begin(_perf_counters[i]); hal.util->perf_begin(_perf_counters[i]);
} }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
fill_nanf_stack();
#endif
_tasks[i].function(); _tasks[i].function();
if (_debug > 1 && _perf_counters && _perf_counters[i]) { if (_debug > 1 && _perf_counters && _perf_counters[i]) {
hal.util->perf_end(_perf_counters[i]); hal.util->perf_end(_perf_counters[i]);