GCC Code Coverage Report

Directory: src/
File: src/Controller.cpp
Date: 2026-09-03 17:04:53
Exec Total Coverage
Lines: 104 142 73.2%
Branches: 66 178 37.1%
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
27 #include <cmath> // fabs(), copysign()
28
29 using std::copysign;
30 using std::fabs;
31 using std::function;
32 using std::make_unique;
33 using std::shared_ptr;
34 using std::string;
35 using std::vector;
36
37 using namespace Reta;
38
39 /****************************************************************************/
40
41 21 struct RETA_LOCAL Controller::Impl : public Base
42 {
43 Impl(shared_ptr<Task>,
44 const string &,
45 double,
46 double,
47 unsigned int,
48 Controller::LimitMode,
49 Controller::InvertMode);
50
51 void
52 update(const vector<double> &,
53 const vector<double> &,
54 const vector<uint8_t> &,
55 const vector<uint8_t> & = vector<uint8_t>());
56
57 double limitIOut(const double &, unsigned int);
58 shared_ptr<Task> task;
59
60 vector<double> error;
61
62 vector<double> kp;
63 vector<double> pOut;
64 vector<double> ki;
65 vector<double> iOut;
66
67 vector<double> kPilot;
68 vector<double> minPilotValue;
69 vector<double> pilotControl;
70
71 double invert;
72 Controller::LimitMode limitMode;
73 double lowerLimit;
74 double upperLimit;
75 vector<double> output;
76 };
77
78 /****************************************************************************/
79
80 22 Controller::Impl::Impl(
81 shared_ptr<Task> task,
82 const string &prefix,
83 double kpInit,
84 double kiInit,
85 unsigned int width,
86 Controller::LimitMode limitModeInit,
87 22 Controller::InvertMode invertModeInit) :
88 Base {prefix},
89 task {task},
90 error(width, 0.0),
91 kp(width, kpInit),
92 pOut(width, 0.0),
93 ki(width, kiInit),
94 iOut(width, 0.0),
95 kPilot(width, 0.0),
96 minPilotValue(width, 0.0),
97 pilotControl(width, 0.0),
98
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
 22 invert {invertModeInit == Controller::Invert ? -1.0 : 1.0},
99 limitMode {limitModeInit},
100 lowerLimit {-1.0},
101 upperLimit {1.0},
102
9/18
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 22 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 22 times.
✗ Branch 25 not taken.
✓ Branch 29 taken 22 times.
✗ Branch 30 not taken.
✓ Branch 34 taken 22 times.
✗ Branch 35 not taken.
✓ Branch 39 taken 22 times.
✗ Branch 40 not taken.
✓ Branch 44 taken 22 times.
✗ Branch 45 not taken.
 45 output(width, 0.0)
