Rover: Add SmartRTL failsafe action

Add failsafe actions SmartRTL or RTL and SmartRTL or Hold

APMrover2/Parameters.cpp

@@ -152,7 +152,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: FS_ACTION
     // @DisplayName: Failsafe Action
     // @Description: What to do on a failsafe event
-    // @Values: 0:Nothing,1:RTL,2:Hold
+    // @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold
     // @User: Standard
     GSCALAR(fs_action,    "FS_ACTION",     2),
 

APMrover2/failsafe.cpp

@@ -77,6 +77,18 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
             case 2:
                 set_mode(mode_hold, MODE_REASON_FAILSAFE);
                 break;
+            case 3:
+                if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
+                    if (!set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
+                        set_mode(mode_hold, MODE_REASON_FAILSAFE);
+                    }
+                }
+                break;
+            case 4:
+                if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
+                    set_mode(mode_hold, MODE_REASON_FAILSAFE);
+                }
+                break;
         }
     }
 }