Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

WP Mission RTH Change (#6524)

Browse source 
* Initial changes

Change WP mission RTH to normal RTH mode

* Cleanup and remove PR6050

* Clean up

* Persistent ID fix + existing bug fix

* Add system message

* Revert "Add system message"

This reverts commit 28ab7f33ba.

* Add System Message

* Update navigation.c
This commit is contained in:
breadoven 2021-04-28 16:48:28 +01:00 • committed by GitHub
parent 459645801b
commit f2692f322b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 51 additions and 99 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/io/osd.c
View file

@ -1710,7 +1710,7 @@ static bool osdDrawSingleElement(uint8_t item)
else if (FLIGHT_MODE(MANUAL_MODE)) else if (FLIGHT_MODE(MANUAL_MODE))
p = "MANU"; p = "MANU";
else if (FLIGHT_MODE(NAV_RTH_MODE)) else if (FLIGHT_MODE(NAV_RTH_MODE))
p = "RTH "; p = isWaypointMissionRTHActive() ? "WRTH" : "RTH ";
else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
p = "HOLD"; p = "HOLD";
else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
@ -3612,6 +3612,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
} }
} else { } else {
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
if (isWaypointMissionRTHActive()) {
// if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
}
if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
// Countdown display for remaining Waypoints // 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);

1
src/main/io/osd.h Executable file → Normal file
View file

@ -87,6 +87,7 @@
#define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT" #define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT"
#define OSD_MSG_TO_WP "TO WP" #define OSD_MSG_TO_WP "TO WP"
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP MODE TO EXIT RTH"
#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" #define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING"
#define OSD_MSG_LANDING "LANDING" #define OSD_MSG_LANDING "LANDING"
#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" #define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME"

137
src/main/navigation/navigation.c Executable file → Normal file
View file

@ -238,6 +238,7 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
static navigationFSMEvent_t nextForNonGeoStates(void); static navigationFSMEvent_t nextForNonGeoStates(void);
static bool isWaypointMissionValid(void);
void initializeRTHSanityChecker(const fpVector3_t * pos); void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void); bool validateRTHSanityChecker(void);
@ -273,7 +274,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
@ -665,11 +665,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwState = MW_NAV_STATE_PROCESS_NEXT, .mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE, .mwError = MW_NAV_ERROR_NONE,
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
} }
}, },
@ -703,19 +704,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwState = MW_NAV_STATE_PROCESS_NEXT, .mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE, .mwError = MW_NAV_ERROR_NONE,
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
} }
}, },
@ -794,27 +794,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
} }
}, },
[NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
/** EMERGENCY LANDING ************************************************/ /** EMERGENCY LANDING ************************************************/
[NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = { [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
@ -1331,30 +1310,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// If position ok OR within valid timeout - continue // If position ok OR within valid timeout - continue
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
// Wait until target heading is reached (with 15 deg margin for error) if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
if (STATE(FIXED_WING_LEGACY)) {
resetLandingDetector(); resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
}
else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else { else {
if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
resetLandingDetector(); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); return NAV_FSM_EVENT_NONE;
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
}
else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
} }
} else { } else {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
@ -1549,10 +1519,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
return nextForNonGeoStates(); return nextForNonGeoStates();
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
posControl.rthState.rthInitialDistance = posControl.homeDistance; return NAV_FSM_EVENT_SWITCH_TO_RTH;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
}; };
UNREACHABLE(); UNREACHABLE();
@ -1598,21 +1565,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_JUMP:
case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_HEAD:
case NAV_WP_ACTION_SET_POI: case NAV_WP_ACTION_SET_POI:
UNREACHABLE();
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { UNREACHABLE();
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY))
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
else
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
break;
} }
} }
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
@ -1637,15 +1591,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_JUMP:
case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_HEAD:
case NAV_WP_ACTION_SET_POI: case NAV_WP_ACTION_SET_POI:
UNREACHABLE();
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) { UNREACHABLE();
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
}
else {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME;
}
case NAV_WP_ACTION_LAND: case NAV_WP_ACTION_LAND:
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1725,14 +1672,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
} }
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
{
UNUSED(previousState);
const navigationFSMEvent_t hoverEvent = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(previousState);
return hoverEvent;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
{ {
// TODO: // TODO:
@ -3656,6 +3595,11 @@ rthState_e getStateOfForcedRTH(void)
} }
} }
bool isWaypointMissionRTHActive(void)
{
return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated);
}
bool navigationIsExecutingAnEmergencyLanding(void) bool navigationIsExecutingAnEmergencyLanding(void)
{ {
return navGetCurrentStateFlags() & NAV_CTL_EMERG; return navGetCurrentStateFlags() & NAV_CTL_EMERG;
@ -3681,9 +3625,12 @@ bool navigationIsFlyingAutonomousMode(void)
bool navigationRTHAllowsLanding(void) bool navigationRTHAllowsLanding(void)
{ {
if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND) // WP mission RTH landing setting
return true; if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
return posControl.waypointList[posControl.waypointCount - 1].p1 > 0;
}
// normal RTH landing setting
navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
return allow == NAV_RTH_ALLOW_LANDING_ALWAYS || return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
(allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));

1
src/main/navigation/navigation.h
View file

@ -520,6 +520,7 @@ bool navigationIsExecutingAnEmergencyLanding(void);
* or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
*/ */
bool navigationRTHAllowsLanding(void); bool navigationRTHAllowsLanding(void);
bool isWaypointMissionRTHActive(void);
bool isNavLaunchEnabled(void); bool isNavLaunchEnabled(void);
bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchDetected(void);

4
src/main/navigation/navigation_private.h
View file

@ -141,7 +141,6 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME,
NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD,
NAV_FSM_EVENT_SWITCH_TO_CRUISE, NAV_FSM_EVENT_SWITCH_TO_CRUISE,
@ -200,7 +199,7 @@ typedef enum {
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36,
NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37, NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME
} navigationPersistentId_e; } navigationPersistentId_e;
@ -232,7 +231,6 @@ typedef enum {
NAV_STATE_WAYPOINT_NEXT, NAV_STATE_WAYPOINT_NEXT,
NAV_STATE_WAYPOINT_FINISHED, NAV_STATE_WAYPOINT_FINISHED,
NAV_STATE_WAYPOINT_RTH_LAND, NAV_STATE_WAYPOINT_RTH_LAND,
NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME,
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,