Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +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

33
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;