Copter: add althold as ekf failsafe action

ArduCopter/Parameters.cpp

@@ -479,6 +479,13 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(land_repositioning, "LAND_REPOSITION",     LAND_REPOSITION_DEFAULT),
 
+    // @Param: FS_EKF_ACTION
+    // @DisplayName: EKF Failsafe Action
+    // @Description: Controls the action that will be taken when an EKF failsafe is invoked
+    // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize
+    // @User: Advanced
+    GSCALAR(fs_ekf_action, "FS_EKF_ACTION",    FS_EKF_ACTION_DEFAULT),
+
     // @Param: FS_EKF_THRESH
     // @DisplayName: EKF failsafe variance threshold
     // @Description: Allows setting the maximum acceptable compass and velocity variance

ArduCopter/Parameters.h

@@ -329,6 +329,7 @@ public:
         k_param_autotune_axis_bitmask,  // 245
         k_param_autotune_aggressiveness,  // 246
         k_param_pi_vel_xy,  // 247
+        k_param_fs_ekf_action, // 248
 
         // 254,255: reserved
     };
@@ -414,6 +415,7 @@ public:
     AP_Int8         arming_check;
 
     AP_Int8         land_repositioning;
+    AP_Int8         fs_ekf_action;
     AP_Float        fs_ekf_thresh;
     AP_Int16        gcs_pid_mask;
 

ArduCopter/config.h

@@ -270,6 +270,9 @@
 
 //////////////////////////////////////////////////////////////////////////////
 //  EKF Failsafe
+#ifndef FS_EKF_ACTION_DEFAULT
+ # define FS_EKF_ACTION_DEFAULT         FS_EKF_ACTION_LAND  // EKF failsafe triggers land by default
+#endif
 #ifndef FS_EKF_THRESHOLD_DEFAULT
  # define FS_EKF_THRESHOLD_DEFAULT      0.8f    // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
 #endif

ArduCopter/defines.h

@@ -392,6 +392,11 @@ enum FlipState {
 #define FS_GPS_ALTHOLD                      2       // switch to ALTHOLD mode on GPS failsafe
 #define FS_GPS_LAND_EVEN_STABILIZE          3       // switch to LAND mode on GPS failsafe even if in a manual flight mode like Stabilize
 
+// EKF failsafe definitions (FS_EKF_ACTION parameter)
+#define FS_EKF_ACTION_LAND                  1       // switch to LAND mode on EKF failsafe
+#define FS_EKF_ACTION_ALTHOLD               2       // switch to ALTHOLD mode on EKF failsafe
+#define FS_EKF_ACTION_LAND_EVEN_STABILIZE   3       // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize
+
 // for mavlink SET_POSITION_TARGET messages
 #define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE      ((1<<0) | (1<<1) | (1<<2))
 #define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE      ((1<<3) | (1<<4) | (1<<5))

ArduCopter/ekf_check.cpp

@@ -121,8 +121,13 @@ void Copter::failsafe_ekf_event()
         return;
     }
 
-    // do nothing if motors disarmed or not in flight mode that requires GPS
-    if (!motors.armed() || !mode_requires_GPS(control_mode)) {
+    // do nothing if motors disarmed
+    if (!motors.armed()) {
+        return;
+    }
+
+    // do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
+    if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
         return;
     }
 
@@ -130,12 +135,20 @@ void Copter::failsafe_ekf_event()
     failsafe.ekf = true;
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
 
-    // take action based on flight mode
-    if (mode_requires_GPS(control_mode)) {
-        set_mode_land_with_pause();
+    // take action based on fs_ekf_action parameter
+    switch (g.fs_ekf_action) {
+        case FS_EKF_ACTION_ALTHOLD:
+            // AltHold
+            if (!set_mode(ALT_HOLD)) {
+                set_mode_land_with_pause();
+            }
+            break;
+        default:
+            set_mode_land_with_pause();
+            break;
     }
 
-    // if flight mode is LAND ensure it's not the GPS controlled LAND
+    // if flight mode is already LAND ensure it's not the GPS controlled LAND
     if (control_mode == LAND) {
         land_do_not_use_GPS();
     }