New safehome options to control when safehomes are applied

docs/Safehomes.md

@@ -12,9 +12,13 @@ One potential risk when landing is that there might be buildings, trees and othe
 
 ## Safehome
 
-Safehomes are a list of GPS coordinates that identify safe landing points.  When the flight controller is armed, it checks the list of safehomes.  The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be selected.  Otherwise, it reverts to the old behaviour of using your current GPS position as home.  
+Safehomes are a list of GPS coordinates that identify safe landing points.  When the flight controller is armed, it checks the list of safehomes.  The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be remembered.  The arming home location remains as home.
 
-Be aware that the safehome replaces your arming position as home.  When flying, RTH will return to the safehome and OSD elements such as distance to home and direction to home will refer to the selected safehome.
+When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the arming home.  If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return to arming point.
+
+The safehome operating mode is set using ```safehome_usage_mode```.  If ```OFF```, safehomes will not be used.  If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe.  If ```RTH_FS```, the safehome will only be used for RX failsafe.  This option can be changed using the OSD menu.
+
+This behavior has changed from the initial release, where the safehome location replaced the arming location during the arming phase.  That would result in flight information involving home (home distance, home bearing, etc) using the safehome, instead of the arming location.
 
 You can define up to 8 safehomes for different locations you fly at.
 
