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

Update navigation.c

Browse source
This commit is contained in:
breadoven 2024-04-11 22:57:28 +01:00
parent 80adbbc1e5
commit db13d5da54
1 changed files with 6 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

39
src/main/navigation/navigation.c
View file

@ -1142,7 +1142,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwError = MW_NAV_ERROR_NONE, .mwError = MW_NAV_ERROR_NONE,
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
} }
}, },
@ -2043,21 +2042,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
#ifdef USE_FW_AUTOLAND if (landEvent == NAV_FSM_EVENT_SUCCESS) {
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else
#endif
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
// Landing controller returned success - invoke RTH finishing state and finish the waypoint // Landing controller returned success - invoke RTH finishing state and finish the waypoint
navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState); navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
return NAV_FSM_EVENT_SUCCESS;
}
else {
return NAV_FSM_EVENT_NONE;
} }
return landEvent;
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
@ -2376,12 +2366,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
} }
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) { if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
resetPositionController(); resetPositionController();
posControl.cruise.course = posControl.fwLandState.landingDirection; posControl.cruise.course = posControl.fwLandState.landingDirection;
@ -2427,12 +2411,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -2441,11 +2419,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
{ {
UNUSED(previousState); UNUSED(previousState);
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SUCCESS;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
@ -4083,7 +4056,7 @@ bool isNavHoldPositionActive(void)
// Also hold position during emergency landing if position valid // Also hold position during emergency landing if position valid
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_POSHOLD_MODE) ||
(posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
navigationIsExecutingAnEmergencyLanding(); navigationIsExecutingAnEmergencyLanding();
} }
@ -4123,7 +4096,7 @@ bool isWaypointNavTrackingActive(void)
// is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP. // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
// (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called) // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
return FLIGHT_MODE(NAV_WP_MODE) return FLIGHT_MODE(NAV_WP_MODE)
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
} }
@ -5011,7 +4984,7 @@ bool navigationRTHAllowsLanding(void)
return false; return false;
} }
#endif #endif
// WP mission RTH landing setting // WP mission RTH landing setting
if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0; return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;