103 {
104
2/2
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 1 times.
 22 checkZeroWidth(width);
105
106
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 pdserv *pdserv {task->getPdServ()};
107
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 pdtask *pdtask {task->getPdTask()};
108
109
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
110
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdtask, 1, (prefix + "/Error").c_str(), pd_double_T, error.data(),
111 error.size(), NULL);
112
113
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_parameter(
114
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdserv, (prefix + "/Kp").c_str(), 0666, pd_double_T, kp.data(),
115 kp.size(), NULL, NULL, NULL);
116
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
117
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdtask, 1, (prefix + "/KpSignal").c_str(), pd_double_T, kp.data(),
118 kp.size(), NULL);
119
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
120
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdtask, 1, (prefix + "/POut").c_str(), pd_double_T, pOut.data(),
121 pOut.size(), NULL);
122
123
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_parameter(
124
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdserv, (prefix + "/Ki").c_str(), 0666, pd_double_T, ki.data(),
125 ki.size(), NULL, NULL, NULL);
126
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
127
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdtask, 1, (prefix + "/KiSignal").c_str(), pd_double_T, ki.data(),
128 ki.size(), NULL);
129
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
130
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 pdtask, 1, (prefix + "/IOut").c_str(), pd_double_T, iOut.data(),
131 iOut.size(), NULL);
132
133
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_parameter(
134
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 pdserv, (prefix + "/KPilot").c_str(), 0666, pd_double_T,
135 21 kPilot.data(), kPilot.size(), NULL, NULL, NULL);
136
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_parameter(
137
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 pdserv, (prefix + "/MinPilotValue").c_str(), 0666, pd_double_T,
138 21 minPilotValue.data(), minPilotValue.size(), NULL, NULL, NULL);
139
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
140
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 pdtask, 1, (prefix + "/PilotControl").c_str(), pd_double_T,
141 21 pilotControl.data(), pilotControl.size(), NULL);
142
143
1/2
✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
 21 if (limitMode == Controller::StaticLimits) {
144
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 pdserv_parameter(
145
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 pdserv, (prefix + "/LowerLimit").c_str(), 0666, pd_double_T,
146 21 &lowerLimit, 1, NULL, NULL, NULL);
147
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 pdserv_parameter(
148
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 pdserv, (prefix + "/UpperLimit").c_str(), 0666, pd_double_T,
149 21 &upperLimit, 1, NULL, NULL, NULL);
150 }
151 else { // DynamicLimits
152 ✗ pdserv_signal(
153 ✗ pdtask, 1, (prefix + "/LowerLimit").c_str(), pd_double_T,
154 ✗ &lowerLimit, 1, NULL);
155 ✗ pdserv_signal(
156 ✗ pdtask, 1, (prefix + "/UpperLimit").c_str(), pd_double_T,
157 ✗ &upperLimit, 1, NULL);
158 }
159
160
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 pdserv_signal(
161
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 pdtask, 1, (prefix + "/Output").c_str(), pd_double_T,
162 21 output.data(), output.size(), NULL);
163 21 }
164
165 /****************************************************************************/
166
167 21 Controller::~Controller()
168 21 {}
169
170 /****************************************************************************/
171
172 10 void Controller::Impl::update(
173 const vector<double> &currentValue,
174 const vector<double> &targetValue,
175 const vector<uint8_t> &enableIntegrator,
176 const vector<uint8_t> &integrate)
177 {
178 10 checkSize(currentValue, output);
179 9 checkSize(targetValue, output);
180 9 checkSize(enableIntegrator, output);
181 9 checkSize(integrate, output);
182
183
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 9 times.
 21 for (unsigned int i = 0; i < error.size(); i++) {
184 12 error.at(i) = (targetValue.at(i) - currentValue.at(i)) * invert;
185
186 24 pilotControl.at(i) = fabs(pilotControl.at(i)) > minPilotValue.at(i)
187
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
 24 ? targetValue.at(i) * kPilot.at(i)
188 12 : copysign(1.0, pilotControl.at(i)) * minPilotValue.at(i);
189
190 12 pOut.at(i) = error.at(i) * kp.at(i);
191
192
6/6
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 8 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 10 times.
 12 if (enableIntegrator.at(i) && ki.at(i) != 0.0) {
193
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)) {
194 2 iOut.at(i) += error.at(i) * ki.at(i) * task->getPeriod();
195 }
196
197 4 output.at(i) = pOut.at(i) + limitIOut(iOut.at(i), i)
198 2 + pilotControl.at(i);
199 }
200 else { // without integration
201
202 10 iOut.at(i) = 0.0;
203
204 10 double auxOutput {pOut.at(i) + pilotControl.at(i)};
205
206
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 8 times.
 10 if (auxOutput > upperLimit) {
207 2 auxOutput = upperLimit;
208 }
209
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 6 times.
 8 else if (auxOutput < lowerLimit) {
210 2 auxOutput = lowerLimit;
211 }
212
213 10 output.at(i) = auxOutput;
214 }
215 }
216 9 }
217
218 /****************************************************************************/
219
220 10 double Controller::Impl::limitIOut(const double &outputValue, unsigned int i)
221 {
222 10 double upperOutputLimit {upperLimit - pilotControl.at(i) - pOut.at(i)};
223 8 double lowerOutputLimit {lowerLimit - pilotControl.at(i) - pOut.at(i)};
224
225
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 if (iOut.at(i) > upperOutputLimit) {
226 ✗ return upperOutputLimit;
227 }
228
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 else if (iOut.at(i) < lowerOutputLimit) {
229 ✗ return lowerOutputLimit;
230 }
231
232 8 return outputValue;
233 }
234
235 /****************************************************************************/
236
237 22 Controller::Controller(
238 shared_ptr<Task> task,
239 const string &prefix,
240 double kpInit,
241 double kiInit,
242 unsigned int width,
243 LimitMode limitMode,
244 22 InvertMode invertMode) :
245 impl {make_unique<
246 22 Impl>(task, prefix, kpInit, kiInit, width, limitMode, invertMode)}
247 21 {}
248
249 /****************************************************************************/
250
251 8 void Controller::setIntegratorValue(const double &value, unsigned int i)
252 {
253 8 impl->iOut.at(i) = impl->limitIOut(value, i);
254 6 }
255
256 /****************************************************************************/
257
258 2 void Controller::setIntegratorValueVector(const vector<double> &value)
259 {
260 2 impl->checkSize(value, impl->output);
261
262
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
 5 for (unsigned int i = 0; i < value.size(); i++) {
263 4 setIntegratorValue(value.at(i), i);
264 }
265 1 }
266
267 /****************************************************************************/
268
269 21 void Controller::setLimits(double lower, double upper)
270 {
271 21 impl->lowerLimit = lower;
272 21 impl->upperLimit = upper;
273 21 }
274
275 /****************************************************************************/
276
277 ✗ void Controller::setOutput(double value, unsigned int i)
278 {
279 ✗ if (value > impl->upperLimit) {
280 ✗ value = impl->upperLimit;
281 }
282 ✗ if (value < impl->lowerLimit) {
283 ✗ value = impl->lowerLimit;
284 }
285
286 ✗ impl->iOut.at(i) = value - impl->pOut.at(i) - impl->pilotControl.at(i);
287 }
288
289 /****************************************************************************/
290
291 8 void Controller::update(
292 double currentValue,
293 double targetValue,
294 bool enableIntegrator,
295 bool integrate)
296 {
297
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 impl->update(
298
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 16 vector<double> {currentValue}, vector<double> {targetValue},
299
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 16 vector<uint8_t> {enableIntegrator}, vector<uint8_t> {integrate});
300 8 }
301
302 /****************************************************************************/
303
304 2 void Controller::update(
305 const vector<double> &currentValue,
306 const vector<double> &targetValue,
307 const vector<uint8_t> &enableIntegrator,
308 const vector<uint8_t> &integrate)
309 {
310 2 impl->update(currentValue, targetValue, enableIntegrator, integrate);
311 1 }
312
313 /****************************************************************************/
314
315 ✗ void Controller::update(
316 function<double(unsigned int)> currentValue,
317 function<double(unsigned int)> targetValue,
318 function<bool(unsigned int)> enableIntegrator)
319 {
320 ✗ vector<double> currentVector(impl->error.size());
321 ✗ vector<double> targetVector(impl->error.size());
322 ✗ vector<uint8_t> enableIntegratorVector(impl->error.size());
323
324 ✗ for (unsigned int i = 0; i < impl->error.size(); i++) {
325 ✗ currentVector.at(i) = currentValue(i);
326 ✗ targetVector.at(i) = targetValue(i);
327 ✗ enableIntegratorVector.at(i) = enableIntegrator(i);
328 }
329
330 ✗ impl->update(currentVector, targetVector, enableIntegratorVector);
331 }
332
333 /****************************************************************************/
334
335 ✗ void Controller::update(
336 function<double(unsigned int)> currentValue,
337 function<double(unsigned int)> targetValue,
338 function<bool(unsigned int)> enableIntegrator,
339 function<bool(unsigned int)> integrate)
340 {
341 ✗ vector<double> curVector(impl->error.size());
342 ✗ vector<double> targetVector(impl->error.size());
343 ✗ vector<uint8_t> enableIntegratorVector(impl->error.size());
344 ✗ vector<uint8_t> integrateVector(impl->error.size());
345 ✗ for (unsigned int i = 0; i < impl->error.size(); i++) {
346 ✗ curVector.at(i) = currentValue(i);
347 ✗ targetVector.at(i) = targetValue(i);
348 ✗ enableIntegratorVector.at(i) = enableIntegrator(i);
349 ✗ integrateVector.at(i) = integrate(i);
350 }
351 ✗ update(curVector, targetVector, enableIntegratorVector, integrateVector);
352 }
353
354 /****************************************************************************/
355
356 6 double Controller::getOutput(unsigned int i) const
357 {
358 6 return impl->output.at(i);
359 }
360
361 /****************************************************************************/
362
363 2 double Controller::getError(unsigned int i) const
364 {
365 2 return impl->error.at(i);
366 }
367
368 /****************************************************************************/
369
370 ✗ vector<double> Controller::getOutputVector() const
371 {
372 ✗ return impl->output;
373 }
374
375 /****************************************************************************/
376
377 ✗ vector<double> Controller::getErrorVector() const
378 {
379 ✗ return impl->error;
380 }
381
382 /****************************************************************************/
383
384 2 unsigned int Controller::size() const
385 {
386 2 return impl->output.size();
387 }
388
389 /****************************************************************************/
390