Unverified Commit 852ef277 authored by Konstantin Sharlaimov's avatar Konstantin Sharlaimov Committed by GitHub
Browse files

Merge pull request #6655 from tonyyng/delayed_safehome

Delayed safehome
parents 7f04e0f6 e5b2fef6
......@@ -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
......
......@@ -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. |
......
......@@ -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,
};
......
......@@ -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
......
......@@ -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;
......
......@@ -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();
......
......@@ -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,
......
......@@ -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);
}
......
......@@ -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)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment