diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp index c5de2f0c87..7321dd2e9c 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp @@ -15,6 +15,8 @@ #include "Scheduler.h" +#include + #define millis libmaple_millis #define micros libmaple_micros #include "FlymapleWirish.h" @@ -232,8 +234,15 @@ void FLYMAPLEScheduler::panic(const prog_char_t *errormsg, ...) { /* Suspend timer processes. We still want the timer event to go off * to run the _failsafe code, however. */ // REVISIT: not tested on FLYMAPLE + va_list ap; + _timer_suspended = true; - hal.console->println_P(errormsg); + + va_start(ap, errormsg); + hal.console->vprintf_P(errormsg, ap); + va_end(ap); + hal.console->printf_P("\n"); + for(;;); }