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

@@ -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,12 +362,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

@@ -777,6 +777,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 (safehome_applied) {
+	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
+	}
+	return NULL;
+}
+#endif
 
 static const char * navigationStateMessage(void)
 {
@@ -785,7 +794,7 @@ static const char * navigationStateMessage(void)
             break;
         case MW_NAV_STATE_RTH_START:
             return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH);
-        case MW_NAV_STATE_RTH_CLIMB:            
+        case MW_NAV_STATE_RTH_CLIMB:
             return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB);
         case MW_NAV_STATE_RTH_ENROUTE:
             return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
@@ -3086,13 +3095,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 {
@@ -3160,7 +3173,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);
@@ -3354,6 +3367,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;
                 }
@@ -3363,6 +3377,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) {
@@ -3381,7 +3401,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
                     if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
                         // Countdown display for remaining Waypoints
-                        tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount);                            
+                        tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount);
                         messages[messageCount++] = messageBuf;
                     } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                         // WP hold time countdown in seconds
@@ -3391,12 +3411,18 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining);
                             messages[messageCount++] = messageBuf;
                         }
-                    } else {                            
-                        const char *navStateMessage = navigationStateMessage();                             
+                    } else {
+                        const char *navStateMessage = navigationStateMessage();
                         if (navStateMessage) {
                             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

@@ -98,6 +98,10 @@
 #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"
+#endif
+
 typedef enum {
     OSD_RSSI_VALUE,
     OSD_MAIN_BATT_VOLTAGE,
@@ -337,7 +341,7 @@ typedef struct osdConfig_s {
 
     bool osd_failsafe_switch_layout;
     uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
-    uint8_t plus_code_short; 
+    uint8_t plus_code_short;
     uint8_t osd_ahi_style;
     uint8_t force_grid;                 // Force a pixel based OSD to use grid mode.
     uint8_t ahi_bordered;               // Only used by the AHI widget