From c0ad98be312f63ff100b99bab769fc9ad3305421 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Tue, 9 Oct 2012 12:30:17 +0900
Subject: [PATCH] ArduCopter: failsafe added to shutdown motors if mainloop
 fails

---
 ArduCopter/ArduCopter.pde |  5 ++++
 ArduCopter/failsafe.pde   | 59 +++++++++++++++++++++++++++++++++++++++
 ArduCopter/motors.pde     | 10 ++++++-
 ArduCopter/system.pde     |  6 ++++
 4 files changed, 79 insertions(+), 1 deletion(-)
 create mode 100644 ArduCopter/failsafe.pde

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 43c5118071..16510e32cc 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -911,6 +911,8 @@ static byte medium_loopCounter;
 static byte slow_loopCounter;
 // Counters for branching at 1 hz
 static byte counter_one_herz;
+// Counter of main loop executions.  Used for performance monitoring and failsafe processing
+static uint16_t mainLoop_count;
 // used to track the elapsed time between GPS reads
 static uint32_t nav_loopTimer;
 // Delta Time in milliseconds for navigation computations, updated with every good GPS read
@@ -1002,6 +1004,9 @@ void loop()
         G_Dt                            = (float)(timer - fast_loopTimer) / 1000000.f;                  // used by PI Loops
         fast_loopTimer          = timer;
 
+        // for mainloop failure monitoring
+        mainLoop_count++;
+
         // Execute the fast loop
         // ---------------------
         fast_loop();////
diff --git a/ArduCopter/failsafe.pde b/ArduCopter/failsafe.pde
new file mode 100644
index 0000000000..e53bd16b5b
--- /dev/null
+++ b/ArduCopter/failsafe.pde
@@ -0,0 +1,59 @@
+// -*- 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 disarm the motors
+//
+
+static bool failsafe_enabled = true;
+static uint16_t failsafe_last_mainLoop_count;
+static uint32_t failsafe_last_timestamp;
+static bool in_failsafe;
+
+//
+// failsafe_enable - enable failsafe
+//
+void failsafe_enable()
+{
+    failsafe_enabled = true;
+    failsafe_last_timestamp = micros();
+}
+
+//
+// failsafe_disable - used when we know we are going to delay the mainloop significantly
+//
+void failsafe_disable()
+{
+    failsafe_enabled = false;
+}
+ 
+//
+//  failsafe_check - this function is called from the core timer interrupt at 1kHz.
+//
+void failsafe_check(uint32_t tnow)
+{
+    if (mainLoop_count != failsafe_last_mainLoop_count) {
+        // the main loop is running, all is OK
+        failsafe_last_mainLoop_count = mainLoop_count;
+        failsafe_last_timestamp = tnow;
+        in_failsafe = false;
+        return;
+    }
+
+    if (failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
+        // motors are running but we have gone 2 second since the 
+        // main loop ran. That means we're in trouble and should
+        // disarm the motors.
+        in_failsafe = true;
+    }
+
+    if (in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
+        // disarm motors every second
+        failsafe_last_timestamp = tnow;
+        if( motors.armed() ) {
+            motors.armed(false);
+            motors.output();
+        }
+    }
+}
\ No newline at end of file
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 58a8ed503b..236c3c2294 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -106,6 +106,9 @@ static void init_arm_motors()
     // which calibrates the IMU
     static bool did_ground_start = false;
 
+    // disable failsafe because initialising everything takes a while
+    failsafe_disable();
+
     //Serial.printf("\nARM\n");
 #if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
     gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
@@ -118,7 +121,6 @@ static void init_arm_motors()
     if (gcs3.initialised) {
         Serial3.set_blocking_writes(false);
     }
-    motors.armed(true);
 
 #if COPTER_LEDS == ENABLED
     if ( bitRead(g.copter_leds_mode, 3) ) {
@@ -164,6 +166,12 @@ static void init_arm_motors()
 #if SECONDARY_DMP_ENABLED == ENABLED
     ahrs2.set_fast_gains(false);
 #endif
+
+    // finally actually arm the motors
+    motors.armed(true);
+
+    // reenable failsafe
+    failsafe_enable();
 }
 
 
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 4405d7741f..53ae53f1ba 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -206,6 +206,12 @@ static void init_ardupilot()
 
     timer_scheduler.init( &isr_registry );
 
+    /*
+     *  setup the 'main loop is dead' check. Note that this relies on
+     *  the RC library being initialised.
+     */
+    timer_scheduler.set_failsafe(failsafe_check);
+
     // initialise the analog port reader
     AP_AnalogSource_Arduino::init_timer(&timer_scheduler);