GCC Code Coverage Report

Directory: src/
File: src/Limiter.cpp
Date: 2026-09-03 17:04:53
Exec Total Coverage
Lines: 0 74 0.0%
Branches: 0 92 0.0%
Line Branch Exec Source
1 /*****************************************************************************
2 *
3 * Copyright (C) 2021 Jonathan Grahl <jonathan.grahl@igh.de>
4 * 2021 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/Limiter.h"
24
25 #include "Base.h"
26
27 #include <cmath> // fabs()
28
29 using std::fabs;
30 using std::function;
31 using std::make_unique;
32 using std::shared_ptr;
33 using std::string;
34 using std::vector;
35
36 using namespace Reta;
37
38 /****************************************************************************/
39
40 ✗ struct RETA_LOCAL Limiter::Impl : public Base
41 {
42 Impl(shared_ptr<Task>,
43 const string &,
44 double,
45 double,
46 double,
47 unsigned int);
48
49 void update(const vector<double> &, const vector<double> &);
50
51 shared_ptr<Task> task;
52
53 vector<double> limit;
54 vector<double> error;
55
56 vector<double> kp;
57 vector<double> pOut;
58
59 vector<double> integratorValue;
60 vector<double> ki;
61
62 vector<double> limitState;
63 vector<double> output;
64 };
65
66 /****************************************************************************/
67
68 ✗ Limiter::Impl::Impl(
69 shared_ptr<Task> task,
70 const string &prefix,
71 double limitInit,
72 double kpInit,
73 double kiInit,
74 ✗ unsigned int width) :
75 Base {prefix},
76 task {task},
77 limit(width, limitInit),
78 error(width, 0.0),
79 kp(width, kpInit),
80 pOut(width, 0.0),
81 integratorValue(width, 0.0),
82 ki(width, kiInit),
83 limitState(width, 0.0),
84 ✗ output(width, 0.0)
85 {
86 ✗ checkZeroWidth(width);
87
88 ✗ pdserv *pdserv {task->getPdServ()};
89 ✗ pdtask *pdtask {task->getPdTask()};
90
91 ✗ pdserv_parameter(
92 ✗ pdserv, (prefix + "/Limit").c_str(), 0666, pd_double_T,
93 ✗ limit.data(), limit.size(), NULL, NULL, NULL);
94 ✗ pdserv_signal(
95 ✗ pdtask, 1, (prefix + "/LimitSignal").c_str(), pd_double_T,
96 ✗ limit.data(), limit.size(), NULL);
97 ✗ pdserv_signal(
98 ✗ pdtask, 1, (prefix + "/Error").c_str(), pd_double_T, error.data(),
99 error.size(), NULL);
100 ✗ pdserv_parameter(
101 ✗ pdserv, (prefix + "/Kp").c_str(), 0666, pd_double_T, kp.data(),
102 kp.size(), NULL, NULL, NULL);
103 ✗ pdserv_signal(
104 ✗ pdtask, 1, (prefix + "/POut").c_str(), pd_double_T, pOut.data(),
105 pOut.size(), NULL);
106 ✗ pdserv_signal(
107 ✗ pdtask, 1, (prefix + "/IntegratorValue").c_str(), pd_double_T,
108 ✗ integratorValue.data(), integratorValue.size(), NULL);
109 ✗ pdserv_parameter(
110 ✗ pdserv, (prefix + "/Ki").c_str(), 0666, pd_double_T, ki.data(),
111 ki.size(), NULL, NULL, NULL);
112 ✗ pdserv_signal(
113 ✗ pdtask, 1, (prefix + "/LimitState").c_str(), pd_double_T,
114 ✗ limitState.data(), limitState.size(), NULL);
115 ✗ pdserv_signal(
116 ✗ pdtask, 1, (prefix + "/Output").c_str(), pd_double_T,
117 ✗ output.data(), output.size(), NULL);
118 }
119
120 /****************************************************************************/
121
122 ✗ void Limiter::Impl::update(
123 const vector<double> &signal, /**< Signal to limit. */
124 const vector<double> &current /**< Controller input. */
125 )
126 {
127 ✗ checkSize(signal, output);
128 ✗ checkSize(current, output);
129
130 ✗ for (unsigned int i = 0; i < limit.size(); i++) {
131 ✗ error[i] = limit[i] - fabs(current[i]);
132
133 /* Proportional part */
134 ✗ pOut[i] = error[i] * kp[i];
135
136 /* Integration part */
137
138 ✗ if (ki[i] != 0.0) {
139 ✗ integratorValue[i] += error[i] * ki[i] * task->getPeriod();
140 ✗ if (integratorValue[i] > 1.0) {
141 ✗ integratorValue[i] = 1.0;
142 }
143 ✗ else if (integratorValue[i] < 0.0) {
144 ✗ integratorValue[i] = 0.0;
145 }
146 }
147 else { // without integration
148 ✗ integratorValue[i] = 0.0;
149 }
150
151 ✗ limitState[i] = pOut[i] + integratorValue[i];
152 ✗ if (limitState[i] > 1.0) {
153 ✗ limitState[i] = 1.0;
154 }
155 ✗ else if (limitState[i] < 0.0) {
156 ✗ limitState[i] = 0.0;
157 }
158
159 // limit input signal
160 ✗ if (signal[i] > limitState[i]) {
161 ✗ output[i] = limitState[i];
162 }
163 ✗ else if (signal[i] < -limitState[i]) {
164 ✗ output[i] = -limitState[i];
165 }
166 else {
167 ✗ output[i] = signal[i];
168 }
169 }
170 }
171
172 /****************************************************************************/
173
174 ✗ Limiter::Limiter(
175 shared_ptr<Task> task,
176 const string &prefix,
177 double limitInit,
178 double kpInit,
179 double kiInit,
180 ✗ unsigned int width) :
181 ✗ impl {make_unique<Impl>(task, prefix, limitInit, kpInit, kiInit, width)}
182 {}
183
184 /****************************************************************************/
185
186 ✗ Limiter::~Limiter()
187 {}
188
189 /****************************************************************************/
190
191 ✗ void Limiter::update(
192 double signal, /**< Signal to limit. */
193 double current /**< Controller input. */
194 )
195 {
196 ✗ update(vector<double> {signal}, vector<double> {current});
197 }
198
199 /****************************************************************************/
200
201 ✗ void Limiter::update(
202 const vector<double> &signal,
203 const vector<double> &current)
204 {
205 ✗ impl->update(signal, current);
206 }
207
208 /****************************************************************************/
209
210 ✗ void Limiter::update(
211 function<double(unsigned int)> signal,
212 function<double(unsigned int)> current)
213 {
214 ✗ vector<double> signalVector(impl->limit.size());
215 ✗ vector<double> currentVector(impl->limit.size());
216 ✗ for (unsigned int i = 0; i < impl->limit.size(); i++) {
217 ✗ signalVector[i] = signal(i);
218 ✗ currentVector[i] = current(i);
219 }
220 ✗ update(signalVector, currentVector);
221 }
222
223 /****************************************************************************/
224
225 ✗ double Limiter::getOutput(unsigned int i) const
226 {
227 ✗ return impl->output.at(i);
228 }
229
230 /****************************************************************************/
231
232 ✗ vector<double> Limiter::getOutputVector() const
233 {
234 ✗ return impl->output;
235 }
236
237 /****************************************************************************/
238
239 ✗ bool Limiter::getLimiting(unsigned int i) const
240 {
241 ✗ return impl->integratorValue.at(i) < 1.0;
242 }
243
244 /****************************************************************************/
245