From 11bc2a464816b754242f517ecae8ca24d8d31ecc Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 3 Jun 2021 17:28:04 +0100 Subject: [PATCH 1/3] Initial build --- src/main/fc/fc_core.c | 2 +- src/main/navigation/navigation.c | 8 ++++---- src/main/navigation/navigation.h | 11 +++++++++-- src/main/navigation/navigation_fw_launch.c | 19 ++++++++++++------- src/main/navigation/navigation_private.h | 3 +-- 5 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 34dc4e670..1425b4e49 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -845,7 +845,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; - if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { + if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() > FW_LAUNCH_START))) { flightTime += cycleTime; armTime += cycleTime; updateAccExtremes(); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 37a6c3c00..ef06defda 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1719,7 +1719,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF const timeUs_t currentTimeUs = micros(); UNUSED(previousState); - if (isFixedWingLaunchDetected()) { + if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) { enableFixedWingLaunchController(currentTimeUs); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS } @@ -1740,7 +1740,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi { UNUSED(previousState); - if (isFixedWingLaunchFinishedOrAborted()) { + if (fixedWingLaunchStatus() >= FW_LAUNCH_END_ABORT) { return NAV_FSM_EVENT_SUCCESS; } @@ -2579,11 +2579,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti } else { - /* + /* * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 * In other words, when altitude is reached, allow it only to shrink */ - if (navConfig()->general.max_altitude > 0 && + if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0 ) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b71f932f9..cb3656fc8 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -145,6 +145,14 @@ typedef enum { ON_FW_SPIRAL, } navRTHClimbFirst_e; +typedef enum { + FW_LAUNCH_START, + FW_LAUNCH_DETECTED, + FW_LAUNCH_FLYING, + FW_LAUNCH_END_ABORT, + FW_LAUNCH_END_SUCCESS, +} navFwLaunchStatus_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -533,8 +541,7 @@ bool navigationRTHAllowsLanding(void); bool isWaypointMissionRTHActive(void); bool isNavLaunchEnabled(void); -bool isFixedWingLaunchDetected(void); -bool isFixedWingLaunchFinishedOrAborted(void); +uint8_t fixedWingLaunchStatus(void); const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 086d9a378..f3fa4e6e3 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -111,6 +111,7 @@ typedef struct fixedWingLaunchData_s { } fixedWingLaunchData_t; static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; +static uint8_t launchStatus; static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { @@ -262,6 +263,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t curren { UNUSED(currentTimeUs); + launchStatus = launchStatus == FW_LAUNCH_FLYING ? FW_LAUNCH_END_SUCCESS : FW_LAUNCH_END_ABORT; + return FW_LAUNCH_EVENT_NONE; } @@ -269,6 +272,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs { UNUSED(currentTimeUs); + launchStatus = FW_LAUNCH_START; + if (!isThrottleLow()) { if (isThrottleIdleEnabled()) { return FW_LAUNCH_EVENT_SUCCESS; @@ -336,6 +341,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); + + launchStatus = FW_LAUNCH_DETECTED; + // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY applyThrottleIdleLogic(false); @@ -403,6 +411,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; + launchStatus = FW_LAUNCH_FLYING; // considered a success if this far in launch sequence + if (areSticksDeflectedMoreThanPosHoldDeadband()) { return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state } @@ -450,19 +460,14 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs) setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); } -bool isFixedWingLaunchDetected(void) -{ - return fwLaunch.currentState >= FW_LAUNCH_STATE_DETECTED; -} - void enableFixedWingLaunchController(timeUs_t currentTimeUs) { setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); } -bool isFixedWingLaunchFinishedOrAborted(void) +uint8_t fixedWingLaunchStatus(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; + return launchStatus; } void abortFixedWingLaunch(void) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index b26299e70..4ba2ed005 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -455,9 +455,8 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos); /* Fixed-wing launch controller */ void resetFixedWingLaunchController(timeUs_t currentTimeUs); -bool isFixedWingLaunchDetected(void); void enableFixedWingLaunchController(timeUs_t currentTimeUs); -bool isFixedWingLaunchFinishedOrAborted(void); +uint8_t fixedWingLaunchStatus(void); void abortFixedWingLaunch(void); void applyFixedWingLaunchController(timeUs_t currentTimeUs); From d5b19cad0b5fb006e05e8341814ecabca03014a9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 1 Jul 2021 13:14:30 +0100 Subject: [PATCH 2/3] Changed method Replaced status flag with additional states instead --- src/main/fc/fc_core.c | 2 +- src/main/navigation/navigation.c | 2 +- src/main/navigation/navigation.h | 10 +-- src/main/navigation/navigation_fw_launch.c | 92 +++++++++++----------- src/main/navigation/navigation_private.h | 1 - 5 files changed, 54 insertions(+), 53 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 5536df329..dd4fe3d1b 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -845,7 +845,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; - if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() > FW_LAUNCH_START))) { + if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) { flightTime += cycleTime; armTime += cycleTime; updateAccExtremes(); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 533450b7b..b7a554277 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1734,7 +1734,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi { UNUSED(previousState); - if (fixedWingLaunchStatus() >= FW_LAUNCH_END_ABORT) { + if (fixedWingLaunchStatus() >= FW_LAUNCH_ABORTED) { return NAV_FSM_EVENT_SUCCESS; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cefd59496..64a026997 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -145,12 +145,10 @@ typedef enum { ON_FW_SPIRAL, } navRTHClimbFirst_e; -typedef enum { - FW_LAUNCH_START, - FW_LAUNCH_DETECTED, - FW_LAUNCH_FLYING, - FW_LAUNCH_END_ABORT, - FW_LAUNCH_END_SUCCESS, +typedef enum { // keep aligned with fixedWingLaunchState_t + FW_LAUNCH_DETECTED = 4, + FW_LAUNCH_ABORTED = 9, + FW_LAUNCH_FLYING = 10, } navFwLaunchStatus_e; typedef struct positionEstimationConfig_s { diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 79f67e840..ec9f1af73 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -79,21 +79,21 @@ typedef enum { FW_LAUNCH_EVENT_COUNT } fixedWingLaunchEvent_t; -typedef enum { - FW_LAUNCH_STATE_IDLE = 0, - FW_LAUNCH_STATE_WAIT_THROTTLE, +typedef enum { // if changed update navFwLaunchStatus_e + FW_LAUNCH_STATE_WAIT_THROTTLE = 0, FW_LAUNCH_STATE_IDLE_MOTOR_DELAY, FW_LAUNCH_STATE_MOTOR_IDLE, FW_LAUNCH_STATE_WAIT_DETECTION, - FW_LAUNCH_STATE_DETECTED, + FW_LAUNCH_STATE_DETECTED, // 4 FW_LAUNCH_STATE_MOTOR_DELAY, FW_LAUNCH_STATE_MOTOR_SPINUP, FW_LAUNCH_STATE_IN_PROGRESS, FW_LAUNCH_STATE_FINISH, + FW_LAUNCH_STATE_ABORTED, // 9 + FW_LAUNCH_STATE_FLYING, // 10 FW_LAUNCH_STATE_COUNT } fixedWingLaunchState_t; -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); @@ -103,6 +103,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs); typedef struct fixedWingLaunchStateDescriptor_s { fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); @@ -118,18 +120,9 @@ typedef struct fixedWingLaunchData_s { static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; static bool idleMotorAboutToStart; -static uint8_t launchStatus; static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { - [FW_LAUNCH_STATE_IDLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, - .onEvent = { - - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE - }, - [FW_LAUNCH_STATE_WAIT_THROTTLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, .onEvent = { @@ -178,7 +171,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_ABORTED }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, @@ -187,7 +180,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_ABORTED }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, @@ -196,7 +189,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_ABORTED }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, @@ -204,10 +197,25 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FLYING }, .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING + }, + + [FW_LAUNCH_STATE_ABORTED] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_ABORTED, + .onEvent = { + + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE + }, + + [FW_LAUNCH_STATE_FLYING] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_FLYING, + .onEvent = { + + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE } }; @@ -275,21 +283,10 @@ static void updateRcCommand(void) /* onEntry state handlers */ -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) -{ - UNUSED(currentTimeUs); - - launchStatus = launchStatus == FW_LAUNCH_FLYING ? FW_LAUNCH_END_SUCCESS : FW_LAUNCH_END_ABORT; - - return FW_LAUNCH_EVENT_NONE; -} - static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - launchStatus = FW_LAUNCH_START; - if (!isThrottleLow()) { if (isThrottleIdleEnabled()) { return FW_LAUNCH_EVENT_SUCCESS; @@ -376,8 +373,6 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t cu { UNUSED(currentTimeUs); - launchStatus = FW_LAUNCH_DETECTED; - // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY applyThrottleIdleLogic(false); @@ -389,7 +384,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t applyThrottleIdleLogic(false); if (areSticksMoved(0, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE + return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_ABORTED } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { @@ -402,7 +397,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE + return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_ABORTED } const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); @@ -430,7 +425,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t } if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state + return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_ABORTED state } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { @@ -445,13 +440,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - launchStatus = FW_LAUNCH_FLYING; // considered a success if this far in launch sequence - - if (isRollPitchStickDeflected()) { - return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state - } - if (elapsedTimeMs > endTimeMs) { - return FW_LAUNCH_EVENT_SUCCESS; + if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected()) { + return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state } else { // make a smooth transition from the launch state to the current state for throttle and the pitch angle @@ -462,6 +452,20 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr return FW_LAUNCH_EVENT_NONE; } +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + return FW_LAUNCH_EVENT_NONE; +} + // Public methods --------------------------------------------------------------- void applyFixedWingLaunchController(timeUs_t currentTimeUs) @@ -505,12 +509,12 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs) uint8_t fixedWingLaunchStatus(void) { - return launchStatus; + return fwLaunch.currentState; } void abortFixedWingLaunch(void) { - setCurrentState(FW_LAUNCH_STATE_IDLE, 0); + setCurrentState(FW_LAUNCH_STATE_ABORTED, 0); } const char * fixedWingLaunchStateMessage(void) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index d96b73c13..86da67806 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -457,7 +457,6 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos); /* Fixed-wing launch controller */ void resetFixedWingLaunchController(timeUs_t currentTimeUs); void enableFixedWingLaunchController(timeUs_t currentTimeUs); -uint8_t fixedWingLaunchStatus(void); void abortFixedWingLaunch(void); void applyFixedWingLaunchController(timeUs_t currentTimeUs); From 163dc13065880d554b64bb9db526c8008d5deea4 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 17 Aug 2021 20:43:07 +0100 Subject: [PATCH 3/3] Update fc_core.c --- src/main/fc/fc_core.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index dd4fe3d1b..9915a2038 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -845,7 +845,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; +#if defined(USE_NAV) if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) { +#else + if (ARMING_FLAG(ARMED)) { +#endif flightTime += cycleTime; armTime += cycleTime; updateAccExtremes();