GCC Code Coverage Report

Directory: ./
File: firmware/util/math/efi_pid.cpp
Date: 2025-10-24 14:26:41
Coverage Exec Excl Total
Lines: 70.5% 110 0 156
Functions: 64.5% 20 0 31
Branches: 60.9% 28 0 46
Decisions: 63.3% 19 - 30
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