diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 241f426052..f19c833a1a 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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 } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 0fb2f40560..e837ad7617 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -176,41 +176,82 @@ typedef enum { NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; +// This enum is used to keep values in blackbox logs stable, so we can +// freely change navigationFSMState_t. typedef enum { - NAV_STATE_UNDEFINED = 0, // 0 + NAV_PERSISTENT_ID_UNDEFINED = 0, // 0 - NAV_STATE_IDLE, // 1 + NAV_PERSISTENT_ID_IDLE, // 1 - NAV_STATE_ALTHOLD_INITIALIZE, // 2 - NAV_STATE_ALTHOLD_IN_PROGRESS, // 3 + NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE, // 2 + NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS, // 3 - NAV_STATE_POSHOLD_3D_INITIALIZE, // 6 - NAV_STATE_POSHOLD_3D_IN_PROGRESS, // 7 + NAV_PERSISTENT_ID_UNUSED_1, // 4, was NAV_STATE_POSHOLD_2D_INITIALIZE + NAV_PERSISTENT_ID_UNUSED_2, // 5, was NAV_STATE_POSHOLD_2D_IN_PROGRESS - NAV_STATE_RTH_INITIALIZE, // 8 - NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // 9 - NAV_STATE_RTH_HEAD_HOME, // 10 - NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, // 11 - NAV_STATE_RTH_LANDING, // 12 - NAV_STATE_RTH_FINISHING, // 13 - NAV_STATE_RTH_FINISHED, // 14 + NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE, // 6 + NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS, // 7 - NAV_STATE_WAYPOINT_INITIALIZE, // 15 - NAV_STATE_WAYPOINT_PRE_ACTION, // 16 - NAV_STATE_WAYPOINT_IN_PROGRESS, // 17 - NAV_STATE_WAYPOINT_REACHED, // 18 - NAV_STATE_WAYPOINT_NEXT, // 19 - NAV_STATE_WAYPOINT_FINISHED, // 20 - NAV_STATE_WAYPOINT_RTH_LAND, // 21 + NAV_PERSISTENT_ID_RTH_INITIALIZE, // 8 + NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, // 9 + NAV_PERSISTENT_ID_RTH_HEAD_HOME, // 10 + NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING, // 11 + NAV_PERSISTENT_ID_RTH_LANDING, // 12 + NAV_PERSISTENT_ID_RTH_FINISHING, // 13 + NAV_PERSISTENT_ID_RTH_FINISHED, // 14 - NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 22 - NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 23 - NAV_STATE_EMERGENCY_LANDING_FINISHED, // 24 + NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE, // 15 + NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION, // 16 + NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS, // 17 + NAV_PERSISTENT_ID_WAYPOINT_REACHED, // 18 + NAV_PERSISTENT_ID_WAYPOINT_NEXT, // 19 + NAV_PERSISTENT_ID_WAYPOINT_FINISHED, // 20 + NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND, // 21 - NAV_STATE_LAUNCH_INITIALIZE, // 25 - NAV_STATE_LAUNCH_WAIT, // 26 - NAV_STATE_LAUNCH_MOTOR_DELAY, // 27 - NAV_STATE_LAUNCH_IN_PROGRESS, // 28 + NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, // 22 + NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS, // 23 + NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED, // 24 + + NAV_PERSISTENT_ID_LAUNCH_INITIALIZE, // 25 + NAV_PERSISTENT_ID_LAUNCH_WAIT, // 26 + NAV_PERSISTENT_ID_UNUSED_3, // 27, was NAV_STATE_LAUNCH_MOTOR_DELAY + NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS, // 28 +} navigationPersistentId_e; + +typedef enum { + NAV_STATE_UNDEFINED = 0, + + NAV_STATE_IDLE, + + NAV_STATE_ALTHOLD_INITIALIZE, + NAV_STATE_ALTHOLD_IN_PROGRESS, + + NAV_STATE_POSHOLD_3D_INITIALIZE, + NAV_STATE_POSHOLD_3D_IN_PROGRESS, + + NAV_STATE_RTH_INITIALIZE, + NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, + NAV_STATE_RTH_HEAD_HOME, + NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + NAV_STATE_RTH_LANDING, + NAV_STATE_RTH_FINISHING, + NAV_STATE_RTH_FINISHED, + + NAV_STATE_WAYPOINT_INITIALIZE, + NAV_STATE_WAYPOINT_PRE_ACTION, + NAV_STATE_WAYPOINT_IN_PROGRESS, + NAV_STATE_WAYPOINT_REACHED, + NAV_STATE_WAYPOINT_NEXT, + NAV_STATE_WAYPOINT_FINISHED, + NAV_STATE_WAYPOINT_RTH_LAND, + + NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, + NAV_STATE_EMERGENCY_LANDING_FINISHED, + + NAV_STATE_LAUNCH_INITIALIZE, + NAV_STATE_LAUNCH_WAIT, + NAV_STATE_LAUNCH_IN_PROGRESS, NAV_STATE_COUNT, } navigationFSMState_t; @@ -243,6 +284,7 @@ typedef enum { } navigationFSMStateFlags_t; typedef struct { + navigationPersistentId_e persistentId; navigationFSMEvent_t (*onEntry)(navigationFSMState_t previousState); uint32_t timeoutMs; navSystemStatus_State_e mwState; @@ -261,6 +303,7 @@ typedef struct { typedef struct { /* Flags and navigation system state */ navigationFSMState_t navState; + navigationPersistentId_e navPersistentId; navigationFlags_t flags;