From 29fa78214b170cbe215a833ef5a1c7bdde820287 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 21 Dec 2011 23:25:51 +1100
Subject: [PATCH] APM: added failsafe support

we now pass all channels through at 50Hz if the main loop stops
running, regardless of why. This gives us a reasonable failsafe, as
long as the low level RC library is still working, and interrupts
don't get completely disabled.
---
 ArduPlane/failsafe.pde | 45 ++++++++++++++++++++++++++++++++++++++++++
 ArduPlane/system.pde   |  6 ++++--
 2 files changed, 49 insertions(+), 2 deletions(-)
 create mode 100644 ArduPlane/failsafe.pde

diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde
new file mode 100644
index 0000000000..7b300800f7
--- /dev/null
+++ b/ArduPlane/failsafe.pde
@@ -0,0 +1,45 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+  failsafe support
+  Andrew Tridgell, December 2011
+ */
+
+/*
+  our failsafe strategy is to detect main loop lockup and switch to
+  passing inputs straight from the RC inputs to RC outputs.
+ */
+
+/*
+  this failsafe_check function is called from the core timer interrupt
+  at 1kHz.
+ */
+void failsafe_check(uint32_t tnow)
+{
+    static uint16_t last_mainLoop_count;
+    static uint32_t last_timestamp;
+    static bool in_failsafe;
+
+    if (mainLoop_count != last_mainLoop_count) {
+        // the main loop is running, all is OK
+        last_mainLoop_count = mainLoop_count;
+        last_timestamp = tnow;
+        in_failsafe = false;
+        return;
+    }
+
+    if (tnow - last_timestamp > 200000) {
+        // we have gone at least 0.2 seconds since the main loop
+        // ran. That means we're in trouble, or perhaps are in 
+        // an initialisation routine or log erase. Start passing RC
+        // inputs through to outputs
+        in_failsafe = true;
+    }
+
+    if (in_failsafe && tnow - last_timestamp > 20000) {
+        // pass RC inputs to outputs every 20ms        
+        last_timestamp = tnow;
+        for (uint8_t ch=0; ch<8; ch++) {
+            APM_RC.OutputCh(ch, APM_RC.InputCh(ch));
+        }
+    }
+}
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 638f7b8de7..1e98d005b6 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -46,6 +46,9 @@ MENU(main_menu, THISFIRMWARE, main_menu_commands);
 // the user wants the CLI. It never exits
 static void run_cli(void)
 {
+    // disable the failsafe code in the CLI
+    timer_scheduler.set_failsafe(NULL);
+
     while (1) {
         main_menu.run();
     }
@@ -119,9 +122,8 @@ static void init_ardupilot()
 	// Initialize the timer scheduler to use the ISR registry.
 	//
 
-#if HIL_MODE != HIL_MODE_ATTITUDE
     timer_scheduler.init( & isr_registry );
-#endif
+    timer_scheduler.set_failsafe(failsafe_check);
 
 	//
 	// Check the EEPROM format version before loading any parameters from EEPROM.