ArduCopter: failsafe improvements

resolves momentary throttle drop to zero before failsafe engages
resolves motor cut after 30seconds if flying in stabilize without GPS (now switches to ALT_HOLD with target altitude zero)
disables motors if throttle was zero before failsafe event

ArduCopter/config.h

@@ -427,9 +427,13 @@
 #ifndef THROTTE_FS_VALUE
  # define THROTTLE_FS_VALUE                      975
 #endif
+
+#define THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL         1
+#define THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION   2
 #ifndef THROTTLE_FAILSAFE_ACTION
- # define THROTTLE_FAILSAFE_ACTION       2
+ # define THROTTLE_FAILSAFE_ACTION                  THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION
 #endif
+
 #ifndef MINIMUM_THROTTLE
  # define MINIMUM_THROTTLE       130
 #endif

ArduCopter/events.pde

@@ -7,59 +7,53 @@
 static void failsafe_on_event()
 {
     // This is how to handle a failsafe.
-    switch(control_mode)
+    switch(control_mode) {
-    {
+        case STABILIZE:
-    case AUTO:
+        case ACRO:
-        if (g.throttle_fs_action == 1) {
+            // if throttle is zero disarm motors
-            // do_rtl sets the altitude to the current altitude by default
+            if (g.rc_3.control_in == 0) {
+                init_disarm_motors();
+            }else if(ap.home_is_set == true) {
                 set_mode(RTL);
-            // We add an additional 10m to the current altitude
+            }else{
-            //next_WP.alt += 1000;
+                // We have no GPS so we will crash land in alt hold mode
-            set_new_altitude(next_WP.alt + 1000);
+                // the low throttle will bring us down at the maximum
+                // the low throttle will bring us down at the maximum descent speed
+                // To-Do: make LAND a throttle mode so it can operate without GPS
+                set_mode(ALT_HOLD);
+                set_new_altitude(0);
             }
-        //if (g.throttle_fs_action == 2)
-        //  Stay in AUTO and ignore failsafe
             break;
-
+        case AUTO:
+            // throttle_fs_action is 1 do RTL, everything else means continue with the mission
+            if (g.throttle_fs_action == THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL) {
+                set_mode(RTL);
+            }
+            // if throttle_fs_action is 2 (i.e. THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION) no need to do anything
+            break;
         default:
-        if(ap.home_is_set) {
+            if(ap.home_is_set == true) {
                 // same as above ^
                 // do_rtl sets the altitude to the current altitude by default
                 set_mode(RTL);
-            // We add an additional 10m to the current altitude
-            //next_WP.alt += 1000;
-            set_new_altitude(next_WP.alt + 1000);
             }else{
-            // We have no GPS so we must land
+                // We have no GPS so we will crash land in alt hold mode
-            set_mode(LAND);
+                // the low throttle will bring us down at the maximum descent speed
+                // To-Do: make LAND a throttle mode so it can operate without GPS
+                set_mode(ALT_HOLD);
+                set_new_altitude(0);
             }
             break;
     }
 }
 
+// failsafe_off_event - respond to radio contact being regained
+// we must be in AUTO, LAND or RTL modes
+// or Stabilize or ACRO mode but with motors disarmed
 static void failsafe_off_event()
 {
-    // If we are in AUTO, no need to do anything
+    // no need to do anything
-    if(control_mode == AUTO)
+    // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
-        return;
-
-    if (g.throttle_fs_action == 2) {
-        // We're back in radio contact
-        // return to AP
-        // ---------------------------
-
-        // re-read the switch so we can return to our preferred mode
-        // --------------------------------------------------------
-        reset_control_switch();
-
-
-    }else if (g.throttle_fs_action == 1) {
-        // We're back in radio contact
-        // return to Home
-        // we should already be in RTL and throttle set to cruise
-        // ------------------------------------------------------
-        set_mode(RTL);
-    }
 }
 
 static void low_battery_event(void)

ArduCopter/radio.pde

@@ -2,8 +2,6 @@
 
 // Function that will read the radio data, limit servos and trigger a failsafe
 // ----------------------------------------------------------------------------
-static int8_t failsafeCounter = 0;              // we wait a second to take over the throttle and send the plane circling
-
 
 extern RC_Channel* rc_ch[NUM_CHANNELS];
 
@@ -132,7 +130,9 @@ static void read_radio()
         system.new_radio_frame = true;
         g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
         g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
-        g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
+
+        set_throttle_and_failsafe(APM_RC.InputCh(CH_3));
+
         g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
         g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
         g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
@@ -143,55 +143,51 @@ static void read_radio()
         // limit our input to 800 so we can still pitch and roll
         g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE);
 #endif
+    }
+}
 
