GCC Code Coverage Report

Directory: src/
File: src/Controller.cpp
Date: 2026-09-16 13:46:10
Exec Total Coverage
Lines: 121 160 75.6%
Branches: 83 208 39.9%
Line Branch Exec Source
1 /*****************************************************************************
2 *
3 * Copyright (C) 2021 - 2022 Jonathan Grahl <jonathan.grahl@igh.de>
4 * 2021 - 2022 Florian Pose <florian.pose@igh.de>
5 *
6 * This file is part of the reta library (realtime-automation).
7 *
8 * The reta library is free software: you can redistribute it and/or modify
9 * it under the terms of the GNU Lesser General Public License as published
10 * by the Free Software Foundation, either version 3 of the License, or (at
11 * your option) any later version.
12 *
13 * The reta library is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
16 * General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public License
19 * along with the reta library. If not, see <http://www.gnu.org/licenses/>.
20 *
21 ****************************************************************************/
22
23 #include "reta/Controller.h"
24
25 #include "Base.h"
26 #include "reta/LowPassFilter.h"
27
28 #include <cmath> // fabs(), copysign()
29
30 using std::copysign;
31 using std::fabs;
32 using std::function;
33 using std::make_unique;
34 using std::shared_ptr;
35 using std::string;
36 using std::vector;
37
38 using namespace Reta;
39
40 /****************************************************************************/
41
42 28 struct RETA_LOCAL Controller::Impl : public Base
43 {
44 Impl(shared_ptr<Task>,
45 const string &,
46 double,
47 double,
48 double,
49 unsigned int,
50 Controller::LimitMode,
51 Controller::InvertMode);
52
53 void
54 update(const vector<double> &,
55 const vector<double> &,
56 const vector<uint8_t> &,
57 const vector<uint8_t> & = vector<uint8_t>());
58
59 double limitIOut(const double &, unsigned int);
60 shared_ptr<Task> task;
61
62 vector<double> error;
63 vector<double> prevFilteredError;
64
65 vector<double> kp;
66 vector<double> pOut;
67 vector<double> ki;
68 vector<double> iOut;
69 vector<double> kd;
70 LowPassFilter dFilter; // smoothens the error before differentiating
71 vector<double> dOut;
72
73 vector<double> kPilot;
74 vector<double> minPilotValue;
75 vector<double> pilotControl;
76
77 double invert;
78 Controller::LimitMode limitMode;
79 double lowerLimit;
80 double upperLimit;
81 vector<double> output;
82 };
83
84 /****************************************************************************/
85
86 29 Controller::Impl::Impl(
87 shared_ptr<Task> task,
88 const string &prefix,
89 double kpInit,
90 double kiInit,
91 double kdInit,
92 unsigned int width,
93 Controller::LimitMode limitModeInit,
94 29 Controller::InvertMode invertModeInit) :
95 Base {prefix},
96 task {task},
97 error(width, 0.0),
98 prevFilteredError(width, 0.0),
99 kp(width, kpInit),
100 pOut(width, 0.0),
101 ki(width, kiInit),
102 iOut(width, 0.0),
103 kd(width, kdInit),
104
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 58 dFilter(task, prefix + "/DFilter", 0.0, width),
105 dOut(width, 0.0),
106 kPilot(width, 0.0),
107 minPilotValue(width, 0.0),
108 pilotControl(width, 0.0),
109
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28 times.
 28 invert {invertModeInit == Controller::Invert ? -1.0 : 1.0},
110 limitMode {limitModeInit},
111 lowerLimit {-1.0},
112 upperLimit {1.0},
113
14/26
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 29 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 29 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 29 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 29 times.
✗ Branch 25 not taken.
✓ Branch 29 taken 29 times.
✗ Branch 30 not taken.
✓ Branch 34 taken 29 times.
✗ Branch 35 not taken.
✓ Branch 39 taken 28 times.
✓ Branch 40 taken 1 times.
✓ Branch 44 taken 28 times.
✗ Branch 45 not taken.
✓ Branch 49 taken 28 times.
✗ Branch 50 not taken.
✓ Branch 54 taken 28 times.
✗ Branch 55 not taken.
✓ Branch 59 taken 28 times.
✗ Branch 60 not taken.
✓ Branch 64 taken 28 times.
✗ Branch 65 not taken.
 114 output(width, 0.0)
