| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /** | |||
| 2 | * @file pid.cpp | |||
| 3 | * | |||
| 4 | * https://en.wikipedia.org/wiki/Feedback | |||
| 5 | * http://en.wikipedia.org/wiki/PID_controller | |||
| 6 | * | |||
| 7 | * @date Sep 16, 2014 | |||
| 8 | * @author Andrey Belomutskiy, (c) 2012-2020 | |||
| 9 | */ | |||
| 10 | ||||
| 11 | #include "pch.h" | |||
| 12 | ||||
| 13 | #include "efi_pid.h" | |||
| 14 | #include "math.h" | |||
| 15 | ||||
| 16 | 2812 | Pid::Pid() { | ||
| 17 | 2812 | initPidClass(nullptr); | ||
| 18 | 2812 | } | ||
| 19 | ||||
| 20 | 6 | Pid::Pid(pid_s *p_parameters) { | ||
| 21 | 6 | initPidClass(p_parameters); | ||
| 22 | 6 | } | ||
| 23 | ||||
| 24 | 5327 | void Pid::initPidClass(pid_s *p_parameters) { | ||
| 25 | 5327 | parameters = p_parameters; | ||
| 26 | 5327 | resetCounter = 0; | ||
| 27 | ||||
| 28 | 5327 | Pid::reset(); | ||
| 29 | 5327 | } | ||
| 30 | ||||
| 31 | 2226 | static bool isClose(float a, float b) { | ||
| 32 | 2226 | bool result = (a == b); | ||
| 33 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 2210 times.
|
2/2✓ Decision 'true' taken 16 times.
✓ Decision 'false' taken 2210 times.
|
2226 | if (!result) { |
| 34 | 16 | efiPrintf("PID: not same %f %f", a, b); | ||
| 35 | } | |||
| 36 | 2226 | return result; | ||
| 37 | } | |||
| 38 | ||||
| 39 | 458 | bool Pid::isSame(const pid_s *p_parameters) const { | ||
| 40 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 458 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 458 times.
|
458 | if (!parameters) { |
| 41 | // this 'null' could happen on first execution during initialization | |||
| 42 | ✗ | return false; | ||
| 43 | } | |||
| 44 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 458 times.
|
458 | efiAssert(ObdCode::OBD_PCM_Processor_Fault, p_parameters != nullptr, "PID::isSame nullptr", false); | |
| 45 | 458 | return isClose(parameters->pFactor, p_parameters->pFactor) | ||
| 46 |
1/2✓ Branch 1 taken 442 times.
✗ Branch 2 not taken.
|
442 | && isClose(parameters->iFactor, p_parameters->iFactor) | |
| 47 |
1/2✓ Branch 1 taken 442 times.
✗ Branch 2 not taken.
|
442 | && isClose(parameters->dFactor, p_parameters->dFactor) | |
| 48 |
1/2✓ Branch 1 taken 442 times.
✗ Branch 2 not taken.
|
442 | && isClose(parameters->offset, p_parameters->offset) | |
| 49 |
3/4✓ Branch 0 taken 442 times.
✓ Branch 1 taken 16 times.
✓ Branch 3 taken 442 times.
✗ Branch 4 not taken.
|
900 | && isClose(parameters->periodMs, p_parameters->periodMs); | |
| 50 | } | |||
| 51 | ||||
| 52 | /** | |||
| 53 | * @param Controller input / process output | |||
| 54 | * @returns Output from the PID controller / the input to the process | |||
| 55 | */ | |||
| 56 | 44 | float Pid::getOutput(float p_target, float p_input) { | ||
| 57 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 44 times.
|
44 | efiAssert(ObdCode::OBD_PCM_Processor_Fault, parameters != nullptr, "PID::getOutput nullptr", 0); | |
| 58 | 44 | float dTime = MS2SEC(GET_PERIOD_LIMITED(parameters)); | ||
| 59 | 44 | return getOutput(p_target, p_input, dTime); | ||
| 60 | } | |||
| 61 | ||||
| 62 | 200 | float Pid::getUnclampedOutput(float p_target, float p_input, float dTime) { | ||
| 63 | 200 | target = p_target; | ||
| 64 | 200 | input = p_input; | ||
| 65 | 200 | float error = (target - input) * errorAmplificationCoef; | ||
| 66 | ||||
| 67 | 200 | float pTerm = parameters->pFactor * error; | ||
| 68 | 200 | updateITerm(parameters->iFactor * dTime * error); | ||
| 69 | 200 | dTerm = parameters->dFactor / dTime * (error - previousError); | ||
| 70 | ||||
| 71 | 200 | previousError = error; | ||
| 72 | ||||
| 73 |
2/2✓ Branch 0 taken 154 times.
✓ Branch 1 taken 46 times.
|
2/2✓ Decision 'true' taken 154 times.
✓ Decision 'false' taken 46 times.
|
200 | if (dTime <=0) { |
| 74 | 154 | warning(ObdCode::CUSTOM_PID_DTERM, "PID: unexpected dTime"); | ||
| 75 | 154 | return pTerm + getOffset(); | ||
| 76 | } | |||
| 77 | ||||
| 78 | 46 | return pTerm + iTerm + dTerm + getOffset(); | ||
| 79 | } | |||
| 80 | ||||
| 81 | /** | |||
| 82 | * @param dTime seconds probably? :) | |||
| 83 | */ | |||
| 84 | 200 | float Pid::getOutput(float p_target, float p_input, float dTime) { | ||
| 85 | 200 | float l_output = getUnclampedOutput(p_target, p_input, dTime); | ||
| 86 | ||||
| 87 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 194 times.
|
2/2✓ Decision 'true' taken 6 times.
✓ Decision 'false' taken 194 times.
|
200 | if (l_output > parameters->maxValue) { |
| 88 | 6 | l_output = parameters->maxValue; | ||
| 89 |
2/2✓ Branch 1 taken 11 times.
✓ Branch 2 taken 183 times.
|
2/2✓ Decision 'true' taken 11 times.
✓ Decision 'false' taken 183 times.
|
194 | } else if (l_output < getMinValue()) { |
| 90 | 11 | l_output = getMinValue(); | ||
| 91 | } | |||
| 92 | 200 | output = l_output; | ||
| 93 | 200 | return output; | ||
| 94 | } | |||
| 95 | ||||
| 96 | 1 | void Pid::updateFactors(float pFactor, float iFactor, float dFactor) { | ||
| 97 | 1 | parameters->pFactor = pFactor; | ||
| 98 | 1 | parameters->iFactor = iFactor; | ||
| 99 | 1 | parameters->dFactor = dFactor; | ||
| 100 | 1 | reset(); | ||
| 101 | 1 | } | ||
| 102 | ||||
| 103 | 5488 | void Pid::reset() { | ||
| 104 | 5488 | dTerm = iTerm = 0; | ||
| 105 | 5488 | output = input = target = previousError = 0; | ||
| 106 | 5488 | errorAmplificationCoef = 1.0f; | ||
| 107 | 5488 | resetCounter++; | ||
| 108 | 5488 | } | ||
| 109 | ||||
| 110 | ✗ | float Pid::getP() const { | ||
| 111 | ✗ | return parameters->pFactor; | ||
| 112 | } | |||
| 113 | ||||
| 114 | ✗ | float Pid::getI() const { | ||
| 115 | ✗ | return parameters->iFactor; | ||
| 116 | } | |||
| 117 | ||||
| 118 | ✗ | float Pid::getPrevError() const { | ||
| 119 | ✗ | return previousError; | ||
| 120 | } | |||
| 121 | ||||
| 122 | 1113 | float Pid::getIntegration() const { | ||
| 123 | 1113 | return iTerm; | ||
| 124 | } | |||
| 125 | ||||
| 126 | ✗ | float Pid::getD() const { | ||
| 127 | ✗ | return parameters->dFactor; | ||
| 128 | } | |||
| 129 | ||||
| 130 | 229 | float Pid::getOffset(void) const { | ||
| 131 | 229 | return parameters->offset; | ||
| 132 | } | |||
| 133 | ||||
| 134 | 234 | float Pid::getMinValue(void) const { | ||
| 135 | 234 | return parameters->minValue; | ||
| 136 | } | |||
| 137 | ||||
| 138 | 9 | void Pid::setErrorAmplification(float coef) { | ||
| 139 | 9 | errorAmplificationCoef = coef; | ||
| 140 | 9 | } | ||
| 141 | ||||
| 142 | #if EFI_TUNER_STUDIO | |||
| 143 | ||||
| 144 | 224 | void Pid::postState(pid_status_s& pidStatus) const { | ||
| 145 | 224 | pidStatus.output = output; | ||
| 146 | 224 | pidStatus.error = previousError; | ||
| 147 |
1/2✓ Branch 0 taken 224 times.
✗ Branch 1 not taken.
|
224 | pidStatus.pTerm = parameters == nullptr ? 0 : parameters->pFactor * previousError; | |
| 148 | 224 | pidStatus.iTerm = iTerm; | ||
| 149 | 224 | pidStatus.dTerm = dTerm; | ||
| 150 | 224 | pidStatus.resetCounter = resetCounter; | ||
| 151 | 224 | } | ||
| 152 | #endif /* EFI_TUNER_STUDIO */ | |||
| 153 | ||||
| 154 | ✗ | void Pid::sleep() { | ||
| 155 | #if !EFI_UNIT_TEST | |||
| 156 | int periodMs = maxI(10, parameters->periodMs); | |||
| 157 | chThdSleepMilliseconds(periodMs); | |||
| 158 | #endif /* EFI_UNIT_TEST */ | |||
| 159 | ✗ | } | ||
| 160 | ||||
| 161 | ✗ | void Pid::showPidStatus(const char*msg) const { | ||
| 162 | ✗ | efiPrintf("%s settings: offset=%f P=%.5f I=%.5f D=%.5f period=%dms", | ||
| 163 | msg, | |||
| 164 | getOffset(), | |||
| 165 | parameters->pFactor, | |||
| 166 | parameters->iFactor, | |||
| 167 | parameters->dFactor, | |||
| 168 | parameters->periodMs); | |||
| 169 | ||||
| 170 | ✗ | efiPrintf("%s status: value=%.2f input=%.2f/target=%.2f iTerm=%.5f dTerm=%.5f", | ||
| 171 | msg, | |||
| 172 | output, | |||
| 173 | input, | |||
| 174 | target, | |||
| 175 | iTerm, dTerm); | |||
| 176 | ||||
| 177 | ✗ | } | ||
| 178 | ||||
| 179 | 229 | void Pid::updateITerm(float value) { | ||
| 180 | 229 | iTerm += value; | ||
| 181 | /** | |||
| 182 | * If we have exceeded the ability of the controlled device to hit target, the I factor will keep accumulating and approach infinity. | |||
| 183 | * Here we limit the I-term #353 | |||
| 184 | */ | |||
| 185 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 229 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 229 times.
|
229 | if (iTerm > parameters->maxValue * 100) { |
| 186 | ✗ | iTerm = parameters->maxValue * 100; | ||
| 187 | } | |||
| 188 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 227 times.
|
2/2✓ Decision 'true' taken 2 times.
✓ Decision 'false' taken 227 times.
|
229 | if (iTerm > iTermMax) { |
| 189 | 2 | iTerm = iTermMax; | ||
| 190 | } | |||
| 191 | ||||
| 192 | // this is kind of a hack. a proper fix would be having separate additional settings 'maxIValue' and 'minIValye' | |||
| 193 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 229 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 229 times.
|
229 | if (iTerm < -parameters->maxValue * 100) |
| 194 | ✗ | iTerm = -parameters->maxValue * 100; | ||
| 195 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 229 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 229 times.
|
229 | if (iTerm < iTermMin) { |
| 196 | ✗ | iTerm = iTermMin; | ||
| 197 | } | |||
| 198 | 229 | } | ||
| 199 | ||||
| 200 | ||||
| 201 | ✗ | PidCic::PidCic() { | ||
| 202 | // call our derived reset() | |||
| 203 | ✗ | PidCic::reset(); | ||
| 204 | ✗ | } | ||
| 205 | ||||
| 206 | ✗ | PidCic::PidCic(pid_s *p_parameters) : Pid(p_parameters) { | ||
| 207 | // call our derived reset() | |||
| 208 | ✗ | PidCic::reset(); | ||
| 209 | ✗ | } | ||
| 210 | ||||
| 211 | ✗ | void PidCic::reset(void) { | ||
| 212 | ✗ | Pid::reset(); | ||
| 213 | ||||
| 214 | ✗ | totalItermCnt = 0; | ||
| 215 | ✗ | for (int i = 0; i < PID_AVG_BUF_SIZE; i++) | ||
| 216 | ✗ | iTermBuf[i] = 0; | ||
| 217 | ✗ | iTermInvNum = 1.0f / (float)PID_AVG_BUF_SIZE; | ||
| 218 | ✗ | } | ||
| 219 | ||||
| 220 | ✗ | float PidCic::getOutput(float p_target, float p_input, float dTime) { | ||
| 221 | ✗ | return getUnclampedOutput(p_target, p_input, dTime); | ||
| 222 | } | |||
| 223 | ||||
| 224 | ✗ | void PidCic::updateITerm(float value) { | ||
| 225 | // use a variation of cascaded integrator-comb (CIC) filtering to get non-overflow iTerm | |||
| 226 | ✗ | totalItermCnt++; | ||
| 227 | ✗ | int localBufPos = (totalItermCnt >> PID_AVG_BUF_SIZE_SHIFT) % PID_AVG_BUF_SIZE; | ||
| 228 | ✗ | int localPrevBufPos = ((totalItermCnt - 1) >> PID_AVG_BUF_SIZE_SHIFT) % PID_AVG_BUF_SIZE; | ||
| 229 | ||||
| 230 | // reset old buffer cell | |||
| 231 | ✗ | if (localPrevBufPos != localBufPos) | ||
| 232 | ✗ | iTermBuf[localBufPos] = 0; | ||
| 233 | // integrator stage | |||
| 234 | ✗ | iTermBuf[localBufPos] += value; | ||
| 235 | ||||
| 236 | // return moving average of all sums, to smoothen the result | |||
| 237 | ✗ | float iTermSum = 0; | ||
| 238 | ✗ | for (int i = 0; i < PID_AVG_BUF_SIZE; i++) { | ||
| 239 | ✗ | iTermSum += iTermBuf[i]; | ||
| 240 | } | |||
| 241 | ✗ | iTerm = iTermSum * iTermInvNum; | ||
| 242 | ✗ | } | ||
| 243 | ||||
| 244 | 704 | PidIndustrial::PidIndustrial() : Pid() { | ||
| 245 | 704 | } | ||
| 246 | ||||
| 247 | 1 | PidIndustrial::PidIndustrial(pid_s *p_parameters) : Pid(p_parameters) { | ||
| 248 | 1 | } | ||
| 249 | ||||
| 250 | 29 | float PidIndustrial::getOutput(float p_target, float p_input, float dTime) { | ||
| 251 | float ad, bd; | |||
| 252 | 29 | float error = (p_target - p_input) * errorAmplificationCoef; | ||
| 253 | 29 | float pTerm = parameters->pFactor * error; | ||
| 254 | ||||
| 255 | // calculate dTerm coefficients | |||
| 256 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 23 times.
|
2/2✓ Decision 'true' taken 6 times.
✓ Decision 'false' taken 23 times.
|
29 | if (fabsf(derivativeFilterLoss) > DBL_EPSILON) { |
| 257 | // restore Td in the Standard form from the Parallel form: Td = Kd / Kc | |||
| 258 | 6 | float Td = parameters->dFactor / parameters->pFactor; | ||
| 259 | // calculate the backward differences approximation of the derivative term | |||
| 260 | 6 | ad = Td / (Td + dTime / derivativeFilterLoss); | ||
| 261 | 6 | bd = parameters->pFactor * ad / derivativeFilterLoss; | ||
| 262 | } else { | |||
| 263 | // According to the Theory of limits, if p.derivativeFilterLoss -> 0, then | |||
| 264 | // lim(ad) = 0; lim(bd) = p.pFactor * Td / dTime = p.dFactor / dTime | |||
| 265 | // i.e. dTerm becomes equal to Pid's | |||
| 266 | 23 | ad = 0.0f; | ||
| 267 | 23 | bd = parameters->dFactor / dTime; | ||
| 268 | } | |||
| 269 | ||||
| 270 | // (error - previousError) = (target-input) - (target-prevousInput) = -(input - prevousInput) | |||
| 271 | 29 | dTerm = dTerm * ad + (error - previousError) * bd; | ||
| 272 | ||||
| 273 | 29 | updateITerm(parameters->iFactor * dTime * error); | ||
| 274 | ||||
| 275 | // calculate output and apply the limits | |||
| 276 | 29 | float l_output = pTerm + iTerm + dTerm + getOffset(); | ||
| 277 | 29 | float limitedOutput = limitOutput(l_output); | ||
| 278 | ||||
| 279 | // apply the integrator anti-windup on top of the "normal" iTerm change above | |||
| 280 | // If p.antiwindupFreq = 0, then iTerm is equal to PidParallelController's | |||
| 281 | 29 | iTerm += dTime * antiwindupFreq * (limitedOutput - l_output); | ||
| 282 | ||||
| 283 | // update the state | |||
| 284 | 29 | previousError = error; | ||
| 285 | ||||
| 286 | 29 | return limitedOutput; | ||
| 287 | } | |||
| 288 | ||||
| 289 | 29 | float PidIndustrial::limitOutput(float v) const { | ||
| 290 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 29 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 29 times.
|
29 | if (v < getMinValue()) |
| 291 | ✗ | v = getMinValue(); | ||
| 292 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 26 times.
|
2/2✓ Decision 'true' taken 3 times.
✓ Decision 'false' taken 26 times.
|
29 | if (v > parameters->maxValue) |
| 293 | 3 | v = parameters->maxValue; | ||
| 294 | 29 | return v; | ||
| 295 | } | |||
| 296 |