GCC Code Coverage Report

Directory: ./
File: src/devices/Movitrac.cpp
Date: 2026-09-03 16:05:57
Exec Total Coverage
Lines: 77 155 49.7%
Branches: 43 400 10.8%
Line Branch Exec Source
1 /*****************************************************************************
2 *
3 * This file is part of the ecapp library (EtherCAT application devices).
4 *
5 * Copyright (C) 2026 Florian Pose <fp@igh.de>
6 *
7 * The ecapp library is free software: you can redistribute it and/or
8 * modify it under the terms of the GNU Lesser General Public License
9 * as published by the Free Software Foundation, version 3 of the
10 * License.
11 *
12 * The ecapp library is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 * Lesser General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public
18 * License along with the ecapp library. If not, see
19 * <https://www.gnu.org/licenses/>.
20 *
21 ****************************************************************************/
22
23 #include "devices/Movitrac.h"
24
25 #include "SlaveContext.h"
26 #include "ecapp/Domain.h"
27 #include "ecapp/Exceptions.h"
28 #include "ecapp/Factory.h"
29 #include "ecapp/Master.h"
30 #include "ecapp/SwappedSync.h"
31
32 #include <cmath>
33 #include <memory>
34 #include <sstream>
35 #include <stdexcept>
36
37 using std::make_unique;
38 using std::runtime_error;
39 using std::size_t;
40 using std::stringstream;
41 using std::unique_ptr;
42
43 /****************************************************************************/
44
45 namespace EcApp {
46
47 /* Statuswort (aus Dokument 31556108.pdf)
48 *
49 * 0 Endstufe freigegeben "1" / Endstufe gesperrt "0"
50 * 1 Umrichter betriebsbereit "1" / Umrichter nicht betriebsbereit "0"
51 * 2 PA-Daten freigegeben "1" / PA-Daten gesperrt "0"
52 * 3 Aktueller Integratorsatz: Integrator 2 "1" / Integrator 1 "0"
53 * 4 Aktueller Parametersatz: Parametersatz 2 "1" / Parametersatz 1 "0"
54 * 5 Störung / Warnung: Störung / Warnung liegt an "1" / Keine Störung "0"
55 */
56
57 /* Master 0, Slave 5, "MOVITRAC+FSE24B"
58 * Vendor ID: 0x00000059
59 * Product code: 0x00000004
60 * Revision number: 0x00000002
61 */
62
63 ec_pdo_entry_info_t movitrac_pdo_entries[] = {
64 {0x3db8, 0x00, 16}, /* PO 0001 Steuerwort */
65 {0x3db9, 0x00, 16}, /* PO 0002 Sollgeschw. LSW */
66 {0x3dba, 0x00, 16}, /* PO 0003 Sollgeschw. MSW */
67 {0x3e1c, 0x00, 16}, /* PI 0001 Statuswort */
68 {0x3e1d, 0x00, 16}, /* PI 0002 Istgeschw. LSW */
69 {0x3e1e, 0x00, 16}, /* PI 0003 Istgeschw. MSW */
70 };
71
72 ec_pdo_info_t movitrac_pdos[] = {
73 {0x1600, 3, movitrac_pdo_entries + 0}, /* RxPDO 1 mapping */
74 {0x1a00, 3, movitrac_pdo_entries + 3}, /* TxPDO 1 mapping */
75 };
76
77 ec_sync_info_t movitrac_syncs[] = {
78 {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
79 {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
80 {2, EC_DIR_OUTPUT, 1, movitrac_pdos + 0, EC_WD_ENABLE},
81 {3, EC_DIR_INPUT, 1, movitrac_pdos + 1, EC_WD_DISABLE},
82 {0xff}};
83
84 /****************************************************************************/
85
86 template <Mode mode>
87 1 Movitrac<mode>::Movitrac(
88 pdserv *pdserv,
89 pdtask *task,
90 const std::string &prefix,
91 Master *master,
92 uint16_t alias,
93 uint16_t position,
94 Domain *domainOut,
95 Domain *domainIn) :
96 sc(NULL),
97 domainOut(domainOut),
98 domainIn(domainIn),
99 offControl(-1),
100 offTargetSpeed(-1),
101 offStatus(-1),
102 offCurrentSpeed(-1),
103 enable(false),
104 controlWord(0x0002),
105 statusWord(0x0002),
106 targetSpeed(0.0),
107 currentSpeed(0.0),
108 speedScale(M_PI / 150.0), // 157.08 rad/s ~ 1500 RPM ~ 7500 inc.
109 enableForcing(false),
110 forcedControlWord(0x0002),
111 forcedTargetSpeed(0.0),
112 effControlWord(0x0002),
113 effTargetSpeed(0.0),
114 rawTargetSpeed(0x0000),
115 status0Enabled(false),
116 status1Ready(false),
117 status2PaDataEnabled(false),
118 status5FaultWarning(false),
119 faultFlag(false),
120 warning(false),
121 deviceState(0),
122 1 errorNumber(0)
123 {
124
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 if (pdserv && task) {
125 ✗ pdserv_signal(
126 task, 1, (prefix + "/Enable").c_str(), pd_boolean_T, &enable,
127 1, NULL);
128 ✗ pdserv_signal(
129 task, 1, (prefix + "/ControlWord").c_str(), pd_uint16_T,
130 &controlWord, 1, NULL);
131 ✗ pdserv_signal(
132 task, 1, (prefix + "/TargetSpeed").c_str(), pd_double_T,
133 &targetSpeed, 1, NULL);
134 ✗ pdserv_signal(
135 task, 1, (prefix + "/StatusWord").c_str(), pd_uint16_T,
136 &statusWord, 1, NULL);
137 ✗ pdserv_signal(
138 task, 1, (prefix + "/CurrentSpeed").c_str(), pd_double_T,
139 &currentSpeed, 1, NULL);
140 ✗ pdserv_parameter(
141 pdserv, (prefix + "/SpeedScale").c_str(), 0666, pd_double_T,
142 &speedScale, 1, NULL, NULL, NULL);
143 ✗ pdserv_parameter(
144 pdserv, (prefix + "/EnableForcing").c_str(), 0666,
145 pd_boolean_T, &enableForcing, 1, NULL, NULL, NULL);
146 ✗ pdserv_parameter(
147 pdserv, (prefix + "/ForcedControlWord").c_str(), 0666,
148 pd_uint16_T, &forcedControlWord, 1, NULL, NULL, NULL);
149 ✗ pdserv_parameter(
150 pdserv, (prefix + "/ForcedTargetSpeed").c_str(), 0666,
151 pd_double_T, &forcedTargetSpeed, 1, NULL, NULL, NULL);
152 ✗ pdserv_signal(
153 task, 1, (prefix + "/EffControlWord").c_str(), pd_uint16_T,
154 &effControlWord, 1, NULL);
155 ✗ pdserv_signal(
156 task, 1, (prefix + "/EffTargetSpeed").c_str(), pd_double_T,
157 &effTargetSpeed, 1, NULL);
158 ✗ pdserv_signal(
159 task, 1, (prefix + "/RawTargetSpeed").c_str(), pd_sint16_T,
160 &rawTargetSpeed, 1, NULL);
161
162 ✗ pdserv_signal(
163 task, 1, (prefix + "/Status_0_Enabled").c_str(), pd_boolean_T,
164 &status0Enabled, 1, NULL);
165 ✗ pdserv_signal(
166 task, 1, (prefix + "/Status_1_Ready").c_str(), pd_boolean_T,
167 &status1Ready, 1, NULL);
168 ✗ pdserv_signal(
169 task, 1, (prefix + "/Status_2_PaDataEnabled").c_str(),
170 pd_boolean_T, &status2PaDataEnabled, 1, NULL);
171 ✗ pdserv_signal(
172 task, 1, (prefix + "/Status_5_FaultWarning").c_str(),
173 pd_boolean_T, &status5FaultWarning, 1, NULL);
174 ✗ pdserv_signal(
175 task, 1, (prefix + "/Fault").c_str(), pd_boolean_T,
176 &faultFlag, 1, NULL);
177 ✗ pdserv_signal(
178 task, 1, (prefix + "/Warning").c_str(), pd_boolean_T,
179 &warning, 1, NULL);
180 ✗ pdserv_signal(
181 task, 1, (prefix + "/DeviceState").c_str(), pd_uint8_T,
182 &deviceState, 1, NULL);
183 ✗ pdserv_signal(
184 task, 1, (prefix + "/ErrorNumber").c_str(), pd_uint8_T,
185 &errorNumber, 1, NULL);
186 }
187
188
1/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 if (!master) {
189 1 return;
190 }
191
192 ✗ sc = ecrt_master_slave_config(
193 master->getMaster(), alias, position, 0x00000059, 0x00000004);
194 ✗ if (!sc) {
195 ✗ stringstream err;
196 ✗ err << "Failed to get slave configuration for Movitrac.";
197 ✗ throw runtime_error(err.str());
198 }
199
200 ✗ if (ecrt_slave_config_pdos(
201 sc, EC_END, SwappedSync<mode>(movitrac_syncs).getSync())) {
202 ✗ stringstream err;
203 ✗ err << "Failed to configure PDOs of Movitrac.";
204 ✗ throw runtime_error(err.str());
205 }
206
207 ✗ offControl = ecrt_slave_config_reg_pdo_entry(
208 sc, 0x3db8, 0x00, domainOut->getDomain(), NULL);
209 ✗ if (offControl < 0) {
210 ✗ stringstream err;
211 ✗ err << "Failed to register Movitrac control word PDO entry.";
212 ✗ throw runtime_error(err.str());
213 }
214
215 ✗ offTargetSpeed = ecrt_slave_config_reg_pdo_entry(
216 sc, 0x3db9, 0x00, domainOut->getDomain(), NULL);
217 ✗ if (offTargetSpeed < 0) {
218 ✗ stringstream err;
219 ✗ err << "Failed to register Movitrac target speed PDO entry.";
220 ✗ throw runtime_error(err.str());
221 }
222
223 ✗ offStatus = ecrt_slave_config_reg_pdo_entry(
224 sc, 0x3e1c, 0x00, domainIn->getDomain(), NULL);
225 ✗ if (offStatus < 0) {
226 ✗ stringstream err;
227 ✗ err << "Failed to register Movitrac status word PDO entry.";
228 ✗ throw runtime_error(err.str());
229 }
230
231 ✗ offCurrentSpeed = ecrt_slave_config_reg_pdo_entry(
232 sc, 0x3e1d, 0x00, domainIn->getDomain(), NULL);
233 ✗ if (offCurrentSpeed < 0) {
234 ✗ stringstream err;
235 ✗ err << "Failed to register Movitrac current speed PDO entry.";
236 ✗ throw runtime_error(err.str());
237 }
238 }
239
240 /****************************************************************************/
241
242 template <Mode mode>
243 1 void Movitrac<mode>::updateInputs()
244 {
245
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1 if (domainIn->getData()) {
246 ✗ statusWord = EC_READ_U16(domainIn->getData() + offStatus);
247 ✗ currentSpeed = EC_READ_S16(domainIn->getData() + offCurrentSpeed)
248 ✗ * speedScale;
249 }
250 else {
251 1 statusWord = 0x0000;
252 1 currentSpeed = 0.0;
253 }
254
255 1 status0Enabled = (statusWord & 0x0001) != 0;
256 1 status1Ready = (statusWord & 0x0002) != 0;
257 1 status2PaDataEnabled = (statusWord & 0x0004) != 0;
258 1 status5FaultWarning = (statusWord & 0x0020) != 0;
259
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 faultFlag = status5FaultWarning && !status1Ready;
260
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 warning = status5FaultWarning && status1Ready;
261
262
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 deviceState = status5FaultWarning ? 0x00 : ((statusWord & 0xff00) >> 8);
263
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 errorNumber = status5FaultWarning ? ((statusWord & 0xff00) >> 8) : 0x00;
264 1 }
265
266 /****************************************************************************/
267
268 template <Mode mode>
269 1 void Movitrac<mode>::setEnableMotor(bool en)
270 {
271 1 enable = en;
272 1 }
273
274 /****************************************************************************/
275
276 template <Mode mode>
277 1 void Movitrac<mode>::setTargetSpeed(double target)
278 {
279 1 targetSpeed = target;
280 1 }
281
282 /****************************************************************************/
283
284 template <Mode mode>
285 1 void Movitrac<mode>::updateOutputs()
286 {
287
1/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 controlWord = enable ? 0x0006 : 0x0002;
288
289
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 if (enableForcing) {
290 ✗ effControlWord = forcedControlWord;
291 ✗ effTargetSpeed = forcedTargetSpeed;
292 }
293 else {
294 1 effControlWord = controlWord;
295 1 effTargetSpeed = targetSpeed;
296 }
297
298
1/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 if (speedScale) {
299 1 rawTargetSpeed = effTargetSpeed / speedScale;
300 }
301 else {
302 ✗ rawTargetSpeed = 0x0000;
303 }
304
305
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1 if (domainOut->getData()) {
306 ✗ EC_WRITE_U16(domainOut->getData() + offControl, effControlWord);
307 ✗ EC_WRITE_S16(domainOut->getData() + offTargetSpeed, rawTargetSpeed);
308 }
309 1 }
310
311 /****************************************************************************/
312
313 template class Movitrac<Mode::Control>;
314 template class Movitrac<Mode::Simulation>;
315
316 /****************************************************************************/
317 // Generic SubDevice adapter. Every channel is its own distinctly-named
318 // scalar (no repeated kind) -- application code never names Movitrac,
319 // only "Movitrac" itself as the createSubDevice() name (there is only
320 // the one product).
321 /****************************************************************************/
322
323 template <Mode mode>
324 2 class MovitracAdapter : public SubDevice
325 {
326 public:
327 1 explicit MovitracAdapter(const SlaveContext &ctx) :
328 1 device(ctx.pdServ,
329 1 ctx.task,
330 ctx.prefix,
331 1 ctx.master,
332 1 ctx.alias,
333 1 ctx.position,
334 1 ctx.domainOut,
335 1 ctx.domainIn),
336 inputKinds_ {
337 {"StatusWord", 1, uint16_t(0)},
338 {"CurrentSpeed", 1, double(0)},
339 {"Status0Enabled", 1, false},
340 {"Status1Ready", 1, false},
341 {"Status2PaDataEnabled", 1, false},
342 {"Status5FaultWarning", 1, false},
343 {"Fault", 1, false},
344 {"Warning", 1, false},
345 {"DeviceState", 1, uint16_t(0)},
346 {"ErrorNumber", 1, uint16_t(0)},
347 {"AnyFault", 1, false}},
348 outputKinds_ {
349 {"EnableMotor", 1, false},
350
20/80
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✓ Branch 42 taken 1 times.
✗ Branch 43 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 50 taken 1 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 11 times.
✓ Branch 54 taken 1 times.
✓ Branch 69 taken 1 times.
✗ Branch 70 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 2 times.
✓ Branch 81 taken 1 times.
✗ Branch 86 not taken.
✗ Branch 87 not taken.
✗ Branch 101 not taken.
✗ Branch 102 not taken.
✗ Branch 110 not taken.
✗ Branch 111 not taken.
✗ Branch 114 not taken.
✗ Branch 115 not taken.
✗ Branch 118 not taken.
✗ Branch 119 not taken.
✗ Branch 122 not taken.
✗ Branch 123 not taken.
✗ Branch 126 not taken.
✗ Branch 127 not taken.
✗ Branch 130 not taken.
✗ Branch 131 not taken.
✗ Branch 134 not taken.
✗ Branch 135 not taken.
✗ Branch 138 not taken.
✗ Branch 139 not taken.
✗ Branch 142 not taken.
✗ Branch 143 not taken.
✗ Branch 146 not taken.
✗ Branch 147 not taken.
✗ Branch 150 not taken.
✗ Branch 151 not taken.
✗ Branch 154 not taken.
✗ Branch 155 not taken.
✗ Branch 158 not taken.
✗ Branch 159 not taken.
✗ Branch 161 not taken.
✗ Branch 162 not taken.
✗ Branch 177 not taken.
✗ Branch 178 not taken.
✗ Branch 181 not taken.
✗ Branch 182 not taken.
✗ Branch 185 not taken.
✗ Branch 186 not taken.
✗ Branch 188 not taken.
✗ Branch 189 not taken.
✗ Branch 194 not taken.
✗ Branch 195 not taken.
✗ Branch 209 not taken.
✗ Branch 210 not taken.
 5 {"TargetSpeed", 1, double(0)}}
351 1 {}
352
353 1 void updateInputs() override { device.updateInputs(); }
354 1 void updateOutputs() override { device.updateOutputs(); }
355
356 5 const std::vector<ChannelKind> &inputKinds() const override
357 {
358 5 return inputKinds_;
359 }
360 5 const std::vector<ChannelKind> &outputKinds() const override
361 {
362 5 return outputKinds_;
363 }
364
365 protected:
366 3 ChannelValue getInputAt(size_t i) const override
367 {
368
3/24
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 3 switch (i) {
369 1 case 0:
370 1 return device.getStatusWord();
371 ✗ case 1:
372 ✗ return device.getCurrentSpeed();
373 ✗ case 2:
374 ✗ return device.getStatus0Enabled();
375 ✗ case 3:
376 ✗ return device.getStatus1Ready();
377 ✗ case 4:
378 ✗ return device.getStatus2PaDataEnabled();
379 ✗ case 5:
380 ✗ return device.getStatus5FaultWarning();
381 1 case 6:
382 1 return device.getFaultFlag();
383 ✗ case 7:
384 ✗ return device.getWarning();
385 ✗ case 8:
386 ✗ return uint16_t(device.getDeviceState());
387 ✗ case 9:
388 ✗ return uint16_t(device.getErrorNumber());
389 1 case 10:
390 1 return device.getFault();
391 ✗ default:
392 ✗ throw UnknownChannel("Movitrac: invalid channel index.");
393 }
394 }
395
396 2 void setOutputAt(size_t i, const ChannelValue &value) override
397 {
398
2/6
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 switch (i) {
399 1 case 0:
400 1 device.setEnableMotor(std::get<bool>(value));
401 1 return;
402 1 case 1:
403 1 device.setTargetSpeed(std::get<double>(value));
404 1 return;
405 ✗ default:
406 ✗ throw UnknownChannel("Movitrac: invalid channel index.");
407 }
408 }
409
410 private:
411 Movitrac<mode> device;
412 std::vector<ChannelKind> inputKinds_;
413 std::vector<ChannelKind> outputKinds_;
414 };
415
416 /****************************************************************************/
417
418 namespace {
419
420 8 DeviceFactoryFn makeMovitracFactory()
421 {
422 1 return [](const SlaveContext &ctx) -> unique_ptr<SubDevice> {
423
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (ctx.mode == Mode::Control) {
424 ✗ return make_unique<MovitracAdapter<Mode::Control>>(ctx);
425 }
426 else {
427 1 return make_unique<MovitracAdapter<Mode::Simulation>>(ctx);
428 }
429
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 };
430 }
431
432 struct MovitracRegistration
433 {
434 8 MovitracRegistration()
435 {
436
4/8
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
 24 Registry::instance().add(
437 {"Movitrac", __FILE__, 0x00000059, 0x00000004, AnyRevision,
438
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 wrapFactory(makeMovitracFactory())});
439 8 }
440 8 } movitracRegistration;
441
442 } // namespace
443
444 24 } // namespace EcApp
445
446 /****************************************************************************/
447