diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cb116542e..57adeacbf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -21,6 +21,7 @@ #include +#include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" @@ -74,33 +75,77 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -pt1Filter_t deltaFilter[3]; -pt1Filter_t yawFilter; -biquadFilter_t dtermFilterLpf[3]; -biquadFilter_t dtermFilterNotch[3]; -bool dtermNotchInitialised; -bool dtermBiquadLpfInitialised; -firFilterDenoise_t dtermDenoisingState[3]; +static filterApplyFnPtr dtermNotchFilterApplyFn; +static void *dtermFilterNotch[2]; +static filterApplyFnPtr dtermLpfApplyFn; +static void *dtermFilterLpf[2]; +static filterApplyFnPtr ptermYawFilterApplyFn; +static void *ptermYawFilter; static void pidInitFilters(const pidProfile_t *pidProfile) { - static uint8_t lowpassFilterType; + static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed + static biquadFilter_t biquadFilterNotch[2]; + static pt1Filter_t pt1Filter[2]; + static biquadFilter_t biquadFilter[2]; + static firFilterDenoise_t denoisingFilter[2]; + static pt1Filter_t pt1FilterYaw; - if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { - float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); - for (int axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); - dtermNotchInitialised = true; + if (initialized) { + return; } - if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) { - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { - for (int axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); - } + BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 + const float dT = (float)targetPidLooptime * 0.000001f; - if (pidProfile->dterm_filter_type == FILTER_FIR) { - for (int axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + if (pidProfile->dterm_notch_hz == 0) { + dtermNotchFilterApplyFn = nullFilterApply; + } else { + dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterNotch[axis] = &biquadFilterNotch[axis]; + biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); } - lowpassFilterType = pidProfile->dterm_filter_type; + } + + if (pidProfile->dterm_lpf_hz == 0) { + dtermLpfApplyFn = nullFilterApply; + } else { + switch (pidProfile->dterm_filter_type) { + default: + dtermLpfApplyFn = nullFilterApply; + break; + case FILTER_PT1: + dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterLpf[axis] = &pt1Filter[axis]; + pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT); + } + break; + case FILTER_BIQUAD: + dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterLpf[axis] = &biquadFilter[axis]; + biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + } + break; + case FILTER_FIR: + dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterLpf[axis] = &denoisingFilter[axis]; + firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + } + break; + } + } + + if (pidProfile->yaw_lpf_hz == 0) { + ptermYawFilterApplyFn = nullFilterApply; + } else { + ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; + ptermYawFilter = &pt1FilterYaw; + pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT); } } @@ -156,7 +201,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // ----------PID controller---------- const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - for (int axis = 0; axis < 3; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { static uint8_t configP[3], configI[3], configD[3]; @@ -177,9 +222,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity; if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + const float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; if (ABS(currentVelocity) > maxVelocity) { setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } @@ -210,30 +255,28 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // --------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 can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- + + // -----calculate error rate const float errorRate = setpointRate[axis] - PVRate; // r - y // -----calculate P component and add Dynamic Part based on stick input float PTerm = Kp[axis] * errorRate * tpaFactor; - // -----calculate I component. + // -----calculate I component // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); + const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); + const float itermScaler = setpointRateScaler * kiThrottleGain; - // Handle All windup Scenarios + float ITerm = errorGyroIf[axis]; + ITerm += Ki[axis] * errorRate * dT * itermScaler;; // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; + ITerm = constrainf(ITerm, -250.0f, 250.0f); + errorGyroIf[axis] = ITerm; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * dT * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - const float ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - float DTerm; - if (axis != YAW) { + // -----calculate D component (Yaw D not yet supported) + float DTerm = 0.0; + if (axis != FD_YAW) { static float previousSetpoint[3]; float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { @@ -248,38 +291,23 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } previousSetpoint[axis] = setpointRate[axis]; const float rD = dynC * setpointRate[axis] - PVRate; // cr - y - float delta = rD - lastRateError[axis]; + // Divide rate change by dT to get differential (ie dr/dt) + const float delta = (rD - lastRateError[axis]) / dT; lastRateError[axis] = rD; - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / dT); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - else if (pidProfile->dterm_filter_type == FILTER_PT1) - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT); - else - delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta); - } - DTerm = Kd[axis] * delta * tpaFactor; + DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm); + + // apply filters + DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm); + DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); - // -----calculate total PID output - axisPIDf[axis] = PTerm + ITerm + DTerm; } else { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT); - - axisPIDf[axis] = PTerm + ITerm; - - DTerm = 0.0f; // needed for blackbox + PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm); } + // -----calculate total PID output + axisPIDf[axis] = PTerm + ITerm + DTerm; // Disable PID control at zero throttle if (!pidStabilisationEnabled) axisPIDf[axis] = 0;