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 2023-03-17 21:55:34 +00:00
parent 2f42925ac3
commit 3b1b585ac0
1 changed files with 43 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -482,7 +482,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
@ -984,12 +984,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
resetGCSFlags();
// If surface tracking mode changed value - reset altitude controller
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
resetAltitudeController(navTerrainFollowingRequested());
}
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
setupAltitudeController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
}
@ -1017,20 +1013,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
resetGCSFlags();
if ((prevFlags & NAV_CTL_POS) == 0) {
resetPositionController();
}
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
resetAltitudeController(navTerrainFollowingRequested());
setupAltitudeController();
}
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
}
if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
{
resetPositionController();
fpVector3_t targetHoldPos;
calculateInitialHoldPosition(&targetHoldPos);
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
@ -1054,18 +1048,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
return NAV_FSM_EVENT_NONE;
}
/////////////////
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now
return NAV_FSM_EVENT_ERROR;
}
DEBUG_SET(DEBUG_CRUISE, 0, 1);
// Switch to IDLE if we do not have an healty position. Try the next iteration.
if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} // Switch to IDLE if we do not have an healty position. Try the next iteration.
}
resetPositionController();
@ -1082,9 +1078,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
const timeMs_t currentTimeMs = millis();
// Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
}
DEBUG_SET(DEBUG_CRUISE, 0, 2);
DEBUG_SET(DEBUG_CRUISE, 2, 0);
@ -1153,7 +1150,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(naviga
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
UNUSED(previousState);
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
@ -1169,17 +1166,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
// Reset altitude and position controllers if necessary
if ((prevFlags & NAV_CTL_POS) == 0) {
resetPositionController();
}
// Reset controllers
resetPositionController();
// Reset altitude controller if it was not enabled or if we are in terrain follow mode
if ((prevFlags & NAV_CTL_ALT) == 0 || posControl.flags.isTerrainFollowEnabled) {
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
resetAltitudeController(false);
setupAltitudeController();
}
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
resetAltitudeController(false);
setupAltitudeController();
// If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
@ -1506,34 +1498,32 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
{
UNUSED(previousState);
if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
if (!posControl.waypointCount || !posControl.waypointListValid) {
return NAV_FSM_EVENT_ERROR;
}
else {
// Prepare controllers
resetPositionController();
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
resetAltitudeController(false);
setupAltitudeController();
/*
Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
*/
if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
setupJumpCounters();
posControl.activeWaypointIndex = posControl.startWpIndex;
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
}
// Prepare controllers
resetPositionController();
if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
posControl.wpMissionRestart = posControl.activeWaypointIndex > posControl.startWpIndex ? !posControl.wpMissionRestart : false;
} else {
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
}
// Make sure surface tracking is not enabled - WP uses global altitude, not AGL
resetAltitudeController(false);
setupAltitudeController();
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
/* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
Using p3 minimises the risk of saving an invalid counter if a mission is aborted */
setupJumpCounters();
posControl.activeWaypointIndex = posControl.startWpIndex;
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
}
if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
posControl.wpMissionRestart = posControl.activeWaypointIndex > posControl.startWpIndex ? !posControl.wpMissionRestart : false;
} else {
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
}
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
}
static navigationFSMEvent_t nextForNonGeoStates(void)
@ -1562,8 +1552,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
posControl.wpAltitudeReached = false;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
// We use p3 as the volatile jump counter (p2 is the static value)
case NAV_WP_ACTION_JUMP:
// We use p3 as the volatile jump counter (p2 is the static value)
if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1) {
if (posControl.waypointList[posControl.activeWaypointIndex].p3 == 0) {
resetJumpCounter();