From 3e3564e864c75c28538d93d84fe82a12fdb7d898 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 29 Jul 2020 10:14:58 -0400 Subject: [PATCH 1/4] Merge Safehome feature from development repo --- docs/Safehomes.md | 83 +++++++++++++++++++++++++++ src/main/config/parameter_group_ids.h | 3 +- src/main/fc/cli.c | 68 ++++++++++++++++++++++ src/main/io/osd.c | 17 +++++- src/main/navigation/navigation.c | 52 +++++++++++++++++ src/main/navigation/navigation.h | 20 +++++++ src/main/target/common.h | 1 + 7 files changed, 242 insertions(+), 2 deletions(-) create mode 100644 docs/Safehomes.md diff --git a/docs/Safehomes.md b/docs/Safehomes.md new file mode 100644 index 00000000000..f54efbe2c4f --- /dev/null +++ b/docs/Safehomes.md @@ -0,0 +1,83 @@ +# 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/) + + + +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. + +There are two existing settings that can be used change the home landing point: + +setting | explanation +------- | ----------- +nav_rth_home_offset_distance | Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables) +nav_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, + +However, these settings will likely work in one place. If you fly at a different field or the same field, but a different location (because of wind, sun, other pilots), the point identified by the distance and direction, might not be safe. + +## Home Offset and Home Direction + +If `nav_rth_home_offset_distance` is not zero, this takes precedence over the safehome approach. Safehomes are ignored when this is the case. + +## 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 ` - Set the parameters of a safehome with index ``. + +Parameters: + + * `` - 0 is disabled, 1 is enabled. + * `` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789). + * `` - 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 + +``` + diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 078352ce746..67d86a1b9e6 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 7693a0856e4..cdeac26521d 100755 --- a/src/main/fc/cli.c +++ b/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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b4643757e5b..c5b879628af 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2991,6 +2991,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); @@ -3050,7 +3060,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); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 63b5f35f954..e2239161f66 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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) @@ -2403,6 +2410,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 *-----------------------------------------------------------*/ @@ -2431,6 +2474,9 @@ void updateHomePosition(void) 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()); } } @@ -2878,6 +2924,12 @@ bool saveNonVolatileWaypointList(void) return true; } +#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) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf54897bcaa..d5e520a4472 100755 --- a/src/main/navigation/navigation.h +++ b/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) diff --git a/src/main/target/common.h b/src/main/target/common.h index 95d55be5360..08876545592 100755 --- a/src/main/target/common.h +++ b/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 From 9b42f1e440ec1a59dd51c8b219b672753ab1bd91 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 29 Jul 2020 10:22:09 -0400 Subject: [PATCH 2/4] Merge Safehome feature from development repo --- src/main/navigation/navigation.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e2239161f66..d194d654723 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2924,6 +2924,8 @@ bool saveNonVolatileWaypointList(void) return true; } +#endif + #if defined(USE_SAFE_HOME) void resetSafeHomes(void) From 1af89571a15b33a6d688fb78f3fcf34d574d381c Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 29 Jul 2020 10:23:14 -0400 Subject: [PATCH 3/4] Merge Safehome feature from development repo --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d194d654723..b965190bc71 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -18,7 +18,7 @@ #include #include #include - +#include #include "platform.h" From 143e2e9e87e8cbd25749097c36556d682453f042 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 29 Jul 2020 14:57:17 -0400 Subject: [PATCH 4/4] Remove set nav_rth_home_offset_* to avoid confusion --- docs/Safehomes.md | 13 ------------- src/main/fc/settings.yaml | 10 ---------- src/main/navigation/navigation.c | 12 +----------- src/main/navigation/navigation.h | 2 -- 4 files changed, 1 insertion(+), 36 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index f54efbe2c4f..58c2cd76560 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -10,19 +10,6 @@ For airplanes, the landing procedure is explained very well by Pawel Spychalski 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. -There are two existing settings that can be used change the home landing point: - -setting | explanation -------- | ----------- -nav_rth_home_offset_distance | Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables) -nav_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, - -However, these settings will likely work in one place. If you fly at a different field or the same field, but a different location (because of wind, sun, other pilots), the point identified by the distance and direction, might not be safe. - -## Home Offset and Home Direction - -If `nav_rth_home_offset_distance` is not zero, this takes precedence over the safehome approach. Safehomes are ignored when this is the case. - ## 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. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 389b0b02189..c1b712eb11f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2107,16 +2107,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" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b965190bc71..165039ff2c9 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -122,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 @@ -2467,18 +2465,10 @@ 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()) + if (!foundNearbySafeHome()) #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - } } } } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d5e520a4472..1d8b853902f 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -194,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 {