114 {
115
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 checkZeroWidth(width);
116
117
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 pdserv *pdserv {task->getPdServ()};
118
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 pdtask *pdtask {task->getPdTask()};
119
120
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
121
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/Error").c_str(), pd_double_T, error.data(),
122 error.size(), NULL);
123
124
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_parameter(
125
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdserv, (prefix + "/Kp").c_str(), 0666, pd_double_T, kp.data(),
126 kp.size(), NULL, NULL, NULL);
127
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
128
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/KpSignal").c_str(), pd_double_T, kp.data(),
129 kp.size(), NULL);
130
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
131
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/POut").c_str(), pd_double_T, pOut.data(),
132 pOut.size(), NULL);
133
134
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_parameter(
135
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdserv, (prefix + "/Ki").c_str(), 0666, pd_double_T, ki.data(),
136 ki.size(), NULL, NULL, NULL);
137
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
138
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/KiSignal").c_str(), pd_double_T, ki.data(),
139 ki.size(), NULL);
140
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
141
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/IOut").c_str(), pd_double_T, iOut.data(),
142 iOut.size(), NULL);
143
144
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_parameter(
145
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdserv, (prefix + "/Kd").c_str(), 0666, pd_double_T, kd.data(),
146 kd.size(), NULL, NULL, NULL);
147
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
148
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/KdSignal").c_str(), pd_double_T, kd.data(),
149 kd.size(), NULL);
150
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
151
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 pdtask, 1, (prefix + "/DOut").c_str(), pd_double_T, dOut.data(),
152 dOut.size(), NULL);
153
154
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_parameter(
155
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 pdserv, (prefix + "/KPilot").c_str(), 0666, pd_double_T,
156 28 kPilot.data(), kPilot.size(), NULL, NULL, NULL);
157
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_parameter(
158
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 pdserv, (prefix + "/MinPilotValue").c_str(), 0666, pd_double_T,
159 28 minPilotValue.data(), minPilotValue.size(), NULL, NULL, NULL);
160
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
161
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 pdtask, 1, (prefix + "/PilotControl").c_str(), pd_double_T,
162 28 pilotControl.data(), pilotControl.size(), NULL);
163
164
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 if (limitMode == Controller::StaticLimits) {
165
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 pdserv_parameter(
166
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 pdserv, (prefix + "/LowerLimit").c_str(), 0666, pd_double_T,
167 28 &lowerLimit, 1, NULL, NULL, NULL);
168
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 pdserv_parameter(
169
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 pdserv, (prefix + "/UpperLimit").c_str(), 0666, pd_double_T,
170 28 &upperLimit, 1, NULL, NULL, NULL);
171 }
172 else { // DynamicLimits
173 ✗ pdserv_signal(
174 ✗ pdtask, 1, (prefix + "/LowerLimit").c_str(), pd_double_T,
175 ✗ &lowerLimit, 1, NULL);
176 ✗ pdserv_signal(
177 ✗ pdtask, 1, (prefix + "/UpperLimit").c_str(), pd_double_T,
178 ✗ &upperLimit, 1, NULL);
179 }
180
181
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 pdserv_signal(
182
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 pdtask, 1, (prefix + "/Output").c_str(), pd_double_T,
183 28 output.data(), output.size(), NULL);
184 28 }
185
186 /****************************************************************************/
187
188 28 Controller::~Controller()
189 28 {}
190
191 /****************************************************************************/
192
193 13 void Controller::Impl::update(
194 const vector<double> &currentValue,
195 const vector<double> &targetValue,
196 const vector<uint8_t> &enableIntegrator,
197 const vector<uint8_t> &integrate)
198 {
199 13 checkSize(currentValue, output);
200 12 checkSize(targetValue, output);
201 12 checkSize(enableIntegrator, output);
202 12 checkSize(integrate, output);
203
204
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 12 times.
 27 for (unsigned int i = 0; i < error.size(); i++) {
205 15 error.at(i) = (targetValue.at(i) - currentValue.at(i)) * invert;
206 }
207
208
2/4
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 12 dFilter.update(error, vector<uint8_t>(error.size(), false));
209
210
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 12 times.
 27 for (unsigned int i = 0; i < error.size(); i++) {
211 30 pilotControl.at(i) = fabs(pilotControl.at(i)) > minPilotValue.at(i)
212
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 30 ? targetValue.at(i) * kPilot.at(i)
213 15 : copysign(1.0, pilotControl.at(i)) * minPilotValue.at(i);
214
215 15 pOut.at(i) = error.at(i) * kp.at(i);
216
217 15 double filteredError {dFilter.getOutput(i)};
218 30 dOut.at(i) = (filteredError - prevFilteredError.at(i)) * kd.at(i)
219 15 / task->getPeriod();
220 15 prevFilteredError.at(i) = filteredError;
221
222
6/6
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 11 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 13 times.
 15 if (enableIntegrator.at(i) && ki.at(i) != 0.0) {
223
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 if (integrate.empty() || integrate.at(i)) {
224 2 iOut.at(i) += error.at(i) * ki.at(i) * task->getPeriod();
225 }
226
227 4 output.at(i) = pOut.at(i) + limitIOut(iOut.at(i), i)
228 2 + pilotControl.at(i) + dOut.at(i);
229 }
230 else { // without integration
231
232 13 iOut.at(i) = 0.0;
233
234 13 double auxOutput {pOut.at(i) + pilotControl.at(i) + dOut.at(i)};
235
236
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 11 times.
 13 if (auxOutput > upperLimit) {
237 2 auxOutput = upperLimit;
238 }
239
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 9 times.
 11 else if (auxOutput < lowerLimit) {
240 2 auxOutput = lowerLimit;
241 }
242
243 13 output.at(i) = auxOutput;
244 }
245 }
246 12 }
247
248 /****************************************************************************/
249
250 10 double Controller::Impl::limitIOut(const double &outputValue, unsigned int i)
251 {
252 double upperOutputLimit {
253 10 upperLimit - pilotControl.at(i) - pOut.at(i) - dOut.at(i)};
254 double lowerOutputLimit {
255 8 lowerLimit - pilotControl.at(i) - pOut.at(i) - dOut.at(i)};
256
257
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 if (iOut.at(i) > upperOutputLimit) {
258 ✗ return upperOutputLimit;
259 }
260
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 else if (iOut.at(i) < lowerOutputLimit) {
261 ✗ return lowerOutputLimit;
262 }
263
264 8 return outputValue;
265 }
266
267 /****************************************************************************/
268
269 22 Controller::Controller(
270 shared_ptr<Task> task,
271 const string &prefix,
272 double kpInit,
273 double kiInit,
274 unsigned int width,
275 LimitMode limitMode,
276 22 InvertMode invertMode) :
277 impl {make_unique<Impl>(
278 task,
279 prefix,
280 kpInit,
281 kiInit,
282 0.0,
283 width,
284 limitMode,
285
2/2
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 1 times.
 23 invertMode)}
