iCub-main
Loading...
Searching...
No Matches
ethResource.cpp
Go to the documentation of this file.
1// -*- Mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3/*
4 * Copyright (C) 2012 iCub Facility, Istituto Italiano di Tecnologia
5 * Author: Alberto Cardellino, Marco Accame
6 * email: alberto.cardellino@iit.it, marco.accame@iit.it
7 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
8 *
9 */
10
11#include "ethResource.h"
12#include <ethManager.h>
13#include <yarp/os/Time.h>
14#include <yarp/os/Network.h>
15#include <yarp/conf/environment.h>
16#include <yarp/os/NetType.h>
17
18// embobj
19#include "EOropframe_hid.h"
20#include "EOarray.h"
21#include "EoProtocol.h"
22#include "EoManagement.h"
23#include "EoProtocolMN.h"
24#include "can_string_eth.h"
25
26using namespace yarp::os;
27using namespace yarp::os::impl;
28
29#include <theNVmanager.h>
30#include "ethParser.h"
31using namespace eth;
32
33
34
35
36
37// - class EthResource
38
40{
41 ethManager = NULL;
42 isInRunningMode = false;
43
44 verifiedBoardPresence = false;
45 askedBoardVersion = false;
46 verifiedBoardTransceiver = false;
47 txrateISset = false;
48 cleanedBoardBehaviour = false;
49
50 boardMNprotocolversion.major = boardMNprotocolversion.minor;
51
52 txconfig.runnermode = eomn_appl_runnermode_synchronized;
53 txconfig.safetygap = 0;
54 txconfig.cycletime = defcycletime;
55 txconfig.txratedivider = defTXrateOfRegularROPs;
56 txconfig.maxtimeRX = defmaxtimeRX;
57 txconfig.maxtimeDO = defmaxtimeDO;
58 txconfig.maxtimeTX = defmaxtimeTX;
59 txconfig.logging.flags = (0x0001 << eomn_appl_log_asynchro_exectime_rxdotx_overflow);
60 txconfig.logging.period10ms = 0;
61
62 memset(verifiedEPprotocol, 0, sizeof(verifiedEPprotocol));
63
64 usedNumberOfRegularROPs = 0;
65
66 for(int i = 0; i<16; i++)
67 {
68 c_string_handler[i] = NULL;
69 }
70
71 std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
72 if (tmp != "")
73 {
74 verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
75 }
76 else
77 {
78 verbosewhenok = false;
79 }
80
81 verbosewhenok = true;
82
83 regularsAreSet = false;
84}
85
86
88{
89 ethManager = NULL;
90
91 // Delete every initialized can_string_eth object
92 for(int i=0; i<16; i++)
93 {
94 if (c_string_handler[i] != NULL)
95 {
96 delete c_string_handler[i];
97 c_string_handler[i] = NULL;
98 }
99 }
100}
101
102bool EthResource::lock(bool on)
103{
104 if(true == on)
105 objLock.lock();
106 else
107 objLock.unlock();
108
109 return true;
110}
111
112
113bool EthResource::open2(eOipv4addr_t remIP, yarp::os::Searchable &cfgtotal)
114{
115 ethManager = eth::TheEthManager::instance();
116
117 eth::parser::pc104Data pc104data;
118 eth::parser::read(cfgtotal, pc104data);
119// eth::parser::print(pc104data);
120
121
123 eth::parser::read(cfgtotal, brddata);
124 eth::parser::print(brddata);
125
126 txconfig = brddata.settings.txconfig;
127
128 properties.ipv4addr = remIP;
129 properties.ipv4addressing = brddata.properties.ipv4addressing;
130 properties.boardtype = brddata.properties.type;
131 properties.ipv4addrString = brddata.properties.ipv4string;
132 properties.ipv4addressingString = brddata.properties.ipv4addressingstring;
133 properties.boardtypeString = brddata.properties.typestring;
134 properties.boardnameString = brddata.settings.name;
135 properties.txROPratedivider = txconfig.txratedivider;
136
137
139
140 // default values ...
141 mpConfig.enabled = brddata.actions.monitorpresence_enabled;
142 mpConfig.timeout = brddata.actions.monitorpresence_timeout;
144 mpConfig.name = brddata.properties.ipv4string + " (" + brddata.settings.name + ")";
145
146
147
148 // now i init objects
149
150 lock(true);
151
152 // 1. init transceiver
153
154 eOipv4addressing_t localIPv4 = ethManager->getLocalIPV4addressing();
155
156
157 if(false == transceiver.init2(this, cfgtotal, localIPv4, remIP))
158 {
159 yError() << "EthResource::open2() cannot init transceiver w/ HostTransceiver::init2() for BOARD" << properties.boardnameString << "IP" << properties.ipv4addrString;
160 lock(false);
161 return false;
162 }
163
164 // 2. init monitor presence
165
166 monitorpresence.config(mpConfig);
167 monitorpresence.tick();
168
169
170 lock(false);
171
172 return true;
173}
174
175
176
178{
179 yTrace();
180 posServiceConfiguration.clearOnStop(eomn_serv_category_all);
181 return false;
182}
183
184
185const void * EthResource::getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)
186{
187 destination = properties.ipv4addressing;
188 const void * udp = transceiver.getUDP(sizeofpacket, numofrops);
189
190// const EOropframeHeader_t * header = reinterpret_cast<const EOropframeHeader_t *>(udp);
191
192// if(nullptr != header)
193// {
194// yDebug() << "EthResource::getUDPtransmit(): the header has: sequencenumber = " << header->sequencenumber << "numofrops = " << header->ropsnumberof << "sizeofrops = " << header->ropssizeof;
195// }
196// else
197// {
198
199// }
200
201 return udp;
202}
203
204
206{
207 monitorpresence.tick();
208 return true;
209}
210
211
213{
214 if(false == regularsAreSet)
215 { // we dont comply if the regulars are not set because ... poor board: it does not regularly transmit
216 return true;
217 }
218
219 return monitorpresence.check();
220}
221
222
223
224bool EthResource::processRXpacket(const void *data, const size_t size)
225{
226 return transceiver.parseUDP(data, size);
227}
228
229
231{
232 return properties;
233}
234
235
236
237//bool EthResource::isID32supported(eOprotID32_t id32)
238//{
239// return transceiver.isID32supported(id32);
240//}
241
242
243
244bool EthResource::isRunning(void)
245{
246 return(isInRunningMode);
247}
248
249
250
251bool EthResource::verifyBoardTransceiver()
252{
253 // the transceiver is verified if we have the same mn protocol version inside eOmn_comm_status_t::managementprotocolversion
254
255 if(verifiedBoardTransceiver)
256 {
257 return(true);
258 }
259
260 // we dont ask anything to the board ...
261#define DONT_ASK_COMM_STATUS
262
263#if defined(DONT_ASK_COMM_STATUS)
264
265 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
266 const eoprot_version_t * brdversionMN = &boardMNprotocolversion;
267
268#else
269
270 theNVmanager& nvman = theNVmanager::getInstance();
271
272
273 // step 1: we ask the remote board the eoprot_tag_mn_comm_status variable and then we verify vs mn protocol version
274
275 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
276 const double timeout = 0.100; // now the timeout can be reduced because the board is already connected.
277
278 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_status);
279 eOmn_comm_status_t brdstatus = {0};
280 uint16_t size = 0;
281
282 bool rr = nvman.ask(ipv4addr, id32, &brdstatus, timeout);
283
284 if(false == rr)
285 {
286 yError() << "EthResource::verifyBoardTransceiver() cannot read brdstatus w/ theNVmanager for BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
287 return(false);
288 }
289
290 eoprot_version_t * brdversionMN = (eoprot_version_t*)&brdstatus.managementprotocolversion;
291
292#endif
293
294 if(pc104versionMN->major != brdversionMN->major)
295 {
296 yError() << "EthResource::verifyBoardTransceiver() detected different mn protocol major versions: local =" << pc104versionMN->major << ", remote =" << brdversionMN->major << ": cannot proceed any further";
297 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
298 return(false);
299 }
300
301 if(pc104versionMN->minor != brdversionMN->minor)
302 {
303 yError() << "EthResource::verifyBoardTransceiver() detected different mn protocol minor versions: local =" << pc104versionMN->minor << ", remote =" << brdversionMN->minor << ": cannot proceed any further.";
304 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
305 return(false);
306 }
307
308
309 if(verbosewhenok)
310 {
311 yDebug() << "EthResource::verifyBoardTransceiver() has validated the transceiver of BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString;
312 }
313
314 verifiedBoardTransceiver = true;
315
316 return(true);
317}
318
319
320
321bool EthResource::setTimingOfRunningCycle()
322{
323
324 if(txrateISset)
325 {
326 return(true);
327 }
328
329 // step 1: we send the remote board a message of type eoprot_tag_mn_appl_config with the value read from the proper section
330 // if does find the section we use default values
331
332 // call a set until verified
333
334 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config);
335
336 theNVmanager& nvman = theNVmanager::getInstance();
337
338 if(false == nvman.setcheck(properties.ipv4addr, id32, &txconfig, 5, 0.010, 2.0))
339 {
340 yWarning() << "EthResource::setTimingOfRunningCycle() for BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "could not configure: cycletime =" << txconfig.cycletime << "usec, RX DO TX = (" << txconfig.maxtimeRX << txconfig.maxtimeDO << txconfig.maxtimeTX << ") usec and TX rate =" << txconfig.txratedivider << " every cycle";
341 return false;
342 }
343 else
344 {
345 if(verbosewhenok)
346 {
347 yDebug() << "EthResource::setTimingOfRunningCycle() for BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "has succesfully set: cycletime =" << txconfig.cycletime << "usec, RX DO TX = (" << txconfig.maxtimeRX << txconfig.maxtimeDO << txconfig.maxtimeTX << ") usec and TX rate =" << txconfig.txratedivider << " every cycle";
348 }
349 }
350
351 txrateISset = true;
352
353
354 return(true);
355}
356
357
358bool EthResource::cleanBoardBehaviour(void)
359{
360 if(cleanedBoardBehaviour)
361 {
362 return(true);
363 }
364
365 // send a ...
366 if(false == serviceStop(eomn_serv_category_all))
367 {
368 yError() << "EthResource::cleanBoardBehaviour() cannot stop services for BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
369 return(false);
370 }
371
372 regularsAreSet = false;
373
374
375 if(verbosewhenok)
376 {
377 yDebug() << "EthResource::cleanBoardBehaviour() has cleaned the application in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": config mode + cleared all its regulars";
378 }
379
380 cleanedBoardBehaviour = true;
381
382 return(true);
383
384}
385
386bool EthResource::testMultipleASK()
387{
388#if 1
389 return true;
390#else
391
392
393 // i ask multiple values such as:
394 eOprotID32_t id32_commstatus = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_status);
395 eOmn_comm_status_t value_commstatus = {0};
396
397 eOprotID32_t id32_applconfig = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config);
398 eOmn_appl_config_t value_applconfig = {0};
399
400 eOprotID32_t id32_applconfig_txratedivider = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config_txratedivider);
401 uint8_t txratedivider = 0;
402
403 std::vector<eOprotID32_t> id32s;
404 std::vector<void*> values;
405
406 id32s.push_back(id32_commstatus);
407 values.push_back(&value_commstatus);
408
409 id32s.push_back(id32_applconfig);
410 values.push_back(&value_applconfig);
411
412 id32s.push_back(id32_applconfig_txratedivider);
413 values.push_back(&txratedivider);
414
415 id32s.push_back(id32_applconfig_txratedivider);
416 values.push_back(&txratedivider);
417
418 id32s.push_back(id32_applconfig_txratedivider);
419 values.push_back(&txratedivider);
420
421 id32s.push_back(id32_applconfig_txratedivider);
422 values.push_back(&txratedivider);
423
424 id32s.push_back(id32_applconfig_txratedivider);
425 values.push_back(&txratedivider);
426
427 id32s.push_back(id32_applconfig_txratedivider);
428 values.push_back(&txratedivider);
429
430 id32s.push_back(id32_applconfig_txratedivider);
431 values.push_back(&txratedivider);
432
433 id32s.push_back(id32_applconfig_txratedivider);
434 values.push_back(&txratedivider);
435
436 theNVmanager& nvman = theNVmanager::getInstance();
437
438 double tprev = SystemClock::nowSystem();
439 double tcurr = SystemClock::nowSystem();
440
441 double delta = tcurr - tprev;
442
443 yDebug() << "before";
444 yDebug() << "value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major << "value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
445 yDebug() << "value_applconfig.cycletime = " << value_applconfig.cycletime << "value_applconfig.txratedivider" << value_applconfig.txratedivider << "etc";
446 yDebug() << "txratedivider = " << txratedivider;
447
448 tprev = SystemClock::nowSystem();
449 //bool ok = nvman.ask(&transceiver, id32s, values, 3.0);
450 bool ok = getRemoteValues(id32s, values, 3.0);
451 delta = SystemClock::nowSystem() - tprev;
452
453 yDebug() << "parallel mode: after" << delta << "seconds";
454 yDebug() << "value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major << "value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
455 yDebug() << "value_applconfig.cycletime = " << value_applconfig.cycletime << "value_applconfig.txratedivider" << value_applconfig.txratedivider << "etc";
456 yDebug() << "txratedivider = " << txratedivider;
457
458 memset(&value_commstatus, 0, sizeof(value_commstatus));
459 memset(&value_applconfig, 0, sizeof(value_applconfig));
460 txratedivider = 0;
461
462
463 tprev = SystemClock::nowSystem();
464 nvman.ask(&transceiver, id32_commstatus, &value_commstatus, 3.0);
465 nvman.ask(&transceiver, id32_applconfig, &value_applconfig, 3.0);
466 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
467 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
468 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
469 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
470 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
471 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
472 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
473 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
474 delta = SystemClock::nowSystem() - tprev;
475
476 yDebug() << "serial mode: after" << delta << "seconds";
477 yDebug() << "value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major << "value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
478 yDebug() << "value_applconfig.cycletime = " << value_applconfig.cycletime << "value_applconfig.txratedivider" << value_applconfig.txratedivider << "etc";
479 yDebug() << "txratedivider = " << txratedivider;
480
481 for(;;);
482
483
484 return true;
485
486#endif
487}
488
489bool EthResource::verifyEPprotocol(eOprot_endpoint_t ep)
490{
491 if((uint8_t)ep >= eoprot_endpoints_numberof)
492 {
493 yError() << "EthResource::verifyEPprotocol() called with wrong ep = " << ep << ": cannot proceed any further";
494 return(false);
495 }
496
497 if(true == verifiedEPprotocol[ep])
498 {
499 return(true);
500 }
501
502 if(false == verifyBoard())
503 {
504 yError() << "EthResource::verifyEPprotocol() cannot verify BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
505 return(false);
506 }
507
508 if(false == askBoardVersion())
509 {
510 yError() << "EthResource::verifyEPprotocol() cannot ask the version to BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
511 return(false);
512 }
513
514 testMultipleASK();
515
516
517 // 1. send a set<eoprot_tag_mn_comm_cmmnds_command_queryarray> and wait for the arrival of a sig<eoprot_tag_mn_comm_cmmnds_command_replyarray>
518 // the opc to send is eomn_opc_query_array_EPdes which will trigger a opc in reception eomn_opc_reply_array_EPdes
519 // 2. the resulting array will contains a eoprot_endpoint_descriptor_t item for the specifeid ep with the protocol version of the ems.
520
521
522
523 const double timeout = 0.100;
524
525 eOprotID32_t id2send = eo_prot_ID32dummy;
526 eOprotID32_t id2wait = eo_prot_ID32dummy;
527 eOmn_command_t command = {0};
528
529
530 // step 1: ask all the EP descriptors. from them we can extract protocol version of MN and of the target ep
531 id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_queryarray);
532 memset(&command, 0, sizeof(command));
533 command.cmd.opc = eomn_opc_query_array_EPdes;
534 command.cmd.queryarray.opcpar.opc = eomn_opc_query_array_EPdes;
535 command.cmd.queryarray.opcpar.endpoint = eoprot_endpoint_all;
536 command.cmd.queryarray.opcpar.setnumber = 0;
537 command.cmd.queryarray.opcpar.setsize = 0;
538
539 // the semaphore must be retrieved using the id of the variable which is waited. in this case, it is the array of descriptors
540 id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray);
541
543
544 if(false == nvman.command(properties.ipv4addr, id2send, &command, id2wait, &command, timeout))
545 {
546 yError() << "EthResource::verifyEPprotocol() retrieve the endpoint descriptors from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
547 return(false);
548 }
549
550 // the array is ...
551 eOmn_cmd_replyarray_t* cmdreplyarray = (eOmn_cmd_replyarray_t*)&command.cmd.replyarray;
552 EOarray* array = (EOarray*)cmdreplyarray->array;
553
554
555 uint8_t sizeofarray = eo_array_Size(array);
556
557
558 for(int i=0; i<sizeofarray; i++)
559 {
560 eoprot_endpoint_descriptor_t *epd = (eoprot_endpoint_descriptor_t*)eo_array_At(array, i);
561
562 if(epd->endpoint == eoprot_endpoint_management)
563 {
564 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
565 if(pc104versionMN->major != epd->version.major)
566 {
567 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.major =" << pc104versionMN->major << "and board.version.major =" << epd->version.major;
568 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.major in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "for eoprot_endpoint_management: cannot proceed any further.";
569 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
570 return(false);
571 }
572 if(pc104versionMN->minor != epd->version.minor)
573 {
574 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.minor =" << pc104versionMN->minor << "and board.version.minor =" << epd->version.minor;
575 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.minor BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "for eoprot_endpoint_management: cannot proceed any further.";
576 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
577 return false;
578 }
579 }
580 if(epd->endpoint == ep)
581 {
582 const eoprot_version_t * pc104versionEP = eoprot_version_of_endpoint_get(ep);
583 if(pc104versionEP->major != epd->version.major)
584 {
585 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.major =" << pc104versionEP->major << "and board.version.major =" << epd->version.major;
586 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.major in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << " for" << eoprot_EP2string(ep) << ": cannot proceed any further.";
587 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update to offer services for" << eoprot_EP2string(ep);
588 return(false);
589 }
590 if(pc104versionEP->minor != epd->version.minor)
591 {
592 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.minor =" << pc104versionEP->minor << "and board.version.minor =" << epd->version.minor;
593 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.minor in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << " for" << eoprot_EP2string(ep) << ": annot proceed any further";
594 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update to offer services for" << eoprot_EP2string(ep);
595 return(false);
596 }
597 }
598 }
599
600 verifiedEPprotocol[ep] = true;
601
602 return(true);
603
604}
605
606
607
608bool EthResource::verifyBoard(void)
609{
610 if((true == verifyBoardPresence()) &&
611 (true == verifyBoardTransceiver()) &&
612 (true == cleanBoardBehaviour()) &&
613 (true == setTimingOfRunningCycle()) )
614 {
615 return(true);
616 }
617
618 return(false);
619}
620
621
622bool EthResource::verifyBoardPresence(void)
623{
624 if(verifiedBoardPresence)
625 {
626 return(true);
627 }
628
629 const double timeout = 1.00; // 1 sec is more than enough if board is present. if link is not on it is a good time to wait
630 const int retries = 20; // the number of retries depends on the above timeout and on link-up time of the EMS.
631
632 double start_time = yarp::os::Time::now();
633
634 theNVmanager& nvman = theNVmanager::getInstance();
635 verifiedBoardPresence = nvman.ping(properties.ipv4addr, boardMNprotocolversion, timeout, retries);
636
637 double end_time = yarp::os::Time::now();
638
639 if(true == verifiedBoardPresence)
640 {
641 verifiedBoardPresence = true;
642 if(verbosewhenok)
643 {
644 yDebug() << "EthResource::verifyBoardPresence() found BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "after" << end_time-start_time << "seconds";
645 }
646 }
647 else
648 {
649 yError() << "EthResource::verifyBoardPresence() DID NOT have replies from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "after" << end_time-start_time << "seconds: CANNOT PROCEED ANY FURTHER";
650 }
651
652 return(verifiedBoardPresence);
653}
654
655
656bool EthResource::askBoardVersion(void)
657{
658
659 if(askedBoardVersion)
660 {
661 return(true);
662 }
663
664 const double timeout = 0.500; // 500 ms is more than enough if board is present. if link is not on it is a good time to wait
665
666 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status);
667 eOmn_appl_status_t applstatus = {0};
668
669 theNVmanager& nvman = theNVmanager::getInstance();
670
671 askedBoardVersion = nvman.ask(properties.ipv4addr, id32, &applstatus, timeout);
672
673 if(false == askedBoardVersion)
674 {
675 yError() << "EthResource::askBoardVersion() cannot reach BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "w/ timeout of" << timeout << "seconds";
676 return false;
677 }
678
679
680 // now i store the ....
681
682 properties.firmwareversion.major = applstatus.version.major;
683 properties.firmwareversion.minor = applstatus.version.minor;
684
685
686 properties.firmwaredate.year = applstatus.buildate.year;
687 properties.firmwaredate.month = applstatus.buildate.month;
688 properties.firmwaredate.day = applstatus.buildate.day;
689 properties.firmwaredate.hour = applstatus.buildate.hour;
690 properties.firmwaredate.min = applstatus.buildate.min;
691
692 char versstr[32] = {0};
693 snprintf(versstr, sizeof(versstr), "ver %d.%d built on ", properties.firmwareversion.major, properties.firmwareversion.minor);
694 char datestr[32] = {0};
695 eo_common_date_to_string(properties.firmwaredate, datestr, sizeof(datestr));
696
697 properties.firmwareString = string(versstr) + string(datestr);
698
699
700 if(eobool_true == eoboards_is_eth((eObrd_type_t)applstatus.boardtype))
701 {
702 detectedBoardType = (eObrd_ethtype_t) applstatus.boardtype;
703 }
704 else
705 {
706 detectedBoardType = eobrd_ethtype_unknown;
707 }
708
709 if(detectedBoardType != properties.boardtype)
710 {
711 yWarning() << "EthResource::askBoardVersion(): detected wrong board. expecting" << properties.boardtypeString << "and detected" << eoboards_type2string2(eoboards_ethtype2type(detectedBoardType), eobool_true);
712 }
713
714
715 yInfo() << "EthResource::askBoardVersion() found BOARD" << properties.boardnameString << "@ IP" << properties.ipv4addrString << "of type" << properties.boardtypeString<< "with FW =" << properties.firmwareString;
716
717
718 return(askedBoardVersion);
719}
720
721
722bool EthResource::getRemoteValue(const eOprotID32_t id32, void *value, const double timeout, const unsigned int retries)
723{
724 bool replied = false;
725
726 double start_time = yarp::os::Time::now();
727
729
730 for(unsigned int numOfattempts=0; numOfattempts<(retries+1); numOfattempts++)
731 {
732 if(true == nvman.ask(&transceiver, id32, value, timeout))
733 {
734 replied = true;
735 // stop attempts
736 break;
737 }
738
739 if(!replied)
740 {
741 yWarning() << "EthResource::getRemoteValue() cannot have a reply from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "at attempt #" << numOfattempts+1 << "w/ timeout of" << timeout << "seconds";
742 }
743
744 }
745
746 double end_time = yarp::os::Time::now();
747
748 if(false == replied)
749 {
750 yError() << " FATAL: EthResource::getRemoteValue() DID NOT have replies from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << " even after" << end_time-start_time << "seconds: CANNOT PROCEED ANY FURTHER";
751 }
752
753 return replied;
754}
755
756
757bool EthResource::getRemoteValues(const std::vector<eOprotID32_t> &id32s, const std::vector<void*> &values, const double timeout)
758{
760
761 double start_time = yarp::os::Time::now();
762
763 bool replied = nvman.ask(&transceiver, id32s, values, timeout);
764
765 double end_time = yarp::os::Time::now();
766
767 if(false == replied)
768 {
769 yError() << " FATAL: EthResource::getRemoteValues() DID NOT have replies from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << " even after" << end_time-start_time << "seconds: CANNOT PROCEED ANY FURTHER";
770 }
771
772 return replied;
773}
774
775
776bool EthResource::setRemoteValue(const eOprotID32_t id32, void *value)
777{
779 return nvman.set(properties.ipv4addr, id32, value);
780}
781
782bool EthResource::setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries, const double waitbeforecheck, const double timeout)
783{
785 return nvman.setcheck(properties.ipv4addr, id32, value, retries, waitbeforecheck, timeout);
786}
787
788bool EthResource::CANPrintHandler(eOmn_info_basic_t *infobasic)
789{
790 char str[256];
791 char canfullmessage[128];
792
793 static const char * sourcestrings[] =
794 {
795 "LOCAL",
796 "CAN1",
797 "CAN2",
798 "UNKNOWN"
799 };
800 int source = EOMN_INFO_PROPERTIES_FLAGS_get_source(infobasic->properties.flags);
801 const char * str_source = (source > eomn_info_source_can2) ? (sourcestrings[3]) : (sourcestrings[source]);
802 uint16_t address = EOMN_INFO_PROPERTIES_FLAGS_get_address(infobasic->properties.flags);
803 uint8_t *p64 = (uint8_t*)&(infobasic->properties.par64);
804
805 int msg_id = (p64[1]&0xF0) >> 4;
806
807 uint32_t sec = infobasic->timestamp / 1000000;
808 uint32_t msec = (infobasic->timestamp % 1000000) / 1000;
809 uint32_t usec = infobasic->timestamp % 1000;
810
811 const char *boardstr = properties.boardnameString.c_str();
812
813 // Validity check
814 if(address > 15)
815 {
816 snprintf(canfullmessage, sizeof(canfullmessage), "Error while parsing the message: CAN address detected is out of allowed range");
817 snprintf(str, sizeof(str), "from BOARD %s (%s), src %s, adr %d, time %ds %dm %du: CAN PRINT MESSAGE[id %d] -> %s",
818 properties.ipv4addrString.c_str(),
819 boardstr,
820 str_source,
821 address,
822 sec,
823 msec,
824 usec,
825 msg_id,
826 canfullmessage
827 );
828 feat_PrintError(str);
829 }
830 else
831 {
832 // Initialization needed?
833 if (c_string_handler[address] == NULL)
834 c_string_handler[address] = new can_string_eth();
835
836 CanFrame can_msg;
837 can_msg.setCanData(infobasic->properties.par64);
838 can_msg.setId(msg_id);
839 can_msg.setSize(infobasic->properties.par16);
840 int ret = c_string_handler[address]->add_string(&can_msg);
841
842 // String finished?
843 if (ret != -1)
844 {
845 char* themsg = c_string_handler[address]->get_string(ret);
846 memcpy(canfullmessage, themsg, sizeof(canfullmessage));
847 canfullmessage[63] = 0;
848 c_string_handler[address]->clear_string(ret);
849
850 snprintf(str,sizeof(str), "from BOARD %s (%s), src %s, adr %d, time %ds %dm %du: CAN PRINT MESSAGE[id %d] -> %s",
851 properties.ipv4addrString.c_str(),
852 boardstr,
853 str_source,
854 address,
855 sec,
856 msec,
857 usec,
858 msg_id,
859 canfullmessage
860 );
861 feat_PrintInfo(str);
862 }
863 }
864 return true;
865}
866
867
868bool EthResource::serviceCommand(eOmn_serv_operation_t operation, eOmn_serv_category_t category, const eOmn_serv_parameter_t* param, double timeout, int times)
869{
870 eOprotID32_t id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_cmmnds_command);
871 eOprotID32_t id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_status_commandresult);
872
873 eOmn_service_cmmnds_command_t command = {0};
874 eOmn_service_command_result_t result = {0};
875
876 command.operation = operation;
877 command.category = category;
878 if(NULL != param)
879 {
880 memcpy(&command.parameter, param, sizeof(eOmn_serv_parameter_t));
881 }
882 else
883 {
884 memset(&command.parameter, 0, sizeof(eOmn_serv_parameter_t));
885 if((eomn_serv_operation_regsig_load == operation) || ((eomn_serv_operation_regsig_clear == operation)))
886 { // we send an empty array
887 eo_array_New(eOmn_serv_capacity_arrayof_id32, 4, &command.parameter.arrayofid32);
888 }
889 else
890 {
891 command.parameter.configuration.type = eomn_serv_NONE;
892 }
893 }
894
895 theNVmanager& nvman = theNVmanager::getInstance();
896
897
898 bool replied = false;
899 for(int i=0; i<times; i++)
900 {
901 if(true == nvman.command(properties.ipv4addr, id2send, &command, id2wait, &result, timeout))
902 {
903 replied = true;
904 break;
905 }
906 }
907
908 if(false == replied)
909 {
910 yError() << "EthResource::serviceCommand() failed an acked activation request to BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "after" << times << "attempts" << "each with waiting timeout of" << timeout << "seconds";
911 return false;
912 }
913
914 //yDebug() << "result is:" << result.latestcommandisok;
915
916 return(result.latestcommandisok);
917}
918
919
920bool EthResource::serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t* param, double timeout)
921{
922 eOmn_serv_parameter_t prepared {};
923 if(!posServiceConfiguration.prepare(category, param, prepared))
924 {
925 yError() << "EthResource::serviceVerifyActivate(): MC requires"
926 << "standalone POS to be activated first on BOARD" << getProperties().boardnameString;
927 return false;
928 }
929 const auto* request = param ? &prepared : nullptr;
930 const bool result = serviceCommand(eomn_serv_operation_verifyactivate, category, request, timeout, 3);
931 if(result)
932 {
933 posServiceConfiguration.rememberActivated(category, request);
934 }
935 return result;
936}
937
938
939bool EthResource::serviceSetRegulars(eOmn_serv_category_t category, vector<eOprotID32_t> &id32vector, double timeout)
940{
941 eOmn_serv_parameter_t param = {0};
942 EOarray *array = eo_array_New(eOmn_serv_capacity_arrayof_id32, 4, &param.arrayofid32);
943 for(int i=0; i<id32vector.size(); i++)
944 {
945 eOprotID32_t id32 = id32vector.at(i);
946 eo_array_PushBack(array, &id32);
947 }
948
949 regularsAreSet = serviceCommand(eomn_serv_operation_regsig_load, category, &param, timeout, 3);
950
951 return regularsAreSet;
952}
953
954
955
956bool EthResource::serviceStart(eOmn_serv_category_t category, double timeout)
957{
958 bool ret = serviceCommand(eomn_serv_operation_start, category, NULL, timeout, 3);
959
960 if(ret)
961 {
962 isInRunningMode = true;
963 }
964
965 return ret;
966}
967
968
969bool EthResource::serviceStop(eOmn_serv_category_t category, double timeout)
970{
971 bool ret = serviceCommand(eomn_serv_operation_stop, category, NULL, timeout, 3);
972
973 if(ret)
974 {
975 posServiceConfiguration.clearOnStop(category);
976 }
977
978 if(ret && (category == eomn_serv_category_all))
979 {
980 regularsAreSet = false;
981 }
982
983 //#warning TODO: the result for command stop shall also tell if the the board is in running mode or not.
984 return ret;
985}
986
987
988// new methods from host transceiver
989
990bool EthResource::getLocalValue(const eOprotID32_t id32, void *data)
991{
992 return transceiver.read(id32, data);
993}
994
995
996bool EthResource::setLocalValue(eOprotID32_t id32, const void *value, bool overrideROprotection)
997{
998 return transceiver.write(id32, value, overrideROprotection);
999}
1000
1001
1003{
1004 return false;
1005}
1006
1008{
1009 return &transceiver;
1010}
1011
1012// eof
1013
1014
1015
1016
1017
1018
1019
@ data
void setSize(uint8_t size)
void setId(uint16_t id)
void setCanData(uint64_t data)
POSServiceConfiguration posServiceConfiguration
bool getRemoteValues(const std::vector< eOprotID32_t > &id32s, const std::vector< void * > &values, const double timeout=0.500)
const void * getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)
bool processRXpacket(const void *data, size_t size)
HostTransceiver * getTransceiver()
bool getLocalValue(const eOprotID32_t id32, void *value)
bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)
bool open2(eOipv4addr_t remIP, yarp::os::Searchable &cfgtotal)
bool serviceStop(eOmn_serv_category_t category, double timeout=0.500)
bool verifyEPprotocol(eOprot_endpoint_t ep)
bool getRemoteValue(const eOprotID32_t id32, void *value, const double timeout=0.100, const unsigned int retries=0)
bool setLocalValue(eOprotID32_t id32, const void *value, bool overrideROprotection=false)
bool setRemoteValue(const eOprotID32_t id32, void *value)
bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)
bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)
const Properties & getProperties()
bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)
bool CANPrintHandler(eOmn_info_basic_t *infobasic)
static TheEthManager * instance()
static theNVmanager & getInstance()
bool setcheck(const eOprotIP_t ipv4, const eOprotID32_t id32, const void *value, const unsigned int retries=10, double waitbeforecheck=0.001, double timeout=0.5)
bool ask(const eOprotIP_t ipv4, const eOprotID32_t id32, void *value, const double timeout=0.5)
bool command(const eOprotIP_t ipv4, const eOprotID32_t id32cmd, const void *cmd, const eOprotID32_t id32rep, void *rep, double timeout=0.5)
bool ping(const eOprotIP_t ipv4, eoprot_version_t &mnprotversion, const double timeout=0.5, const unsigned int retries=20)
bool set(const eOprotIP_t ipv4, const eOprotID32_t id32, const void *value)
void feat_PrintInfo(char *string)
void feat_PrintError(char *string)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition ethParser.cpp:92
bool print(const pc104Data &pc104data)
Definition ethParser.cpp:57
grid on
double monitorpresence_periodofmissingreport
Definition ethParser.h:111
boardActions actions
Definition ethParser.h:126
boardProperties properties
Definition ethParser.h:124
boardSettings settings
Definition ethParser.h:125
eOipv4addressing_t ipv4addressing
Definition ethParser.h:39
std::string ipv4addressingstring
Definition ethParser.h:43
eOmn_appl_config_t txconfig
Definition ethParser.h:59