AP_AdvancedFailsafe: option to not go back to the loss comm mission item if we already in the return path

Author: eppravitra

libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp

@@ -168,6 +168,7 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
     // @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) 
+    // @Bitmask: 2: Option to not jump to AFS_WP_COMMS if already in the return path 
     AP_GROUPINFO("OPTIONS", 21, AP_AdvancedFailsafe, options, 0),
 
     // @Param: GCS_TIMEOUT
@@ -251,13 +252,16 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
         if (!gcs_link_ok) {
             GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: DATA_LINK_LOSS");
             _state = STATE_DATA_LINK_LOSS;
-            if (_wp_comms_hold) {
-                _saved_wp = mission.get_current_nav_cmd().index;
+            _saved_wp = mission.get_current_nav_cmd().index;
+
+            if (should_use_comms_hold()){    
                 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) {
                 _comms_loss_count++;
@@ -414,6 +418,27 @@ AP_AdvancedFailsafe::check_altlimit(void)
     return false;
 }
 
+/*
+  return true if we should jump to _wp_comms_hold
+ */
+bool AP_AdvancedFailsafe::should_use_comms_hold(void){
+    
+    AP_Mission *_mission = AP::mission();
+
+    if (_mission == nullptr) {
+        return false;
+    }
+
+    if (_wp_comms_hold <= 0) {
+        return false;
+    }
+
+    if ((_mission->state() == AP_Mission::MISSION_RUNNING) && _mission->get_in_return_path_flag() && option_is_set(Option::CONTINUE_IF_ALREADY_IN_RETURN_PATH)){
+        return false;
+    }
+
+    return true;
+}
 /*
   return true if we should crash the vehicle
  */

libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h

@@ -82,6 +82,9 @@ class AP_AdvancedFailsafe
     // during sensor calibration
     void heartbeat(void);
 
+    //return true if we should jump to _wp_comms_hold
+    bool should_use_comms_hold(void);
+
     // return true if we are terminating (deliberately crashing the vehicle)
     bool should_crash_vehicle(void);
 
@@ -171,6 +174,7 @@ class AP_AdvancedFailsafe
     enum class Option {
         CONTINUE_AFTER_RECOVERED = (1U<<0),
         GCS_FS_ALL_AUTONOMOUS_MODES = (1U<<1),
+        CONTINUE_IF_ALREADY_IN_RETURN_PATH = (1U<<2),
     };
     bool option_is_set(Option option) const {
         return (options.get() & int16_t(option)) != 0;