GCC Code Coverage Report

Directory: src/
File: src/Encoder.cpp
Date: 2026-09-03 17:04:53
Exec Total Coverage
Lines: 0 85 0.0%
Branches: 0 100 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/Encoder.h"
24
25 #include "Base.h"
26 #include "reta/Delay.h"
27 #include "reta/UserParameter.h"
28
29 #include <cmath> // fmod(), copysign()
30
31 using namespace Reta;
32 using namespace std;
33
34 /****************************************************************************/
35
36 ✗ struct RETA_LOCAL Encoder::Impl : public Base
37 {
38 Impl(shared_ptr<Task>, const string &, Type, unsigned int);
39
40 void update();
41
42 shared_ptr<Task> task;
43
44 vector<Type> type;
45 vector<uint8_t> inputValid;
46 vector<int16_t> data;
47
48 Delay delayedInputValid;
49 vector<uint8_t> lastDelayedInputValid;
50
51 vector<int16_t> incrementDiff;
52 vector<int16_t> lastData;
53 vector<int> encoderDirection;
54 vector<double> scale;
55 UserParameter setZero;
56 vector<double> userPosition;
57 UserParameter setUserPosition;
58 vector<double> referenceStep;
59 vector<double> currentPosition;
60 vector<double> speedFilterConstant;
61 vector<double> currentSpeed;
62 };
63
64 /****************************************************************************/
65
66 ✗ Encoder::Impl::Impl(
67 shared_ptr<Task> task,
68 const string &prefix,
69 Type type,
70 ✗ unsigned int width) :
71 Base {prefix},
72 task {task},
73 type {type},
74 inputValid {false},
75 data {0},
76 ✗ delayedInputValid(task, prefix + "/DelayedInputValid", 0.1, width),
77 lastDelayedInputValid(width, false),
78 incrementDiff(width, 0),
79 lastData(width, 0),
80 encoderDirection(width, 1),
81 scale(width, 0.0),
82 ✗ setZero(task, prefix + "/SetZero", width),
83 userPosition(width, 0.0),
84 ✗ setUserPosition(task, prefix + "/SetUserPosition", width),
85 referenceStep(width, 0.0),
86 currentPosition(width, 0.0),
87 ✗ speedFilterConstant(width, task->getPeriod()),
88 ✗ currentSpeed(width, 0.0)
89 {
90 ✗ checkZeroWidth(width);
91
92 ✗ pdserv *pdserv {task->getPdServ()};
93 ✗ pdtask *pdtask {task->getPdTask()};
94
95 ✗ pdserv_signal(
96 ✗ pdtask, 1, (prefix + "/InputValid").c_str(), pd_boolean_T,
97 ✗ &inputValid, 1, NULL);
98 ✗ pdserv_signal(
99 ✗ pdtask, 1, (prefix + "/Data").c_str(), pd_sint16_T, &data, 1,
100 NULL);
101 ✗ pdserv_signal(
102 ✗ pdtask, 1, (prefix + "/IncrementDiff").c_str(), pd_sint16_T,
103 ✗ &incrementDiff, 1, NULL);
104 ✗ pdserv_parameter(
105 ✗ pdserv, (prefix + "/EncoderDirection").c_str(), 0666, pd_sint32_T,
106 ✗ &encoderDirection, 1, NULL, NULL, NULL);
107 ✗ pdserv_parameter(
108 ✗ pdserv, (prefix + "/Scale").c_str(), 0666, pd_double_T, &scale, 1,
109 NULL, NULL, NULL);
110 ✗ pdserv_parameter(
111 ✗ pdserv, (prefix + "/UserPosition").c_str(), 0666, pd_double_T,
112 ✗ &userPosition, 1, NULL, NULL, NULL);
113 ✗ pdserv_signal(
114 ✗ pdtask, 1, (prefix + "/ReferenceStep").c_str(), pd_double_T,
115 ✗ &referenceStep, 1, NULL);
116 ✗ pdserv_signal(
117 ✗ pdtask, 1, (prefix + "/CurrentPosition").c_str(), pd_double_T,
118 ✗ &currentPosition, 1, NULL);
119 ✗ pdserv_parameter(
120 ✗ pdserv, (prefix + "/SpeedFilterConstant").c_str(), 0666,
121 ✗ pd_double_T, &speedFilterConstant, 1, NULL, NULL, NULL);
122 ✗ pdserv_signal(
123 ✗ pdtask, 1, (prefix + "/CurrentSpeed").c_str(), pd_double_T,
124 ✗ &currentSpeed, 1, NULL);
125 }
126
127 /****************************************************************************/
128
129 ✗ void Encoder::Impl::update()
130 {
131 ✗ for (unsigned int i = 0; i < currentPosition.size(); i++) {
132 /* Ignore position change until inputs get valid. */
133 ✗ delayedInputValid.updateDown(inputValid[i], false);
134 ✗ if (!delayedInputValid.getOutput(i) || !lastDelayedInputValid[i]) {
135 ✗ lastData = data;
136 }
137 ✗ lastDelayedInputValid[i] = delayedInputValid.getOutput(i);
138
139 ✗ setZero.update();
140 ✗ setUserPosition.update();
141
142 ✗ if (setZero.getChanged(i)) {
143 ✗ referenceStep = currentPosition;
144 }
145 ✗ else if (setUserPosition.getChanged(i)) {
146 ✗ referenceStep[i] = currentPosition[i] - userPosition[i];
147 }
148 else {
149 ✗ referenceStep[i] = 0.0;
150 }
151
152 ✗ currentPosition[i] -= referenceStep[i];
153
154 /* Get the position from the increments. */
155 ✗ incrementDiff[i] = data[i] - lastData[i];
156 ✗ lastData[i] = data[i];
157
158 double relPos {
159 ✗ incrementDiff[i] * copysign(1.0, encoderDirection[i])
160 ✗ * scale[i]};
161
162 ✗ if (type[i] == Rotary) {
163 ✗ currentPosition[i] =
164 ✗ fmod(currentPosition[i] + relPos + 2 * M_PI, 2 * M_PI);
165 }
166 else { // linear
167 ✗ currentPosition[i] += relPos;
168 }
169
170 /* Differentiate position (with filter) to get the velocity. */
171 ✗ double rawSpeed {relPos / task->getPeriod()};
172 ✗ currentSpeed[i] = rawSpeed * speedFilterConstant[i]
173 ✗ + (1 - speedFilterConstant[i]) * currentSpeed[i];
174 }
175 }
176
177
178 /****************************************************************************/
179
180 ✗ Encoder::Encoder(
181 shared_ptr<Task> task,
182 const std::string &prefix,
183 Type type,
184 ✗ unsigned int width) :
185 ✗ impl {make_unique<Impl>(task, prefix, type, width)}
186 {}
187
188 /****************************************************************************/
189
190 ✗ Encoder::~Encoder()
191 {}
192
193 /****************************************************************************/
194
195 ✗ void Encoder::setProcessData(bool inputValid, int16_t data, unsigned int i)
196 {
197 ✗ impl->inputValid.at(i) = inputValid;
198 ✗ impl->data.at(i) = data;
199 }
200
201 /****************************************************************************/
202
203 ✗ void Encoder::update()
204 {
205 ✗ impl->update();
206 }
207
208 /****************************************************************************/
209
210 ✗ double Encoder::getPosition(unsigned int i) const
211 {
212 ✗ return impl->currentPosition.at(i);
213 }
214
215 /****************************************************************************/
216
217 ✗ vector<double> Encoder::getPositionVector() const
218 {
219 ✗ return impl->currentPosition;
220 }
221
222 /****************************************************************************/
223
224 ✗ double Encoder::getReferenceStep(unsigned int i) const
225 {
226 ✗ return impl->referenceStep.at(i);
227 }
228
229 /****************************************************************************/
230
231 ✗ vector<double> Encoder::getReferenceStepVector() const
232 {
233 ✗ return impl->referenceStep;
234 }
235
236 /****************************************************************************/
237
238 ✗ double Encoder::getAngleSpeed(unsigned int i) const
239 {
240 ✗ return impl->currentSpeed.at(i);
241 }
242
243 /****************************************************************************/
244
245 ✗ vector<double> Encoder::getAngleSpeedVector() const
246 {
247 ✗ return impl->currentSpeed;
248 }
249
250 /****************************************************************************/
251