From ddde512b748c6dd230d08b007a1b3779b94a06e2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Oct 2019 16:55:48 +1000 Subject: [PATCH] AP_Scheduler: use fill_nanf() on each scheduler function --- libraries/AP_Scheduler/AP_Scheduler.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index eb879f8fc7..273b86862b 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -114,6 +114,18 @@ void AP_Scheduler::tick(void) _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 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]) { hal.util->perf_begin(_perf_counters[i]); } +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + fill_nanf_stack(); +#endif _tasks[i].function(); if (_debug > 1 && _perf_counters && _perf_counters[i]) { hal.util->perf_end(_perf_counters[i]);