From 3a599d98423a72abae7bfec553fa7ad30d42f6b1 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Tue, 23 Feb 2021 00:02:59 +0000 Subject: [PATCH 01/11] New safehome options to control when safehomes are applied --- docs/Safehomes.md | 12 ++++-- docs/Settings.md | 1 + src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 8 ++++ src/main/flight/failsafe.c | 16 +++++--- src/main/io/osd.c | 14 +++---- src/main/navigation/navigation.c | 66 ++++++++++++++++++++---------- src/main/navigation/navigation.h | 15 +++++-- 8 files changed, 91 insertions(+), 42 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index b3ed76403cb..6402d8275b1 100644 --- a/docs/Safehomes.md +++ b/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. -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. ## CLI command `safehome` to manage safehomes diff --git a/docs/Settings.md b/docs/Settings.md index e0a8c522421..5ce43ec94c4 100644 --- a/docs/Settings.md +++ b/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. | diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index d17d3df2901..3563889474a 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/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, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a49d69ff33d..e08b25a6a6f 100644 --- a/src/main/fc/settings.yaml +++ b/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" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 6ac2783c187..8e16842e70d 100644 --- a/src/main/flight/failsafe.c +++ b/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; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3612ef00d5a..f1cfe24ea6e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -785,7 +785,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,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); @@ -3381,7 +3381,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,8 +3391,8 @@ 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; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 01e83e70b79..cf1b7c1ea65 100755 --- a/src/main/navigation/navigation.c +++ b/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 -uint32_t safehome_distance; // distance to the selected safehome +int8_t safehome_index; // -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); @@ -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) -/******************************************************* - * 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; + } } /*********************************************************** * 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++) { @@ -2359,20 +2378,19 @@ bool foundNearbySafeHome(void) distance_to_current = calculateDistanceToDestination(¤tSafeHome); 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 @@ -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()); + 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); } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 50ba6b2632f..59c784e4021 100755 --- a/src/main/navigation/navigation.h +++ b/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 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 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) @@ -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) From 3aa2edf5d7913ae8f2c545647cd00d263365eb13 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Tue, 23 Feb 2021 00:35:27 +0000 Subject: [PATCH 02/11] cleanup --- src/main/navigation/navigation.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cf1b7c1ea65..9e48563ee7d 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -77,7 +77,7 @@ fpVector3_t original_rth_home; // the original rth home - save it, sin radar_pois_t radar_pois[RADAR_MAX_POIS]; #if defined(USE_SAFE_HOME) -int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +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. @@ -2416,7 +2416,9 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - findNearestSafeHome(); + if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { + 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 From aba1790764662969d2ff0da1c4e75fe98361416b Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 24 Feb 2021 02:05:36 +0000 Subject: [PATCH 03/11] add OSD messages --- src/main/io/osd.c | 22 ++++++++++++++++++++++ src/main/io/osd.h | 6 +++++- src/main/navigation/navigation.h | 1 + 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f1cfe24ea6e..06b3eedf8e3 100755 --- a/src/main/io/osd.c +++ b/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) { @@ -3354,6 +3363,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 +3373,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) { @@ -3397,6 +3413,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(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d740b5de521..0433a926fa6 100755 --- a/src/main/io/osd.h +++ b/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 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 59c784e4021..baecd6d32b1 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -61,6 +61,7 @@ PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); 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 findNearestSafeHome(void); // Find nearest safehome From f5b62f02832b9b06852b1ace292f93922b90d806 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 24 Feb 2021 22:40:44 +0000 Subject: [PATCH 04/11] bug fixes --- docs/Safehomes.md | 4 ++++ src/main/navigation/navigation.c | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 6402d8275b1..17c87420733 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -43,6 +43,10 @@ If a safehome is selected, an additional message appears: 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. +## 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 `safehome` - List all safehomes diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9e48563ee7d..a23e9eec483 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3133,7 +3133,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); + 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 @@ -3567,6 +3567,7 @@ void activateForcedRTH(void) { abortFixedWingLaunch(); posControl.flags.forcedRTHActivated = true; + checkSafeHomeState(true); navProcessFSMEvents(selectNavEventFromBoxModeInput()); } From 34422ffe8799f6ac9a0e07995ccf11168606b7c4 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Feb 2021 14:44:33 +0000 Subject: [PATCH 05/11] warning safehome off --- src/main/io/osd.c | 18 +++++++++++------- src/main/navigation/navigation.c | 4 +--- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 06b3eedf8e3..89296fb79b8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3095,13 +3095,17 @@ static void osdShowArmed(void) } y += 4; #if defined (USE_SAFE_HOME) - 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_index); - // 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 { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a23e9eec483..cc6b161e54c 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2416,9 +2416,7 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { - findNearestSafeHome(); - } + 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 From f4e5fd765f8f0a6cf8ad7f264369cf7fd2347aa8 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Feb 2021 15:02:00 +0000 Subject: [PATCH 06/11] add doc changes --- docs/Safehomes.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 17c87420733..6a5492ec710 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -43,6 +43,8 @@ If a safehome is selected, an additional message appears: 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. From 1feb1c318ef2e5c90ca194e4e4bca75c8ed98fdd Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Sun, 28 Feb 2021 23:04:24 +0000 Subject: [PATCH 07/11] set home position info after updating it --- src/main/navigation/navigation.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cc6b161e54c..6172a9a1b8e 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -233,6 +233,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); +void updateHomePosition(void); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -2354,6 +2355,8 @@ void checkSafeHomeState(bool shouldBeEnabled) 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(); } /*********************************************************** From 81a41f89882765322483c9bf184fa8ac94435fc8 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Mon, 1 Mar 2021 13:35:55 +0000 Subject: [PATCH 08/11] add v2 changes --- docs/Safehomes.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 6a5492ec710..e8056595187 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -12,18 +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 remembered. The arming home location remains 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. -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. +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. 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. +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. -You can define up to 8 safehomes for different locations you fly at. +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. From dbe39d3f3bc7d964ccd3acdab56035b61a5b3ab9 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Mar 2021 17:53:24 +0000 Subject: [PATCH 09/11] add more info to safehome_usage_mode --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5ce43ec94c4..ffe03cd4971 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -444,7 +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. | +| 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 | | | | 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. | From c7e6a74c6d97a2df1914c80f4051fa8869c26088 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Mar 2021 18:05:29 +0000 Subject: [PATCH 10/11] put changes in settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e08b25a6a6f..81b5c771db9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2195,7 +2195,7 @@ groups: min: 0 max: 65000 - name: safehome_usage_mode - description: "Used to control when safehomes will be used." + 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 From f6d252ad74415a61a3892acd593163d0e2f56070 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Sat, 27 Mar 2021 00:35:53 +0000 Subject: [PATCH 11/11] change messages for loitering over safehome --- src/main/io/osd.c | 7 ++++++- src/main/io/osd.h | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 89296fb79b8..3255ae79e2e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -780,7 +780,7 @@ static const char * osdFailsafeInfoMessage(void) #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (safehome_applied) { + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); } return NULL; @@ -821,6 +821,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); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0433a926fa6..aeca8340b5c 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -100,6 +100,7 @@ #if defined(USE_SAFE_HOME) #define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif typedef enum {