Copter: GPS Failsafe implemented

Switches to LAND mode 5 seconds after losing GPS if you're in a flight
mode that requires a GPS

ArduCopter/AP_State.pde

@@ -84,6 +84,13 @@ void set_low_battery(bool b)
     ap.low_battery = b;
 }
 
+
+// ---------------------------------------------
+static void set_failsafe_gps(bool mode)
+{
+    ap.failsafe_gps = mode;
+}
+
 // ---------------------------------------------
 void set_takeoff_complete(bool b)
 {

ArduCopter/ArduCopter.pde

@@ -1428,6 +1428,9 @@ static void update_GPS(void)
         // save GPS time so we don't get duplicate reads
         last_gps_time = g_gps->time;
     }
+
+    // check for loss of gps
+    failsafe_gps_check();
 }
 
 // set_yaw_mode - update yaw mode and initialise any variables required

ArduCopter/Parameters.h

@@ -189,7 +189,8 @@ public:
         k_param_radio_tuning_low,
         k_param_rc_speed = 192,
         k_param_failsafe_battery_enabled,
-        k_param_throttle_mid, // 194
+        k_param_throttle_mid,
+        k_param_failsafe_gps_enabled,  // 195
 
         //
         // 200: flight modes
@@ -265,6 +266,7 @@ public:
     AP_Float        curr_amp_per_volt;
     AP_Int16        pack_capacity;              // Battery pack capacity less reserve
     AP_Int8         failsafe_battery_enabled;   // battery failsafe enabled
+    AP_Int8         failsafe_gps_enabled;       // gps failsafe enabled
 
     AP_Int8         compass_enabled;
     AP_Int8         optflow_enabled;

ArduCopter/Parameters.pde

@@ -91,6 +91,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY),
 
+    // @Param: FS_GPS_ENABLE
+    // @DisplayName: GPS Failsafe Enable
+    // @Description: Controls whether failsafe will be invoked when gps signal is lost
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS),
+
     // @Param: VOLT_DIVIDER
     // @DisplayName: Voltage Divider
     // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin voltage * INPUT_VOLTS/1024 * VOLT_DIVIDER)

ArduCopter/config.h

@@ -397,6 +397,13 @@
  # define FS_BATTERY              DISABLED
 #endif
 
+// GPS failsafe
+#ifndef FS_GPS
+ # define FS_GPS                        DISABLED
+#endif
+#ifndef FAILSAFE_GPS_TIMEOUT_MS
+ # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 //  MAGNETOMETER

ArduCopter/events.pde

@@ -104,6 +104,73 @@ static void low_battery_event(void)
 #endif // COPTER_LEDS
 }
 
+// failsafe_gps_check - check for gps failsafe
+static void failsafe_gps_check()
+{
+    uint32_t last_gps_update_ms;
+
+    // return immediately if gps failsafe is disabled
+    if( !g.failsafe_gps_enabled ) {
+        return;
+    }
+
+    // calc time since last gps update
+    last_gps_update_ms = millis() - g_gps->last_fix_time;
+
+    // check if all is well
+    if( last_gps_update_ms < FAILSAFE_GPS_TIMEOUT_MS) {
+        // check for recovery from gps failsafe
+        if( ap.failsafe_gps ) {
+            failsafe_gps_off_event();
+            set_failsafe_gps(false);
+        }
+        return;
+    }
+
+    // do nothing if gps failsafe already triggered or motors disarmed
+    if( ap.failsafe_gps || !motors.armed()) {
+        return;
+    }
+
+    // GPS failsafe event has occured
+    // update state, warn the ground station and log to dataflash
+    set_failsafe_gps(true);
+    gcs_send_text_P(SEVERITY_LOW,PSTR("Lost GPS!"));
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
+
+    // take action based on flight mode
+    switch(control_mode) {
+        // for modes that do not require gps, do nothing
+        case STABILIZE:
+        case ACRO:
+        case ALT_HOLD:
+        case OF_LOITER:
+            // do nothing
+            break;
+
+        // modes requiring GPS force a land
+        case AUTO:
+        case GUIDED:
+        case LOITER:
+        case RTL:
+        case CIRCLE:
+        case POSITION:
+            // We have no GPS or are very close to home so we will land
+            set_mode(LAND);
+            break;
+
+        case LAND:
+            // if we're already landing do nothing
+            break;
+    }
+}
+
+// failsafe_gps_off_event - actions to take when GPS contact is restored
+static void failsafe_gps_off_event(void)
+{
+    // log recovery of GPS in logs?
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED);
+}
 
 static void update_events()     // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
 {

ArduCopter/navigation.pde

@@ -24,15 +24,6 @@ static void update_navigation()
             Log_Write_Nav_Tuning();
         }
     }
-
-    // To-Do: replace below with proper GPS failsafe
-    // reduce nav outputs to zero if we have not seen a position update in 2 seconds
-    if( millis() - nav_last_update > 2000 ) {
-        // after 12 reads we guess we may have lost GPS signal, stop navigating
-        // we have lost GPS signal for a moment. Reduce our error to avoid flyaways
-        auto_roll  >>= 1;
-        auto_pitch >>= 1;
-    }
 }
 
 // run_nav_updates - top level call for the autopilot