diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h index 65c5cb13e7..27a92ec681 100644 --- a/src/main/config/config_unittest.h +++ b/src/main/config/config_unittest.h @@ -50,9 +50,9 @@ bool unittest_outsideRealtimeGuardInterval; float unittest_pidLuxFloat_lastErrorForDelta[3]; float unittest_pidLuxFloat_delta1[3]; float unittest_pidLuxFloat_delta2[3]; -float unittest_pidLuxFloat_PTerm[3]; -float unittest_pidLuxFloat_ITerm[3]; -float unittest_pidLuxFloat_DTerm[3]; +float unittest_pidLuxFloat_pterm[3]; +float unittest_pidLuxFloat_iterm[3]; +float unittest_pidLuxFloat_dterm[3]; #define SET_PID_LUX_FLOAT_LOCALS(axis) \ { \ @@ -66,15 +66,15 @@ float unittest_pidLuxFloat_DTerm[3]; unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \ unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \ - unittest_pidLuxFloat_PTerm[axis] = PTerm; \ - unittest_pidLuxFloat_ITerm[axis] = ITerm; \ - unittest_pidLuxFloat_DTerm[axis] = DTerm; \ + unittest_pidLuxFloat_pterm[axis] = pterm; \ + unittest_pidLuxFloat_iterm[axis] = iterm; \ + unittest_pidLuxFloat_dterm[axis] = dterm; \ } int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3]; -int32_t unittest_pidMultiWiiRewrite_PTerm[3]; -int32_t unittest_pidMultiWiiRewrite_ITerm[3]; -int32_t unittest_pidMultiWiiRewrite_DTerm[3]; +int32_t unittest_pidMultiWiiRewrite_pterm[3]; +int32_t unittest_pidMultiWiiRewrite_iterm[3]; +int32_t unittest_pidMultiWiiRewrite_dterm[3]; #define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ { \ @@ -84,9 +84,9 @@ int32_t unittest_pidMultiWiiRewrite_DTerm[3]; #define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ { \ unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ - unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \ - unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \ - unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \ + unittest_pidMultiWiiRewrite_pterm[axis] = pterm; \ + unittest_pidMultiWiiRewrite_iterm[axis] = iterm; \ + unittest_pidMultiWiiRewrite_dterm[axis] = dterm; \ } #else diff --git a/src/main/fc/core.c b/src/main/fc/core.c index efbb73339c..701223fcb3 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -577,16 +577,16 @@ bool processRx(timeUs_t currentTimeUs) if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { - airmodeIsActivated = true; // Prevent Iterm from being reset + 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 */ + /* 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) { - pidResetITerm(); + pidResetIterm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2bb28aa052..7c7aa3f8db 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -596,8 +596,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) currentThrottleInputRange = rcCommandThrottleRange3dHigh; } if (currentTimeUs - reversalTimeUs < 250000) { - // keep ITerm zero for 250ms after motor reversal - pidResetITerm(); + // keep iterm zero for 250ms after motor reversal + pidResetIterm(); } } else { throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a6bd62e275..ebd93279af 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -117,7 +117,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .pid = { [PID_ROLL] = { 46, 45, 25, 60 }, [PID_PITCH] = { 50, 50, 27, 60 }, - [PID_YAW] = { 65, 45, 0 , 60 }, + [PID_YAW] = { 45, 100, 0, 100 }, [PID_LEVEL] = { 50, 50, 75, 0 }, [PID_MAG] = { 40, 0, 0, 0 }, }, @@ -130,12 +130,12 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_notch_hz = 0, .dterm_notch_cutoff = 0, .dterm_filter_type = FILTER_PT1, - .itermWindupPointPercent = 100, + .itermWindupPointPercent = 40, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, .feedForwardTransition = 0, - .yawRateAccelLimit = 100, + .yawRateAccelLimit = 0, .rateAccelLimit = 0, .itermThrottleThreshold = 250, .itermAcceleratorGain = 5000, @@ -150,7 +150,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .horizon_tilt_effect = 75, .horizon_tilt_expert_mode = false, .crash_limit_yaw = 200, - .itermLimit = 150, + .itermLimit = 300, .throttle_boost = 5, .throttle_boost_cutoff = 15, .iterm_rotation = true, @@ -368,7 +368,7 @@ static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float feedForwardTransition; static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; -static FAST_RAM_ZERO_INIT float ITermWindupPointInv; +static FAST_RAM_ZERO_INIT float itermWindupPointInv; static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode; static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs; static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs; @@ -395,7 +395,7 @@ static FAST_RAM_ZERO_INIT float acLimit; static FAST_RAM_ZERO_INIT float acErrorLimit; #endif -void pidResetITerm(void) +void pidResetIterm(void) { for (int axis = 0; axis < 3; axis++) { pidData[axis].I = 0.0f; @@ -443,12 +443,10 @@ void pidInitConfig(const pidProfile_t *pidProfile) horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; - ITermWindupPointInv = 0.0f; + itermWindupPointInv = 1.0f; if (pidProfile->itermWindupPointPercent < 100) { - float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; - ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); - } else { - ITermWindupPointInv = 0.0f; + const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f; + itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint); } itermAcceleratorGain = pidProfile->itermAcceleratorGain; crashTimeLimitUs = pidProfile->crash_time * 1000; @@ -634,8 +632,8 @@ static void handleCrashRecovery( *errorRate = *currentPidSetpoint - gyroRate; } } - // reset ITerm, since accumulated error before crash is now meaningless - // and ITerm windup during crash recovery can be extreme, especially on yaw axis + // reset iterm, since accumulated error before crash is now meaningless + // and iterm windup during crash recovery can be extreme, especially on yaw axis pidData[axis].I = 0.0f; if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs || (getMotorMixRange() < 1.0f @@ -697,7 +695,7 @@ static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT] } } -static void rotateITermAndAxisError() +static void rotateItermAndAxisError() { if (itermRotation #if defined(USE_ABSOLUTE_CONTROL) @@ -883,7 +881,7 @@ static void applyAbsoluteControl(const int axis, const float gyroRate, const boo #endif #if defined(USE_ITERM_RELAX) -static void applyItermRelax(const int axis, const float ITerm, +static void applyItermRelax(const int axis, const float iterm, const float gyroRate, float *itermErrorRate, float *currentPidSetpoint) { const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint); @@ -897,7 +895,7 @@ static void applyItermRelax(const int axis, const float ITerm, const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD; const bool isDecreasingI = - ((ITerm > 0) && (*itermErrorRate < 0)) || ((ITerm < 0) && (*itermErrorRate > 0)); + ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0)); if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) { // Do Nothing, use the precalculed itermErrorRate } else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) { @@ -931,7 +929,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT static float previousPidSetpoint[XYZ_AXIS_COUNT]; const float tpaFactor = getThrottlePIDAttenuation(); - const float motorMixRange = getMotorMixRange(); #ifdef USE_YAW_SPIN_RECOVERY const bool yawSpinActive = gyroYawSpinDetected(); @@ -946,8 +943,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // gradually scale back integration when above windup point float dynCi = dT * itermAccelerator; - if (ITermWindupPointInv > 0) { - dynCi *= constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f); + if (itermWindupPointInv > 1.0f) { + dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f); } // Precalculate gyro deta for D-term here, this allows loop unrolling @@ -958,7 +955,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]); } - rotateITermAndAxisError(); + rotateItermAndAxisError(); // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { @@ -993,25 +990,25 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, ¤tPidSetpoint, &errorRate); - const float ITerm = pidData[axis].I; + const float iterm = pidData[axis].I; float itermErrorRate = errorRate; #if defined(USE_ITERM_RELAX) - applyItermRelax(axis, ITerm, gyroRate, &itermErrorRate, ¤tPidSetpoint); + applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, ¤tPidSetpoint); #endif // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // 2-DOF PID controller with optional filter on derivative term. // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error). - // -----calculate P component and add Dynamic Part based on stick input + // -----calculate P component pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor; if (axis == FD_YAW) { pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P); } // -----calculate I component - pidData[axis].I = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); + pidData[axis].I = constrainf(iterm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); // -----calculate D component if (pidCoefficient[axis].Kd > 0) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9493ce41d2..365f177649 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -105,7 +105,7 @@ typedef struct pidProfile_s { pidf_t pid[PID_ITEM_COUNT]; uint8_t dterm_filter_type; // Filter selection for dterm - uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation + uint8_t itermWindupPointPercent; // iterm windup threshold, percent motor saturation uint16_t pidSumLimit; uint16_t pidSumLimitYaw; uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. @@ -181,7 +181,7 @@ extern uint32_t targetPidLooptime; extern float throttleBoost; extern pt1Filter_t throttleLpf; -void pidResetITerm(void); +void pidResetIterm(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index d72fae8337..a979c0d4fa 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -823,7 +823,7 @@ extern "C" { void failsafeStartMonitoring(void) {} void failsafeUpdateState(void) {} bool failsafeIsActive(void) { return false; } - void pidResetITerm(void) {} + void pidResetIterm(void) {} void updateAdjustmentStates(void) {} void processRcAdjustments(controlRateConfig_t *) {} void updateGpsWaypointsAndMode(void) {} diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 7ed8c66c78..0ebd069169 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -145,7 +145,7 @@ extern "C" { void failsafeStartMonitoring(void) {} void failsafeUpdateState(void) {} bool failsafeIsActive(void) { return false; } - void pidResetITerm(void) {} + void pidResetIterm(void) {} void updateAdjustmentStates(void) {} void processRcAdjustments(controlRateConfig_t *) {} void updateGpsWaypointsAndMode(void) {}