AP_AdvancedFailsafe: option to automatically set mode to auto when datalink is loss

When enabled, it ensures vehicle enters AUTO even if it is currently in other autonomous modes (for example Guided)

ArduCopter/afs_copter.cpp

@@ -68,4 +68,9 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
     return AP_AdvancedFailsafe::AFS_STABILIZED;
 }
 
+//to force entering auto mode when datalink loss 
+ void AP_AdvancedFailsafe_Copter::set_mode_auto(void)
+ {
+    copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE);
+ }
 #endif // ADVANCED_FAILSAFE

ArduCopter/afs_copter.h

@@ -39,6 +39,9 @@ protected:
 
     // return the AFS mapped control mode
     enum control_mode afs_mode(void) override;
+
+    //to force entering auto mode when datalink loss 
+    void set_mode_auto(void) override;
 };
 
 #endif // ADVANCED_FAILSAFE

ArduPlane/afs_plane.cpp

@@ -103,4 +103,11 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void)
     }
     return AP_AdvancedFailsafe::AFS_STABILIZED;
 }
+
+//to force entering auto mode when datalink loss 
+ void AP_AdvancedFailsafe_Plane::set_mode_auto(void)
+ {
+    plane.set_mode(plane.mode_auto,ModeReason::GCS_FAILSAFE);
+ }
+
 #endif // AP_ADVANCEDFAILSAFE_ENABLED

ArduPlane/afs_plane.h

@@ -39,6 +39,9 @@ protected:
 
     // return the AFS mapped control mode
     enum control_mode afs_mode(void) override;
+
+    //to force entering auto mode when datalink loss 
+    void set_mode_auto(void) override;
 };
 
 #endif // AP_ADVANCEDFAILSAFE_ENABLED

Rover/afs_rover.cpp

@@ -29,4 +29,9 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void)
     return AP_AdvancedFailsafe::AFS_STABILIZED;
 }
 
+//to force entering auto mode when datalink loss 
+ void AP_AdvancedFailsafe_Rover::set_mode_auto(void)
+ {
+    over.set_mode(rover.mode_auto,ModeReason::GCS_FAILSAFE);
+ }
 #endif  // ADVANCED_FAILSAFE

Rover/afs_rover.h

@@ -38,6 +38,9 @@ protected:
 
     // return the AFS mapped control mode
     enum control_mode afs_mode(void) override;
+
+    //to force entering auto mode when datalink loss 
+    void set_mode_auto(void) override;
 };
 
 #endif  // ADVANCED_FAILSAFE

libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp

@@ -167,6 +167,7 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
     // @DisplayName: AFS options
     // @Description: See description for each bitmask bit description
     // @Bitmask: 0: Continue the mission even after comms are recovered (does not go to the mission item at the time comms were lost)
+    // @Bitmask: 1: Enable AFS for all autonomous modes (not just AUTO) 
     AP_GROUPINFO("OPTIONS", 21, AP_AdvancedFailsafe, options, 0),
     
     AP_GROUPEND
@@ -246,6 +247,9 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
             if (_wp_comms_hold) {
                 _saved_wp = mission.get_current_nav_cmd().index;
                 mission.set_current_cmd(_wp_comms_hold);
+                if (mode == AFS_AUTO && option_is_set(Option::GCS_FS_ALL_AUTONOMOUS_MODES)) {
+                    set_mode_auto();
+                }
             }
             // if two events happen within 30s we consider it to be part of the same event
             if (now - _last_comms_loss_ms > 30*1000UL) {

libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h

@@ -105,6 +105,9 @@ protected:
     // return the AFS mapped control mode
     virtual enum control_mode afs_mode(void) = 0;
 
+    //to force entering auto mode when datalink loss 
+    virtual void set_mode_auto(void) = 0;
+
     enum state _state;
 
     AP_Int8 _enable;
@@ -166,6 +169,7 @@ private:
     AP_Int16 options;
     enum class Option {
         CONTINUE_AFTER_RECOVERED = (1U<<0),
+        GCS_FS_ALL_AUTONOMOUS_MODES = (1U<<1),
     };
     bool option_is_set(Option option) const {
         return (options.get() & int16_t(option)) != 0;