mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 06:45:14 +03:00
Merge pull request #3274 from iNavFlight/agh_fix_nav_state_numbers
Fix NAV_STATE enum values
This commit is contained in:
Konstantin Sharlaimov
GitHub
commit
e45cf00820
2 changed files with 101 additions and 30 deletions
|
|
@ -212,6 +212,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi
|
|||
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
||||
/** Idle state ******************************************************/
|
||||
[NAV_STATE_IDLE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_IDLE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_IDLE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = 0,
|
||||
|
|
@ -230,6 +231,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
|
||||
/** ALTHOLD mode ***************************************************/
|
||||
[NAV_STATE_ALTHOLD_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT,
|
||||
|
|
@ -244,6 +246,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_ALTHOLD_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT,
|
||||
|
|
@ -262,6 +265,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
|
||||
/** POSHOLD_3D mode ************************************************/
|
||||
[NAV_STATE_POSHOLD_3D_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
|
||||
|
|
@ -276,6 +280,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_POSHOLD_3D_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
|
||||
|
|
@ -294,6 +299,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
|
||||
/** RTH_3D mode ************************************************/
|
||||
[NAV_STATE_RTH_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
|
|
@ -310,6 +316,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_RTH_CLIMB_TO_SAFE_ALT] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt
|
||||
|
|
@ -326,6 +333,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_RTH_HEAD_HOME] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
|
|
@ -343,6 +351,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
.timeoutMs = 500,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
|
|
@ -360,6 +369,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_RTH_LANDING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
|
||||
.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_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
|
|
@ -377,6 +387,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_RTH_FINISHING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
|
|
@ -390,6 +401,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_RTH_FINISHED] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
|
|
@ -407,6 +419,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
|
||||
/** WAYPOINT mode ************************************************/
|
||||
[NAV_STATE_WAYPOINT_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
|
|
@ -422,6 +435,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
|
|
@ -437,6 +451,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
|
|
@ -455,6 +470,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_REACHED] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_REACHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
|
|
@ -475,6 +491,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_RTH_LAND] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
.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,
|
||||
|
|
@ -493,6 +510,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_NEXT] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_NEXT,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_NEXT,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
|
|
@ -506,6 +524,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_FINISHED] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
|
|
@ -523,6 +542,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
|
||||
/** EMERGENCY LANDING ************************************************/
|
||||
[NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
|
|
@ -538,6 +558,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
|
|
@ -553,6 +574,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_EMERGENCY_LANDING_FINISHED] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
|
|
@ -566,6 +588,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_LAUNCH_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_LAUNCH_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE,
|
||||
|
|
@ -580,6 +603,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_LAUNCH_WAIT] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_LAUNCH_WAIT,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
|
||||
|
|
@ -595,6 +619,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
[NAV_STATE_LAUNCH_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
|
||||
|
|
@ -1166,7 +1191,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
|
|||
|
||||
if (isFixedWingLaunchDetected()) {
|
||||
enableFixedWingLaunchController(currentTimeUs);
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_MOTOR_DELAY
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
|
||||
}
|
||||
|
||||
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
|
||||
|
|
@ -1196,7 +1221,10 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
|
|||
navigationFSMState_t previousState;
|
||||
|
||||
previousState = posControl.navState;
|
||||
posControl.navState = newState;
|
||||
if (posControl.navState != newState) {
|
||||
posControl.navState = newState;
|
||||
posControl.navPersistentId = navFSM[newState].persistentId;
|
||||
}
|
||||
return previousState;
|
||||
}
|
||||
|
||||
|
|
@ -2524,7 +2552,7 @@ void updateWaypointsAndNavigationMode(void)
|
|||
switchNavigationFlightModes();
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navCurrentState = (int16_t)posControl.navState;
|
||||
navCurrentState = (int16_t)posControl.navPersistentId;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue