mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
WP Mission RTH Change (#6524)
* 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
GitHub
parent
459645801b
commit
f2692f322b
5 changed files with 51 additions and 99 deletions
137
src/main/navigation/navigation.c
Executable file → Normal file
137
src/main/navigation/navigation.c
Executable file → Normal 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 void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
|
||||
static navigationFSMEvent_t nextForNonGeoStates(void);
|
||||
static bool isWaypointMissionValid(void);
|
||||
|
||||
void initializeRTHSanityChecker(const fpVector3_t * pos);
|
||||
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_FINISHED(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_IN_PROGRESS(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,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[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_ERROR] = 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_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[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,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
|
||||
[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_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
|
||||
[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_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,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
|
||||
[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_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
[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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -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 ************************************************/
|
||||
[NAV_STATE_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 ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
|
||||
// Wait until target heading is reached (with 15 deg margin for error)
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
|
||||
if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
|
||||
resetLandingDetector();
|
||||
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 {
|
||||
if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
|
||||
resetLandingDetector();
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
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;
|
||||
}
|
||||
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 {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
|
@ -1549,10 +1519,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
return nextForNonGeoStates();
|
||||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
posControl.rthState.rthInitialDistance = posControl.homeDistance;
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH;
|
||||
};
|
||||
|
||||
UNREACHABLE();
|
||||
|
|
@ -1598,21 +1565,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
case NAV_WP_ACTION_JUMP:
|
||||
case NAV_WP_ACTION_SET_HEAD:
|
||||
case NAV_WP_ACTION_SET_POI:
|
||||
UNREACHABLE();
|
||||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||
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;
|
||||
UNREACHABLE();
|
||||
}
|
||||
}
|
||||
/* 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_SET_HEAD:
|
||||
case NAV_WP_ACTION_SET_POI:
|
||||
UNREACHABLE();
|
||||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
|
||||
}
|
||||
else {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME;
|
||||
}
|
||||
UNREACHABLE();
|
||||
|
||||
case NAV_WP_ACTION_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)
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
return navGetCurrentStateFlags() & NAV_CTL_EMERG;
|
||||
|
|
@ -3681,9 +3625,12 @@ bool navigationIsFlyingAutonomousMode(void)
|
|||
|
||||
bool navigationRTHAllowsLanding(void)
|
||||
{
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)
|
||||
return true;
|
||||
// WP mission RTH landing setting
|
||||
if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
|
||||
return posControl.waypointList[posControl.waypointCount - 1].p1 > 0;
|
||||
}
|
||||
|
||||
// normal RTH landing setting
|
||||
navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
|
||||
return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
|
||||
(allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue