| 157 | } |
| 158 | |
| 159 | float AutoGain::updatePIController(float targetGain, float dt) { |
| 160 | const float error = targetGain - mLastGain; |
| 161 | |
| 162 | // Bug 4 fix: Use absolute error threshold when gain is small to avoid |
| 163 | // huge errorRatio from dividing by tiny mLastGain |
| 164 | const bool largeError = (mLastGain > 0.1f) |
| 165 | ? (fl::abs(error) / mLastGain > 0.2f) // Relative: >20% of current gain |
| 166 | : (fl::abs(error) > 0.1f); // Absolute: >0.1 when gain is tiny |
| 167 | const float tau = largeError ? mGainFollowFastTau : mGainFollowSlowTau; |
| 168 | |
| 169 | // Proportional term |
| 170 | const float pTerm = mKp * error; |
| 171 | |
| 172 | // Bug 1 fix: PI target is where we WANT gain to be (not mLastGain + delta + delta) |
| 173 | // targetGain + pTerm + integrator = the desired gain level |
| 174 | const float piTarget = targetGain + pTerm + mIntegrator; |
| 175 | |
| 176 | // Smooth with exponential filter using adaptive tau |
| 177 | float alpha = 1.0f; |
| 178 | if (tau > 0.0f && dt > 0.0f) { |
| 179 | alpha = 1.0f - fl::exp(-dt / tau); |
| 180 | } |
| 181 | |
| 182 | const float unclamped = mLastGain + alpha * (piTarget - mLastGain); |
| 183 | |
| 184 | // Bug 2 fix: Back-calculation anti-windup — only accumulate integrator |
| 185 | // when output is NOT clamped. Decay integrator when saturated (WLED: *= 0.91) |
| 186 | const bool saturated = (unclamped > mConfig.maxGain) || (unclamped < mConfig.minGain); |
| 187 | if (saturated) { |
| 188 | mIntegrator *= 0.91f; |
| 189 | } else { |
| 190 | mIntegrator += mKi * error * dt; |
| 191 | } |
| 192 | // Clamp integrator magnitude to prevent runaway in pathological cases |
| 193 | const float maxIntegral = 4.0f * mConfig.maxGain; |
| 194 | mIntegrator = fl::max(-maxIntegral, fl::min(maxIntegral, mIntegrator)); |
| 195 | |
| 196 | return unclamped; |
| 197 | } |
| 198 | |
| 199 | void AutoGain::applyGain(const vector<i16>& input, float gain, vector<i16>& output) { |
| 200 | output.clear(); |