Add safehomes (#5995)

* Merge Safehome feature from development repo

* Remove set nav_rth_home_offset_* to avoid confusion

docs/Safehomes.md

@@ -0,0 +1,70 @@
+# INav - Safehomes
+
+## Introduction
+
+The "Home" position is used for the landing point when landing is enabled or in an emergency situation.  It is usually determined by the GPS location where the aircraft is armed. 
+
+For airplanes, the landing procedure is explained very well by Pawel Spychalski [here.](https://quadmeup.com/inav-1-8-automated-landing-for-fixed-wings/)
+
+<img src="https://quadmeup.com/wp-content/uploads/2017/06/fixed-wing-landing-1024x683.png" width="600">
+
+One potential risk when landing is that there might be buildings, trees and other obstacles in the way as the airplance circles lower toward the ground at the arming point.  Most people don't go the middle of the field when arming their airplanes.
+
+## 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 first one that is enabled and within 200m of the current position will be selected.  Otherwise, it reverts to the old behaviour of using your current GPS position as home.  
+
+You can define up to 8 safehomes for different locations you fly at.
+
+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.
+
+## OSD Message when Armed
+
+When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time.
+
+If a safehome is selected, an additional message appears:
+```
+     H - DIST -> SAFEHOME n  <- New message
+                                n   is the Safehome index (0-7)
+            ARMED               DIST is the distance from   
+        GPS LATITUDE                 your current position to this safehome
+        GPS LONGITUDE
+        GPS PLUS CODE
+        
+         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.
+
+## CLI command `safehome` to manage safehomes
+
+`safehome` - List all safehomes
+
+`safehome reset` - Clears all safehomes.
+
+`safehome <n> <enabled> <lat> <lon>` - Set the parameters of a safehome with index `<n>`.
+
+Parameters:
+
+  * `<enabled>` - 0 is disabled, 1 is enabled.
+  * `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
+  * `<lon>` - Longitude.
+
+Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output.  Use `save` to save any changes, as with other settings.
+
+### `safehome` example
+
+```
+# safehome
+safehome 0 1 543533193 -45179273
+safehome 1 1 435464846 -78654544
+safehome 2 0 0 0 
+safehome 3 0 0 0 
+safehome 4 0 0 0 
+safehome 5 0 0 0 
+safehome 6 0 0 0 
+safehome 7 0 0 0 
+
+```
+

src/main/config/parameter_group_ids.h

@@ -113,7 +113,8 @@
 #define PG_GLOBAL_VARIABLE_CONFIG 1023
 #define PG_SMARTPORT_MASTER_CONFIG 1024
 #define PG_OSD_LAYOUTS_CONFIG 1025
-#define PG_INAV_END 1025
+#define PG_SAFE_HOME_CONFIG 1026
+#define PG_INAV_END 1026
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/cli.c

@@ -1276,6 +1276,67 @@ static void cliTempSensor(char *cmdline)
 }
 #endif
 
+#if defined(USE_SAFE_HOME)
+static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
+{
+    const char *format = "safehome %u %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon
+    for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
+        bool equalsDefault = false;
+        if (defaultSafeHome) {
+            equalsDefault = navSafeHome[i].enabled == defaultSafeHome[i].enabled
+               && navSafeHome[i].lat == defaultSafeHome[i].lat
+               && navSafeHome[i].lon == defaultSafeHome[i].lon;
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,
+                defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon);
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, i,
+            navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon);
+    }
+}
+
+static void cliSafeHomes(char *cmdline)
+{
+    if (isEmpty(cmdline)) {
+        printSafeHomes(DUMP_MASTER, safeHomeConfig(0), NULL);
+    } else if (sl_strcasecmp(cmdline, "reset") == 0) {
+        resetSafeHomes();
+    } else {
+        int32_t lat, lon;
+        bool enabled;
+        uint8_t validArgumentCount = 0;
+        const char *ptr = cmdline;
+        int8_t i = fastA2I(ptr);
+        if (i < 0 || i >= MAX_SAFE_HOMES) {
+             cliShowArgumentRangeError("safehome index", 0, MAX_SAFE_HOMES - 1);
+        } else {
+            if ((ptr = nextArg(ptr))) {
+                enabled = fastA2I(ptr);
+                validArgumentCount++;
+            }
+            if ((ptr = nextArg(ptr))) {
+                lat = fastA2I(ptr);
+                validArgumentCount++;
+            }
+            if ((ptr = nextArg(ptr))) {
+                lon = fastA2I(ptr);
+                validArgumentCount++;
+            }
+            if ((ptr = nextArg(ptr))) {
+                // check for too many arguments
+                validArgumentCount++;
+            }
+            if (validArgumentCount != 3) {
+                cliShowParseError();
+            } else {
+                safeHomeConfigMutable(i)->enabled = enabled;
+                safeHomeConfigMutable(i)->lat = lat;
+                safeHomeConfigMutable(i)->lon = lon;
+            }
+        }
+    }
+}
+
+#endif 
 #if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
 static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
 {
@@ -3214,6 +3275,10 @@ static void printConfig(const char *cmdline, bool doDiff)
         cliPrintHashLine("servo");
         printServo(dumpMask, servoParams_CopyArray, servoParams(0));
 
+#if defined(USE_SAFE_HOME)
+        cliPrintHashLine("safehome");
+        printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0)); 
+#endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
         cliPrintHashLine("logic");
         printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0));
