Merge pull request #6655 from tonyyng/delayed_safehome

Delayed safehome

docs/Safehomes.md

@@ -12,14 +12,20 @@ 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.  You can define up to 8 safehomes for different locations you fly at.  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 is identified.  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 home location.  If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return back to arming point.
 
-You can define up to 8 safehomes for different locations you fly at.
+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.
+
+If you frequently use RTH to return back to the arming point, you may not want the aircraft to fly to the safehome.  Let it do this at least once to confirm safehomes is working as expected.  Afterward, `set safehome_usage_mode = RTH_FS` and the safehome will only be used for failsafe.  
+
+When using mode `RTH_FS`, you should confirm that your radio's failsafe configuration triggers the iNav failsafe mode.  With many receivers, you have the ability to specify what signal to output during failsafe conditions.
 
 When you are choosing safehome locations, ensure that the location is clear of obstructions for a radius more than 50m (`nav_fw_loiter_radius`).  As the plane descends, the circles aren't always symmetrical, as wind direction could result in some wider or tighter turns.  Also, the direction and length of the final landing stage is also unknown.  You want to choose a point that has provides a margin for variation and the final landing.
 
+If your safehome is not visible from your current location, use extra caution.  A visual check of the safehome is recommend prior to flying.  If the safehome is in use, you can use the OSD menu to disable safehome usage prior to your flight.
+
 ## OSD Message when Armed
 
 When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time.
@@ -36,8 +42,14 @@ If a safehome is selected, an additional message appears:
          CURRENT DATE
          CURRENT TIME
 ```
-The GPS details are those of the selected safehome.
-To draw your attention to "HOME" being replaced, the message flashes and stays visible longer.
+The GPS details are those of the arming location, not the safehome.
+To draw your attention to a safehome being selected, the message flashes and stays visible longer.
+
+If a safehome was found, but ``safehome_usage_mode``` is ```OFF```, the message ```SAFEHOME FOUND; MODE OFF``` will appear.
+
+## OSD Message during RTH
+
+If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed.
 
 ## CLI command `safehome` to manage safehomes
 

docs/Settings.md

@@ -473,6 +473,7 @@
 | rx_spi_protocol | _target default_ |  |
 | rx_spi_rf_channel_count | 0 |  |
 | 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. Possible values are `OFF`, `RTH` and `RTH_FS`.  See [Safehome documentation](Safehomes.md#Safehome) for more information. |
 | sbus_sync_interval | 3000 |  |
 | sdcard_detect_inverted | _target default_ | 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

@@ -163,6 +163,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
   - name: nav_rth_climb_first
     enum: navRTHClimbFirst_e
     values: ["OFF", "ON", "ON_FW_SPIRAL"]
@@ -2372,6 +2375,11 @@ groups:
         field: general.safehome_max_distance
         min: 0
         max: 65000
+      - name: safehome_usage_mode
+        description: "Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`.  See [Safehome documentation](Safehomes.md#Safehome) for more information."
+        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

@@ -47,6 +47,7 @@
 #include "flight/pid.h"
 
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
 
 #include "rx/rx.h"
 
@@ -362,12 +363,15 @@ 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) &&
-        ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) &&
-        sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
-
-        // Use the alternate, minimum distance failsafe procedure instead
-        return failsafeConfig()->failsafe_min_distance_procedure;
+    if (failsafeConfig()->failsafe_min_distance > 0 &&
+            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

@@ -804,6 +804,15 @@ static const char * osdFailsafeInfoMessage(void)
     }
     return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
 }
+#if defined(USE_SAFE_HOME)
+static const char * divertingToSafehomeMessage(void)
+{
+	if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) {
+	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
+	}
+	return NULL;
+}
+#endif
 
 static const char * navigationStateMessage(void)
 {
@@ -839,6 +848,11 @@ static const char * navigationStateMessage(void)
             return OSD_MESSAGE_STR(OSD_MSG_LANDING);
         case MW_NAV_STATE_HOVER_ABOVE_HOME:
             if (STATE(FIXED_WING_LEGACY)) {
+#if defined(USE_SAFE_HOME)
+                if (safehome_applied) {
+                    return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME);
+                }
+#endif
                 return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME);
             }
             return OSD_MESSAGE_STR(OSD_MSG_HOVERING);
@@ -3280,13 +3294,17 @@ static void osdShowArmed(void)
             }
             y += 4;
 #if defined (USE_SAFE_HOME)
