diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index 0343cd2964..48db386fdb 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/libraries/AP_HAL/Scheduler.h @@ -56,7 +56,7 @@ public: virtual bool system_initializing() = 0; virtual void system_initialized() = 0; - virtual void panic(const prog_char_t *errormsg) NORETURN = 0; + virtual void panic(const prog_char_t *errormsg, ...) NORETURN = 0; virtual void reboot(bool hold_in_bootloader) = 0; /** diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index cf922340ad..f746b2e382 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -225,7 +225,7 @@ void AVRScheduler::system_initialized() { _initialized = true; } -void AVRScheduler::panic(const prog_char_t* errormsg) { +void AVRScheduler::panic(const prog_char_t* errormsg, ...) { /* Suspend timer processes. We still want the timer event to go off * to run the _failsafe code, however. */ _timer_suspended = true; diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index 8aadff6236..eca8d71faf 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -45,7 +45,7 @@ public: bool system_initializing(); void system_initialized(); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; void reboot(bool hold_in_bootloader); void set_timer_speed(uint16_t timer_hz); diff --git a/libraries/AP_HAL_Empty/Scheduler.cpp b/libraries/AP_HAL_Empty/Scheduler.cpp index c12026c488..04a287dc7d 100644 --- a/libraries/AP_HAL_Empty/Scheduler.cpp +++ b/libraries/AP_HAL_Empty/Scheduler.cpp @@ -69,7 +69,7 @@ bool EmptyScheduler::system_initializing() { void EmptyScheduler::system_initialized() {} -void EmptyScheduler::panic(const prog_char_t *errormsg) { +void EmptyScheduler::panic(const prog_char_t *errormsg, ...) { hal.console->println_P(errormsg); for(;;); } diff --git a/libraries/AP_HAL_Empty/Scheduler.h b/libraries/AP_HAL_Empty/Scheduler.h index 42d30faea0..f56f338439 100644 --- a/libraries/AP_HAL_Empty/Scheduler.h +++ b/libraries/AP_HAL_Empty/Scheduler.h @@ -32,7 +32,7 @@ public: bool system_initializing(); void system_initialized(); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; void reboot(bool hold_in_bootloader); }; diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp index 798798e189..c5de2f0c87 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp @@ -228,7 +228,7 @@ void FLYMAPLEScheduler::system_initialized() _initialized = true; } -void FLYMAPLEScheduler::panic(const prog_char_t *errormsg) { +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 diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.h b/libraries/AP_HAL_FLYMAPLE/Scheduler.h index 21afa0c5c7..cc3ee78592 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.h +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.h @@ -53,7 +53,7 @@ public: bool system_initializing(); void system_initialized(); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; void reboot(bool hold_in_bootloader); private: diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 5f170fa293..f40e6be03a 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -407,7 +407,7 @@ void *Scheduler::_io_thread(void* arg) return NULL; } -void Scheduler::panic(const prog_char_t *errormsg) +void Scheduler::panic(const prog_char_t *errormsg, ...) { write(1, errormsg, strlen(errormsg)); write(1, "\n", 1); diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index c38de0440a..2861b6f648 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -43,7 +43,7 @@ public: bool system_initializing(); void system_initialized(); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; void reboot(bool hold_in_bootloader); void stop_clock(uint64_t time_usec); diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index 9d474f1983..ec7041e473 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -374,7 +374,7 @@ void *PX4Scheduler::_storage_thread(void) return NULL; } -void PX4Scheduler::panic(const prog_char_t *errormsg) +void PX4Scheduler::panic(const prog_char_t *errormsg, ...) { write(1, errormsg, strlen(errormsg)); write(1, "\n", 1); diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index 7dc0c33d54..2438487671 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -60,7 +60,7 @@ public: void suspend_timer_procs(); void resume_timer_procs(); void reboot(bool hold_in_bootloader); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; bool in_timerprocess(); bool system_initializing(); diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 37e74b674f..2a643565ab 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -259,7 +259,7 @@ void SITLScheduler::_run_io_procs(bool called_from_isr) _in_io_proc = false; } -void SITLScheduler::panic(const prog_char_t *errormsg) { +void SITLScheduler::panic(const prog_char_t *errormsg, ...) { hal.console->println_P(errormsg); for(;;); } diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index 0c50e6e624..b160c842d9 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -41,7 +41,7 @@ public: void system_initialized(); void reboot(bool hold_in_bootloader); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; bool interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp index e04fb11528..4aecb37527 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp @@ -325,7 +325,7 @@ void *VRBRAINScheduler::_io_thread(void) return NULL; } -void VRBRAINScheduler::panic(const prog_char_t *errormsg) +void VRBRAINScheduler::panic(const prog_char_t *errormsg, ...) { write(1, errormsg, strlen(errormsg)); write(1, "\n", 1); diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.h b/libraries/AP_HAL_VRBRAIN/Scheduler.h index 4ba708652b..6e28dbc63d 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.h @@ -39,7 +39,7 @@ public: void suspend_timer_procs(); void resume_timer_procs(); void reboot(bool hold_in_bootloader); - void panic(const prog_char_t *errormsg) NORETURN; + void panic(const prog_char_t *errormsg, ...) NORETURN; bool in_timerprocess(); bool system_initializing();