From 9656b0ad0e8a6d9ec51515645a76320f9d5ae808 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Mon, 17 Dec 2012 16:25:44 -0800
Subject: [PATCH] AP_HAL_AVR: add panic method to scheduler

---
 libraries/AP_HAL_AVR/Scheduler.cpp | 11 +++++++++++
 libraries/AP_HAL_AVR/Scheduler.h   |  1 +
 2 files changed, 12 insertions(+)

diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp
index a9e18e528a..99beb7f4a6 100644
--- a/libraries/AP_HAL_AVR/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR/Scheduler.cpp
@@ -210,6 +210,17 @@ void AVRScheduler::end_atomic() {
     }
 }
 
+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;
+    /* Print the error message on both ports */
+    hal.uartA->println_P(errormsg);
+    hal.uartC->println_P(errormsg);
+    /* Spin forever. */
+    for(;;);
+}
+
 void AVRScheduler::reboot() {
     hal.uartA->println_P(PSTR("GOING DOWN FOR A REBOOT\r\n"));
     hal.scheduler->delay(100);
diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h
index a34374d530..f557ee83c7 100644
--- a/libraries/AP_HAL_AVR/Scheduler.h
+++ b/libraries/AP_HAL_AVR/Scheduler.h
@@ -37,6 +37,7 @@ public:
     void     resume_timer_procs();
     void     begin_atomic();
     void     end_atomic();
+    void     panic(const prog_char_t *errormsg);
     void     reboot();
 
 private: