From 37da70c55568277c5526f35049361e39440dac8b Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 15 Sep 2018 12:19:53 +1000 Subject: [PATCH] rebase and recheck --- src/main/config/config_unittest.h | 24 ++++++++++++------------ src/main/fc/core.c | 8 ++++---- src/main/flight/mixer.c | 4 ++-- src/main/flight/pid.c | 39 +++++++++++++++++++-------------------- src/main/flight/pid.h | 4 ++-- 5 files changed, 39 insertions(+), 40 deletions(-) diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h index 65c5cb13e..27a92ec68 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 efbb73339..701223fcb 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 2bb28aa05..7c7aa3f8d 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 c7dd01da6..ab4487628 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -130,7 +130,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_notch_hz = 0, .dterm_notch_cutoff = 0, .dterm_filter_type = FILTER_PT1, - .iTermWindupPointPercent = 100, + .itermWindupPointPercent = 100, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, @@ -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 float iTermWindupPointInv = 1.0f; +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,11 +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; - if (pidProfile->iTermWindupPointPercent < 100) { - const float iTermWindupPoint = pidProfile->iTermWindupPointPercent / 100.0f; - iTermWindupPointInv = 1.0f / (1.0f - iTermWindupPoint); - } else { - iTermWindupPointInv = 1.0f; + itermWindupPointInv = 1.0f; + if (pidProfile->itermWindupPointPercent < 100) { + const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f; + itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint); } itermAcceleratorGain = pidProfile->itermAcceleratorGain; crashTimeLimitUs = pidProfile->crash_time * 1000; @@ -633,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 @@ -696,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) @@ -882,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); @@ -896,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) { @@ -944,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 > 1.0f) { - dynCi *= constrainf((1.0f - getMotorMixRange()) * 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 @@ -956,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) { @@ -991,12 +990,12 @@ 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); -#endif + 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. @@ -1009,7 +1008,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } // -----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 8ac9d42c4..365f17764 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; // 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); -- 2.11.4.GIT