From dd198b2cda183b74a5d6d634e188381b92efcbff Mon Sep 17 00:00:00 2001 From: Ivan Efimov Date: Tue, 5 Dec 2023 04:54:10 -0600 Subject: [PATCH] Keep I-terms at zero throttle for fixed wings (#13127) * Keep Iterms at zero throttle for fixed wing mixer * Ctz suggestions * Update src/main/fc/core.c based on ladvinap suggestion Co-authored-by: Petr Ledvina --------- Co-authored-by: Petr Ledvina --- src/main/fc/core.c | 27 +++++++-------------------- 1 file changed, 7 insertions(+), 20 deletions(-) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index c2cc8d853c4..efbbdb7bd4e 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -768,30 +768,17 @@ bool processRx(timeUs_t currentTimeUs) failsafeStartMonitoring(); } - const throttleStatus_e throttleStatus = calculateThrottleStatus(); + const bool throttleActive = calculateThrottleStatus() != THROTTLE_LOW; const uint8_t throttlePercent = calculateThrottlePercentAbs(); - const bool launchControlActive = isLaunchControlActive(); + airmodeIsActivated = airmodeIsEnabled() && ARMING_FLAG(ARMED) && throttlePercent >= rxConfig()->airModeActivateThreshold && !launchControlActive; - if (airmodeIsEnabled() && ARMING_FLAG(ARMED) && !launchControlActive) { - if (throttlePercent >= rxConfig()->airModeActivateThreshold) { - airmodeIsActivated = true; // Prevent iterm from being reset - } - } else { - airmodeIsActivated = false; - } - - /* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. - This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ - if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated && !launchControlActive) { - pidSetItermReset(true); - if (currentPidProfile->pidAtMinThrottle) - pidStabilisationState(PID_STABILISATION_ON); - else - pidStabilisationState(PID_STABILISATION_OFF); - } else { + if (ARMING_FLAG(ARMED) && (airmodeIsActivated || throttleActive || launchControlActive || isFixedWing())) { pidSetItermReset(false); pidStabilisationState(PID_STABILISATION_ON); + } else { + pidSetItermReset(true); + pidStabilisationState(currentPidProfile->pidAtMinThrottle ? PID_STABILISATION_ON : PID_STABILISATION_OFF); } #ifdef USE_RUNAWAY_TAKEOFF @@ -814,7 +801,7 @@ bool processRx(timeUs_t currentTimeUs) // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; - if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (throttleStatus != THROTTLE_LOW)) { // are motors running? + if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || throttleActive) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))