286 21 {}
287
288 /****************************************************************************/
289
290 7 Controller::Controller(
291 shared_ptr<Task> task,
292 const string &prefix,
293 double kpInit,
294 double kiInit,
295 double kdInit,
296 unsigned int width,
297 LimitMode limitMode,
298 7 InvertMode invertMode) :
299 impl {make_unique<Impl>(
300 task,
301 prefix,
302 kpInit,
303 kiInit,
304 kdInit,
305 width,
306 limitMode,
307 7 invertMode)}
308 7 {}
309
310 /****************************************************************************/
311
312 8 void Controller::setIntegratorValue(const double &value, unsigned int i)
313 {
314 8 impl->iOut.at(i) = impl->limitIOut(value, i);
315 6 }
316
317 /****************************************************************************/
318
319 2 void Controller::setIntegratorValueVector(const vector<double> &value)
320 {
321 2 impl->checkSize(value, impl->output);
322
323
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
 5 for (unsigned int i = 0; i < value.size(); i++) {
324 4 setIntegratorValue(value.at(i), i);
325 }
326 1 }
327
328 /****************************************************************************/
329
330 28 void Controller::setLimits(double lower, double upper)
331 {
332 28 impl->lowerLimit = lower;
333 28 impl->upperLimit = upper;
334 28 }
335
336 /****************************************************************************/
337
338 ✗ void Controller::setOutput(double value, unsigned int i)
339 {
340 ✗ if (value > impl->upperLimit) {
341 ✗ value = impl->upperLimit;
342 }
343 ✗ if (value < impl->lowerLimit) {
344 ✗ value = impl->lowerLimit;
345 }
346
347 ✗ impl->iOut.at(i) = value - impl->pOut.at(i) - impl->pilotControl.at(i)
348 ✗ - impl->dOut.at(i);
349 }
350
351 /****************************************************************************/
352
353 11 void Controller::update(
354 double currentValue,
355 double targetValue,
356 bool enableIntegrator,
357 bool integrate)
358 {
359
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 22 impl->update(
360
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 22 vector<double> {currentValue}, vector<double> {targetValue},
361
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 22 vector<uint8_t> {enableIntegrator}, vector<uint8_t> {integrate});
362 11 }
363
364 /****************************************************************************/
365
366 2 void Controller::update(
367 const vector<double> &currentValue,
368 const vector<double> &targetValue,
369 const vector<uint8_t> &enableIntegrator,
370 const vector<uint8_t> &integrate)
371 {
372 2 impl->update(currentValue, targetValue, enableIntegrator, integrate);
373 1 }
374
375 /****************************************************************************/
376
377 ✗ void Controller::update(
378 function<double(unsigned int)> currentValue,
379 function<double(unsigned int)> targetValue,
380 function<bool(unsigned int)> enableIntegrator)
381 {
382 ✗ vector<double> currentVector(impl->error.size());
383 ✗ vector<double> targetVector(impl->error.size());
384 ✗ vector<uint8_t> enableIntegratorVector(impl->error.size());
385
386 ✗ for (unsigned int i = 0; i < impl->error.size(); i++) {
387 ✗ currentVector.at(i) = currentValue(i);
388 ✗ targetVector.at(i) = targetValue(i);
389 ✗ enableIntegratorVector.at(i) = enableIntegrator(i);
390 }
391
392 ✗ impl->update(currentVector, targetVector, enableIntegratorVector);
393 }
394
395 /****************************************************************************/
396
397 ✗ void Controller::update(
398 function<double(unsigned int)> currentValue,
399 function<double(unsigned int)> targetValue,
400 function<bool(unsigned int)> enableIntegrator,
401 function<bool(unsigned int)> integrate)
402 {
403 ✗ vector<double> curVector(impl->error.size());
404 ✗ vector<double> targetVector(impl->error.size());
405 ✗ vector<uint8_t> enableIntegratorVector(impl->error.size());
406 ✗ vector<uint8_t> integrateVector(impl->error.size());
407 ✗ for (unsigned int i = 0; i < impl->error.size(); i++) {
408 ✗ curVector.at(i) = currentValue(i);
409 ✗ targetVector.at(i) = targetValue(i);
410 ✗ enableIntegratorVector.at(i) = enableIntegrator(i);
411 ✗ integrateVector.at(i) = integrate(i);
412 }
413 ✗ update(curVector, targetVector, enableIntegratorVector, integrateVector);
414 }
415
416 /****************************************************************************/
417
418 8 double Controller::getOutput(unsigned int i) const
419 {
420 8 return impl->output.at(i);
421 }
422
423 /****************************************************************************/
424
425 3 double Controller::getError(unsigned int i) const
426 {
427 3 return impl->error.at(i);
428 }
429
430 /****************************************************************************/
431
432 ✗ vector<double> Controller::getOutputVector() const
433 {
434 ✗ return impl->output;
435 }
436
437 /****************************************************************************/
438
439 ✗ vector<double> Controller::getErrorVector() const
440 {
441 ✗ return impl->error;
442 }
443
444 /****************************************************************************/
445
446 2 unsigned int Controller::size() const
447 {
448 2 return impl->output.size();
449 }
450
451 /****************************************************************************/
452