-            if (isSafeHomeInUse()) {
-                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);
-                // write this message above the ARMED message to make it obvious
-                displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
+            if (safehome_distance) { // safehome found during arming
+                if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
+                    strcpy(buf, "SAFEHOME FOUND; MODE OFF");
+				} else {
+					char buf2[12]; // format the distance first
+					osdFormatDistanceStr(buf2, safehome_distance);
+					tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
+				}
+				textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
+				// write this message above the ARMED message to make it obvious
+				displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
             }
 #endif
         } else {
@@ -3355,7 +3373,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
             statsPagesCheck = 0;
 #if defined(USE_SAFE_HOME)
-            if (isSafeHomeInUse())
+            if (safehome_distance)
                 delay *= 3;
 #endif
             osdSetNextRefreshIn(delay);
@@ -3557,6 +3575,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 const char *failsafePhaseMessage = osdFailsafePhaseMessage();
                 const char *failsafeInfoMessage = osdFailsafeInfoMessage();
                 const char *navStateFSMessage = navigationStateMessage();
+
                 if (failsafePhaseMessage) {
                     messages[messageCount++] = failsafePhaseMessage;
                 }
@@ -3566,6 +3585,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 if (navStateFSMessage) {
                     messages[messageCount++] = navStateFSMessage;
                 }
+#if defined(USE_SAFE_HOME)
+                const char *safehomeMessage = divertingToSafehomeMessage();
+				if (safehomeMessage) {
+					messages[messageCount++] = safehomeMessage;
+				}
+#endif
                 if (messageCount > 0) {
                     message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
                     if (message == failsafeInfoMessage) {
@@ -3600,6 +3625,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = navStateMessage;
                         }
                     }
+#if defined(USE_SAFE_HOME)
+					const char *safehomeMessage = divertingToSafehomeMessage();
+					if (safehomeMessage) {
+						messages[messageCount++] = safehomeMessage;
+					}
+#endif
                 } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
                         const char *launchStateMessage = fixedWingLaunchStateMessage();

src/main/io/osd.h

@@ -100,6 +100,11 @@
 #define OSD_MSG_HEADFREE            "(HEADFREE)"
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"
 
+#if defined(USE_SAFE_HOME)
+#define OSD_MSG_DIVERT_SAFEHOME     "DIVERTING TO SAFEHOME"
+#define OSD_MSG_LOITERING_SAFEHOME  "LOITERING AROUND SAFEHOME"
+#endif
+
 typedef enum {
     OSD_RSSI_VALUE,
     OSD_MAIN_BATT_VOLTAGE,

src/main/navigation/navigation.c

@@ -74,10 +74,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
-uint32_t safehome_distance;               // distance to the selected safehome
+int8_t safehome_index = -1;               // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+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);
 
@@ -109,6 +113,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
             .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT,
             .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
             .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
+            .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
         },
 
         // General navigation parameters
@@ -235,6 +240,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void);
 
 void initializeRTHSanityChecker(const fpVector3_t * pos);
 bool validateRTHSanityChecker(void);
+void updateHomePosition(void);
 
 static bool rthAltControlStickOverrideCheck(unsigned axis);
 
@@ -2350,26 +2356,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 
 #if defined(USE_SAFE_HOME)
 
-/*******************************************************
- * Is a safehome being used instead of the arming point?
- *******************************************************/
-bool isSafeHomeInUse(void)
-{
-    return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
+void checkSafeHomeState(bool shouldBeEnabled)
+{
+	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;
+	}
+	// if we've changed the home position, update the distance and direction
+    updateHomePosition();
 }
 
 /***********************************************************
  *  See if there are any safehomes near where we are arming.
- *  If so, use it instead of the arming point for home.
- *  Select the nearest safehome
+ *  If so, save the nearest one in case we need it later for RTH.
  **********************************************************/
-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++) {
@@ -2382,20 +2404,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());
-        return true;
+    } else {
+        safehome_distance = 0;
     }
-    safehome_distance = 0;
-    return false;
+    return safehome_distance > 0;
 }
 #endif
 
@@ -2421,9 +2442,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());
+                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;
             }
         }
     }
@@ -3165,6 +3190,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 || posControl.flags.forcedRTHActivated);
 
         // Keep canActivateWaypoint flag at FALSE if there is no mission loaded
         // Also block WP mission if we are executing RTH
@@ -3601,6 +3627,7 @@ void activateForcedRTH(void)
 {
     abortFixedWingLaunch();
     posControl.flags.forcedRTHActivated = true;
+    checkSafeHomeState(true);
     navProcessFSMEvents(selectNavEventFromBoxModeInput());
 }
 
@@ -3609,6 +3636,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,20 @@ 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 uint32_t safehome_distance;               // distance to the selected safehome
+extern int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+extern uint32_t safehome_distance;               // distance to the nearest safehome
+extern bool safehome_applied;                    // whether the safehome has been applied to home.
 
 void resetSafeHomes(void);                       // remove all safehomes
-bool isSafeHomeInUse(void);                      // Are we using a safehome instead of the arming point?
-bool foundNearbySafeHome(void);                  // Did we find a safehome nearby?
+bool findNearestSafeHome(void);                  // Find nearest safehome
 
 #endif // defined(USE_SAFE_HOME)
 
@@ -191,6 +198,7 @@ typedef struct navConfig_s {
             uint8_t rth_climb_ignore_emerg;     // Option to ignore GPS loss on initial climb stage of RTH
             uint8_t rth_alt_control_override;   // Override RTH Altitude and Climb First settings using Pitch and Roll stick
             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)