@@ -3454,6 +3519,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
 #endif
     CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
+#if defined(USE_SAFE_HOME)
+    CLI_COMMAND_DEF("safehome", "safe home list", NULL, cliSafeHomes),
+#endif
     CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
     CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
 #ifdef USE_SERIAL_PASSTHROUGH

src/main/fc/settings.yaml

@@ -2115,16 +2115,6 @@ groups:
         default_value: "0"
         field: general.rth_home_altitude
         max: 65000
-      - name: nav_rth_home_offset_distance
-        description: "Distance offset from GPS established home to \"safe\" position used for RTH (cm, 0 disables)"
-        default_value: "0"
-        field: general.rth_home_offset_distance
-        max: 65000
-      - name: nav_rth_home_offset_direction
-        description: "Direction offset from GPS established home to \"safe\" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance)"
-        default_value: "0"
-        field: general.rth_home_offset_direction
-        max: 359
       - 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/io/osd.c

@@ -3003,6 +3003,16 @@ static void osdShowArmed(void)
             olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf);
             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);
+            }
+#endif
         } else {
             strcpy(buf, "!NO HOME POSITION!");
             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
@@ -3062,7 +3072,12 @@ static void osdRefresh(timeUs_t currentTimeUs)
         if (ARMING_FLAG(ARMED)) {
             osdResetStats();
             osdShowArmed(); // reset statistic etc
-            osdSetNextRefreshIn(ARMED_SCREEN_DISPLAY_TIME);
+            uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
+#if defined(USE_SAFE_HOME)
+            if (isSafeHomeInUse())
+                delay *= 3;
+#endif
+            osdSetNextRefreshIn(delay);
         } else {
             osdShowStats(); // show statistic
             osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);

src/main/navigation/navigation.c

@@ -18,7 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <math.h>
-
+#include <string.h>
 
 #include "platform.h"
 
@@ -74,6 +74,13 @@ uint32_t      GPS_distanceToHome;        // distance to home point in meters
 int16_t       GPS_directionToHome;       // direction to home point in degrees
 
 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
+
+PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
+
+#endif
 
 #if defined(USE_NAV)
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
@@ -115,8 +122,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .rth_home_altitude = 0,                 // altitude in centimeters
         .rth_abort_threshold = 50000,           // centimeters - 500m should be safe for all aircraft
         .max_terrain_follow_altitude = 100,     // max altitude in centimeters in terrain following mode
-        .rth_home_offset_distance = 0,          // Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables)
-        .rth_home_offset_direction = 0,         // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance)
         },
 
     // MC-specific