-        throttle_failsafe(g.rc_3.radio_in);
-    }
-}
 #define FS_COUNTER 3
-static void throttle_failsafe(uint16_t pwm)
+static void set_throttle_and_failsafe(uint16_t throttle_pwm)
 {
-    // Don't enter Failsafe if not enabled by user
+    static int8_t failsafe_counter = 0;
-    if(g.throttle_fs_enabled == 0)
+
+    // if failsafe not enabled pass through throttle and exit
+    if(g.throttle_fs_enabled == 0) {
+        g.rc_3.set_pwm(throttle_pwm);
         return;
+    }
 
-    //check for failsafe and debounce funky reads
+    //check for low throttle value
-    // ------------------------------------------
+    if (throttle_pwm < (uint16_t)g.throttle_fs_value) {
-    if (pwm < (unsigned)g.throttle_fs_value) {
+
-        // we detect a failsafe from radio
+        // if we are already in failsafe or motors not armed pass through throttle and exit
-        // throttle has dropped below the mark
+        if (ap.failsafe || !motors.armed()) {
-        failsafeCounter++;
+            g.rc_3.set_pwm(throttle_pwm);
-        if (failsafeCounter == FS_COUNTER-1) {
+            return;
-            // called right before trigger
+        }
-            // do nothing
+
-        }else if(failsafeCounter == FS_COUNTER) {
+        // check for 3 low throttle values
-            // Don't enter Failsafe if we are not armed
+        // Note: we do not pass through the low throttle until 3 low throttle values are recieved
-            // home distance is in meters
+        failsafe_counter++;
-            // This is to prevent accidental RTL
+        if( failsafe_counter >= FS_COUNTER ) {
-            if(motors.armed() && ap.takeoff_complete) {
+            failsafe_counter = FS_COUNTER;  // check to ensure we don't overflow the counter
-                Serial.print_P(PSTR("MSG FS ON "));
-                Serial.println(pwm, DEC);
             set_failsafe(true);
+            g.rc_3.set_pwm(throttle_pwm);   // pass through failsafe throttle
         }
-        }else if (failsafeCounter > FS_COUNTER) {
+    }else{
-            failsafeCounter = FS_COUNTER+1;
+        // we have a good throttle so reduce failsafe counter
-        }
+        failsafe_counter--;
+        if( failsafe_counter <= 0 ) {
+            failsafe_counter = 0;   // check to ensure we don't underflow the counter
 
-    }else if(failsafeCounter > 0) {
+            // disengage failsafe after three (nearly) consecutive valid throttle values
-        // we are no longer in failsafe condition
+            if (ap.failsafe) {
-        // but we need to recover quickly
-        failsafeCounter--;
-        if (failsafeCounter > 3) {
-            failsafeCounter = 3;
-        }
-        if (failsafeCounter == 1) {
-            Serial.print_P(PSTR("MSG FS OFF "));
-            Serial.println(pwm, DEC);
-        }else if(failsafeCounter == 0) {
                 set_failsafe(false);
-        }else if (failsafeCounter <0) {
-            failsafeCounter = -1;
             }
         }
+        // pass through throttle
+        g.rc_3.set_pwm(throttle_pwm);
+    }
 }
 
 static void trim_radio()