Copter: support Auto RTL for failsafe events

ArduCopter/Copter.h

@@ -594,7 +594,8 @@ private:
         Failsafe_Action_RTL            = 2,
         Failsafe_Action_SmartRTL       = 3,
         Failsafe_Action_SmartRTL_Land  = 4,
-        Failsafe_Action_Terminate      = 5
+        Failsafe_Action_Terminate      = 5,
+        Failsafe_Action_Auto_DO_LAND_START = 6,
     };
 
     enum class FailsafeOption {
@@ -731,6 +732,7 @@ private:
     void set_mode_RTL_or_land_with_pause(ModeReason reason);
     void set_mode_SmartRTL_or_RTL(ModeReason reason);
     void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
+    void set_mode_auto_do_land_start_or_RTL(ModeReason reason);
     bool should_disarm_on_failsafe();
     void do_failsafe_action(Failsafe_Action action, ModeReason reason);
 

ArduCopter/defines.h

@@ -152,6 +152,7 @@ enum LoggingParameters {
 #define FS_THR_ENABLED_ALWAYS_LAND                 3
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL      4
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND     5
+#define FS_THR_ENABLED_AUTO_RTL_OR_RTL             6
 
 // GCS failsafe definitions (FS_GCS_ENABLE parameter)
 #define FS_GCS_DISABLED                        0
@@ -160,6 +161,7 @@ enum LoggingParameters {
 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL  3
 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
 #define FS_GCS_ENABLED_ALWAYS_LAND             5
+#define FS_GCS_ENABLED_AUTO_RTL_OR_RTL         6
 
 // EKF failsafe definitions (FS_EKF_ACTION parameter)
 #define FS_EKF_ACTION_LAND                  1       // switch to LAND mode on EKF failsafe

ArduCopter/events.cpp

@@ -33,6 +33,9 @@ void Copter::failsafe_radio_on_event()
         case FS_THR_ENABLED_ALWAYS_LAND:
             desired_action = Failsafe_Action_Land;
             break;
+        case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
+            desired_action = Failsafe_Action_Auto_DO_LAND_START;
+            break;
         default:
             desired_action = Failsafe_Action_Land;
     }
@@ -170,6 +173,9 @@ void Copter::failsafe_gcs_on_event(void)
         case FS_GCS_ENABLED_ALWAYS_LAND:
             desired_action = Failsafe_Action_Land;
             break;
+        case FS_GCS_ENABLED_AUTO_RTL_OR_RTL:
+            desired_action = Failsafe_Action_Auto_DO_LAND_START;
+            break;
         default: // if an invalid parameter value is set, the fallback is RTL
             desired_action = Failsafe_Action_RTL;
     }
@@ -336,6 +342,18 @@ void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
     }
 }
 
+// Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param
+// This can come from failsafe or RC option
+void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
+{
+    if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(reason)) {
+        return;
+    }
+
+    gcs().send_text(MAV_SEVERITY_WARNING, "Trying RTL Mode");
+    set_mode_RTL_or_land_with_pause(reason);
+}
+
 bool Copter::should_disarm_on_failsafe() {
     if (ap.in_arming_delay) {
         return true;
@@ -383,6 +401,10 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
 #endif
         }
         break;
+        case Failsafe_Action_Auto_DO_LAND_START:
+            set_mode_auto_do_land_start_or_RTL(reason);
+            AP_Notify::events.failsafe_mode_change = 1;
+            break;
     }
 
 #if GRIPPER_ENABLED == ENABLED