@@ -36,8 +40,8 @@ If a safehome is selected, an additional message appears:
          CURRENT DATE
          CURRENT TIME
 ```
-The GPS details are those of the selected safehome.
+The GPS details are those of the arming location, not the safehome.
-To draw your attention to "HOME" being replaced, the message flashes and stays visible longer.
+To draw your attention to a safehome being selected, the message flashes and stays visible longer.
 
 ## CLI command `safehome` to manage safehomes
 

docs/Settings.md

@@ -444,6 +444,7 @@
 | rx_spi_protocol |  |  |
 | rx_spi_rf_channel_count |  |  |
 | safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. |
+| safehome_usage_mode | RTH | Used to control when safehomes will be used. |
 | sbus_sync_interval |  |  |
 | sdcard_detect_inverted | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. |
 | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. |

src/main/cms/cms_menu_navigation.c

@@ -81,6 +81,7 @@ static const CMS_Menu cmsx_menuNavSettings = {
     OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE),
     OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD),
     OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED),
+    OSD_SETTING_ENTRY("SAFEHOME USAGE MODE", SETTING_SAFEHOME_USAGE_MODE),
 
     OSD_BACK_AND_END_ENTRY,
  };

src/main/fc/settings.yaml

@@ -157,6 +157,9 @@ tables:
     values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
   - name: osd_plus_code_short
     values: ["0", "2", "4", "6"]
+  - name: safehome_usage_mode
+    values: ["OFF", "RTH", "RTH_FS"]
+    enum: safehomeUsageMode_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -2191,6 +2194,11 @@ groups:
         field: general.safehome_max_distance
         min: 0
         max: 65000
+      - name: safehome_usage_mode
+        description: "Used to control when safehomes will be used."
+        default_value: "RTH"
+        field: general.flags.safehome_usage_mode
+        table: safehome_usage_mode
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
         default_value: "30"

src/main/flight/failsafe.c

@@ -46,6 +46,7 @@
 #include "flight/pid.h"
 
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
 
 #include "rx/rx.h"
 
@@ -361,13 +362,16 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 
     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
     // GPS must also be working, and home position set
-    if ((failsafeConfig()->failsafe_min_distance > 0) &&
+    if (failsafeConfig()->failsafe_min_distance > 0 &&
-        ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) &&
             sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 
+        // get the distance to the original arming point
+        uint32_t distance = calculateDistanceToDestination(&original_rth_home);
+        if (distance < failsafeConfig()->failsafe_min_distance) {
             // Use the alternate, minimum distance failsafe procedure instead
             return failsafeConfig()->failsafe_min_distance_procedure;
         }
+    }
 
     return failsafeConfig()->failsafe_procedure;
 }

src/main/io/osd.c

@@ -3086,11 +3086,11 @@ static void osdShowArmed(void)
             }
             y += 4;
 #if defined (USE_SAFE_HOME)
-            if (isSafeHomeInUse()) {
+            if (safehome_distance) {
                 textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
                 char buf2[12]; // format the distance first
                 osdFormatDistanceStr(buf2, safehome_distance);
-                tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used);
+                tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
                 // write this message above the ARMED message to make it obvious
                 displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
             }
@@ -3160,7 +3160,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             osdShowArmed(); // reset statistic etc
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
 #if defined(USE_SAFE_HOME)
-            if (isSafeHomeInUse())
+            if (safehome_distance)
                 delay *= 3;
 #endif
             osdSetNextRefreshIn(delay);

src/main/navigation/navigation.c

@@ -73,10 +73,14 @@ gpsLocation_t GPS_home;
 uint32_t      GPS_distanceToHome;        // distance to home point in meters
 int16_t       GPS_directionToHome;       // direction to home point in degrees
 
+fpVector3_t   original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
+
 radar_pois_t radar_pois[RADAR_MAX_POIS];
 #if defined(USE_SAFE_HOME)
-int8_t safehome_used;                     // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
-uint32_t safehome_distance;               // distance to the selected safehome
+uint32_t safehome_distance = 0;           // distance to the nearest safehome
+fpVector3_t nearestSafeHome;              // The nearestSafeHome found during arming
+bool safehome_applied = false;            // whether the safehome has been applied to home.
 
 PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
 
@@ -107,6 +111,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
             .disarm_on_landing = 0,
             .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS,
             .nav_overrides_motor_stop = NOMS_ALL_NAV,
+            .safehome_usage_mode = SAFEHOME_USAGE_RTH,
         },
 
         // General navigation parameters
@@ -2327,26 +2332,40 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 
 #if defined(USE_SAFE_HOME)
 
-/*******************************************************
+void checkSafeHomeState(bool shouldBeEnabled)
- * Is a safehome being used instead of the arming point?
- *******************************************************/
-bool isSafeHomeInUse(void)
 {
-    return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
+	if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
+		shouldBeEnabled = false;
+	} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
+		// if safehomes are only used with failsafe and we're trying to enable safehome
+		// then enable the safehome only with failsafe
+		shouldBeEnabled = posControl.flags.forcedRTHActivated;
+	}
+    // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
+	if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
+		return;
+	}
+    if (shouldBeEnabled) {
+		// set home to safehome
+        setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+		safehome_applied = true;
+	} else {
+		// set home to original arming point
+        setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+		safehome_applied = false;
+	}
 }
 
 /***********************************************************
  *  See if there are any safehomes near where we are arming.
- *  If so, use it instead of the arming point for home.
+ *  If so, save the nearest one in case we need it later for RTH.
- *  Select the nearest safehome
  **********************************************************/
-bool foundNearbySafeHome(void)
+bool findNearestSafeHome(void)
 {
-    safehome_used = -1;
+    safehome_index = -1;
     uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
     uint32_t distance_to_current;
     fpVector3_t currentSafeHome;
-    fpVector3_t nearestSafeHome;
     gpsLocation_t shLLH;
     shLLH.alt = 0;
     for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
@@ -2359,20 +2378,19 @@ bool foundNearbySafeHome(void)
         distance_to_current = calculateDistanceToDestination(&currentSafeHome);
         if (distance_to_current < nearest_safehome_distance) {
              // this safehome is the nearest so far - keep track of it.
-             safehome_used = i;
+             safehome_index = i;
              nearest_safehome_distance = distance_to_current;
              nearestSafeHome.x = currentSafeHome.x;
              nearestSafeHome.y = currentSafeHome.y;
              nearestSafeHome.z = currentSafeHome.z;
         }
     }
-    if (safehome_used >= 0) {
+    if (safehome_index >= 0) {
 		safehome_distance = nearest_safehome_distance;
-        setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+    } else {
-        return true;
-    }
         safehome_distance = 0;
-    return false;
+    }
+    return safehome_distance > 0;
 }
 #endif
 
@@ -2398,9 +2416,13 @@ void updateHomePosition(void)
             }
             if (setHome) {
 #if defined(USE_SAFE_HOME)
-                if (!foundNearbySafeHome())
+                findNearestSafeHome();
 #endif
                 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+                // save the current location in case it is replaced by a safehome or HOME_RESET
+                original_rth_home.x = posControl.rthState.homePosition.pos.x;
+                original_rth_home.y = posControl.rthState.homePosition.pos.y;
+                original_rth_home.z = posControl.rthState.homePosition.pos.z;
             }
         }
     }
@@ -3109,6 +3131,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         const bool canActivatePosHold    = canActivatePosHoldMode();
         const bool canActivateNavigation = canActivateNavigationModes();
         const bool isExecutingRTH        = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
+        checkSafeHomeState(isExecutingRTH);
 
         // Keep canActivateWaypoint flag at FALSE if there is no mission loaded
         // Also block WP mission if we are executing RTH
@@ -3550,6 +3573,7 @@ void abortForcedRTH(void)
     // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
     // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
     posControl.flags.forcedRTHActivated = false;
+    checkSafeHomeState(false);
     navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
 }
 

src/main/navigation/navigation.h

@@ -35,6 +35,7 @@
 extern gpsLocation_t        GPS_home;
 extern uint32_t             GPS_distanceToHome;        // distance to home point in meters
 extern int16_t              GPS_directionToHome;       // direction to home point in degrees
+extern fpVector3_t          original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
 
 extern bool autoThrottleManuallyIncreased;
 
@@ -50,14 +51,19 @@ typedef struct {
     int32_t lon;
 } navSafeHome_t;
 
+typedef enum {
+    SAFEHOME_USAGE_OFF = 0,    // Don't use safehomes
+    SAFEHOME_USAGE_RTH = 1,    // Default - use safehome for RTH
+    SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only
+} safehomeUsageMode_e;
+
 PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
 
-extern int8_t safehome_used;                     // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+extern int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
-extern uint32_t safehome_distance;               // distance to the selected safehome
+extern uint32_t safehome_distance;               // distance to the nearest safehome
 
 void resetSafeHomes(void);                       // remove all safehomes
-bool isSafeHomeInUse(void);                      // Are we using a safehome instead of the arming point?
+bool findNearestSafeHome(void);                  // Find nearest safehome
-bool foundNearbySafeHome(void);                  // Did we find a safehome nearby?
 
 #endif // defined(USE_SAFE_HOME)
 
@@ -184,6 +190,7 @@ typedef struct navConfig_s {
             uint8_t rth_allow_landing;          // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
             uint8_t rth_climb_ignore_emerg;     // Option to ignore GPS loss on initial climb stage of RTH
             uint8_t nav_overrides_motor_stop;   // Autonomous modes override motor_stop setting and user command to stop motor
+            uint8_t safehome_usage_mode;        // Controls when safehomes are used
         } flags;
 
         uint8_t  pos_failure_timeout;           // Time to wait before switching to emergency landing (0 - disable)