@@ -2403,6 +2408,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
     return flags;
 }
 
+#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);
+}
+
+/***********************************************************
+ *  See if there are any safehomes near where we are arming.
+ *  If so, use it instead of the arming point for home.
+ **********************************************************/
+bool foundNearbySafeHome(void)
+{
+    safehome_used = -1; 
+    fpVector3_t safeHome;
+    gpsLocation_t shLLH;
+    shLLH.alt = 0;
+    for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
+        shLLH.lat = safeHomeConfig(i)->lat;
+        shLLH.lon = safeHomeConfig(i)->lon;
+        geoConvertGeodeticToLocal(&safeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
+        safehome_distance = calculateDistanceToDestination(&safeHome); 
+        if (safehome_distance < 20000) { // 200 m
+             safehome_used = i;
+             setHomePosition(&safeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+             return true;
+        }
+    }
+    safehome_distance = 0;
+    return false;
+}
+#endif
+
 /*-----------------------------------------------------------
  * Update home position, calculate distance and bearing to home
  *-----------------------------------------------------------*/
@@ -2424,18 +2465,13 @@ void updateHomePosition(void)
                     break;
             }
             if (setHome) {
-                if (navConfig()->general.rth_home_offset_distance != 0) { // apply user defined offset
-                    fpVector3_t offsetHome;
-                    offsetHome.x = posControl.actualState.abs.pos.x + navConfig()->general.rth_home_offset_distance * cos_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction));
-                    offsetHome.y = posControl.actualState.abs.pos.y + navConfig()->general.rth_home_offset_distance * sin_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction));
-                    offsetHome.z = posControl.actualState.abs.pos.z;
-                    setHomePosition(&offsetHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-                } else {
+#if defined(USE_SAFE_HOME)
+                if (!foundNearbySafeHome())
+#endif
                     setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
             }
         }
     }
-    }
     else {
         static bool isHomeResetAllowed = false;
 
@@ -2880,6 +2916,14 @@ bool saveNonVolatileWaypointList(void)
 }
 #endif
 
+#if defined(USE_SAFE_HOME)
+
+void resetSafeHomes(void) 
+{
+    memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
+}
+#endif
+
 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
 {
     gpsLocation_t wpLLH;

src/main/navigation/navigation.h

@@ -39,6 +39,26 @@ extern bool autoThrottleManuallyIncreased;
 
 /* Navigation system updates */
 void onNewGPSData(void);
+#if defined(USE_SAFE_HOME)
+
+#define MAX_SAFE_HOMES 8
+
+typedef struct {
+    uint8_t enabled;
+    int32_t lat;
+    int32_t lon;
+} navSafeHome_t;
+
+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
+
+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?
+
+#endif // defined(USE_SAFE_HOME)
 
 #if defined(USE_NAV)
 #if defined(USE_BLACKBOX)
@@ -174,8 +194,6 @@ typedef struct navConfig_s {
         uint16_t min_rth_distance;              // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
         uint16_t rth_abort_threshold;           // Initiate emergency landing if during RTH we get this much [cm] away from home
         uint16_t max_terrain_follow_altitude;   // Max altitude to be used in SURFACE TRACKING mode
-        uint16_t rth_home_offset_distance;	  // Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables)
-        uint16_t rth_home_offset_direction;	  // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance)
     } general;
 
     struct {

src/main/target/common.h

@@ -129,6 +129,7 @@
 
 #if (FLASH_SIZE > 128)
 #define NAV_FIXED_WING_LANDING
+#define USE_SAFE_HOME
 #define USE_AUTOTUNE_FIXED_WING
 #define USE_LOG
 #define USE_STATS