Loading [MathJax]/extensions/tex2jax.js
iCub-main
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
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;
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 return false;
181}
182
183
184const void * EthResource::getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)
185{
186 destination = properties.ipv4addressing;
187 const void * udp = transceiver.getUDP(sizeofpacket, numofrops);
188
189// const EOropframeHeader_t * header = reinterpret_cast<const EOropframeHeader_t *>(udp);
190
191// if(nullptr != header)
192// {
193// yDebug() << "EthResource::getUDPtransmit(): the header has: sequencenumber = " << header->sequencenumber << "numofrops = " << header->ropsnumberof << "sizeofrops = " << header->ropssizeof;
194// }
195// else
196// {
197
198// }
199
200 return udp;
201}
202
203
205{
206 monitorpresence.tick();
207 return true;
208}
209
210
212{
213 if(false == regularsAreSet)
214 { // we dont comply if the regulars are not set because ... poor board: it does not regularly transmit
215 return true;
216 }
217
218 return monitorpresence.check();
219}
220
221
222
223bool EthResource::processRXpacket(const void *data, const size_t size)
224{
225 return transceiver.parseUDP(data, size);
226}
227
228
230{
231 return properties;
232}
233
234
235
236//bool EthResource::isID32supported(eOprotID32_t id32)
237//{
238// return transceiver.isID32supported(id32);
239//}
240
241
242
243bool EthResource::isRunning(void)
244{
245 return(isInRunningMode);
246}
247
248
249
250bool EthResource::verifyBoardTransceiver()
251{
252 // the transceiver is verified if we have the same mn protocol version inside eOmn_comm_status_t::managementprotocolversion
253
254 if(verifiedBoardTransceiver)
255 {
256 return(true);
257 }
258
259 // we dont ask anything to the board ...
260#define DONT_ASK_COMM_STATUS
261
262#if defined(DONT_ASK_COMM_STATUS)
263
264 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
265 const eoprot_version_t * brdversionMN = &boardMNprotocolversion;
266
267#else
268
270
271
272 // step 1: we ask the remote board the eoprot_tag_mn_comm_status variable and then we verify vs mn protocol version
273
274 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
275 const double timeout = 0.100; // now the timeout can be reduced because the board is already connected.
276
277 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_status);
278 eOmn_comm_status_t brdstatus = {0};
279 uint16_t size = 0;
280
281 bool rr = nvman.ask(ipv4addr, id32, &brdstatus, timeout);
282
283 if(false == rr)
284 {
285 yError() << "EthResource::verifyBoardTransceiver() cannot read brdstatus w/ theNVmanager for BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
286 return(false);
287 }
288
289 eoprot_version_t * brdversionMN = (eoprot_version_t*)&brdstatus.managementprotocolversion;
290
291#endif
292
293 if(pc104versionMN->major != brdversionMN->major)
294 {
295 yError() << "EthResource::verifyBoardTransceiver() detected different mn protocol major versions: local =" << pc104versionMN->major << ", remote =" << brdversionMN->major << ": cannot proceed any further";
296 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
297 return(false);
298 }
299
300 if(pc104versionMN->minor != brdversionMN->minor)
301 {
302 yError() << "EthResource::verifyBoardTransceiver() detected different mn protocol minor versions: local =" << pc104versionMN->minor << ", remote =" << brdversionMN->minor << ": cannot proceed any further.";
303 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
304 return(false);
305 }
306
307
308 if(verbosewhenok)
309 {
310 yDebug() << "EthResource::verifyBoardTransceiver() has validated the transceiver of BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString;
311 }
312
313 verifiedBoardTransceiver = true;
314
315 return(true);
316}
317
318
319
320bool EthResource::setTimingOfRunningCycle()
321{
322
323 if(txrateISset)
324 {
325 return(true);
326 }
327
328 // step 1: we send the remote board a message of type eoprot_tag_mn_appl_config with the value read from the proper section
329 // if does find the section we use default values
330
331 // call a set until verified
332
333 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config);
334
336
337 if(false == nvman.setcheck(properties.ipv4addr, id32, &txconfig, 5, 0.010, 2.0))
338 {
339 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";
340 return false;
341 }
342 else
343 {
344 if(verbosewhenok)
345 {
346 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";
347 }
348 }
349
350 txrateISset = true;
351
352
353 return(true);
354}
355
356
357bool EthResource::cleanBoardBehaviour(void)
358{
359 if(cleanedBoardBehaviour)
360 {
361 return(true);
362 }
363
364 // send a ...
365 if(false == serviceStop(eomn_serv_category_all))
366 {
367 yError() << "EthResource::cleanBoardBehaviour() cannot stop services for BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
368 return(false);
369 }
370
371 regularsAreSet = false;
372
373
374 if(verbosewhenok)
375 {
376 yDebug() << "EthResource::cleanBoardBehaviour() has cleaned the application in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": config mode + cleared all its regulars";
377 }
378
379 cleanedBoardBehaviour = true;
380
381 return(true);
382
383}
384
385bool EthResource::testMultipleASK()
386{
387#if 1
388 return true;
389#else
390
391
392 // i ask multiple values such as:
393 eOprotID32_t id32_commstatus = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_status);
394 eOmn_comm_status_t value_commstatus = {0};
395
396 eOprotID32_t id32_applconfig = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config);
397 eOmn_appl_config_t value_applconfig = {0};
398
399 eOprotID32_t id32_applconfig_txratedivider = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config_txratedivider);
400 uint8_t txratedivider = 0;
401
402 std::vector<eOprotID32_t> id32s;
403 std::vector<void*> values;
404
405 id32s.push_back(id32_commstatus);
406 values.push_back(&value_commstatus);
407
408 id32s.push_back(id32_applconfig);
409 values.push_back(&value_applconfig);
410
411 id32s.push_back(id32_applconfig_txratedivider);
412 values.push_back(&txratedivider);
413
414 id32s.push_back(id32_applconfig_txratedivider);
415 values.push_back(&txratedivider);
416
417 id32s.push_back(id32_applconfig_txratedivider);
418 values.push_back(&txratedivider);
419
420 id32s.push_back(id32_applconfig_txratedivider);
421 values.push_back(&txratedivider);
422
423 id32s.push_back(id32_applconfig_txratedivider);
424 values.push_back(&txratedivider);
425
426 id32s.push_back(id32_applconfig_txratedivider);
427 values.push_back(&txratedivider);
428
429 id32s.push_back(id32_applconfig_txratedivider);
430 values.push_back(&txratedivider);
431
432 id32s.push_back(id32_applconfig_txratedivider);
433 values.push_back(&txratedivider);
434
436
437 double tprev = SystemClock::nowSystem();
438 double tcurr = SystemClock::nowSystem();
439
440 double delta = tcurr - tprev;
441
442 yDebug() << "before";
443 yDebug() << "value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major << "value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
444 yDebug() << "value_applconfig.cycletime = " << value_applconfig.cycletime << "value_applconfig.txratedivider" << value_applconfig.txratedivider << "etc";
445 yDebug() << "txratedivider = " << txratedivider;
446
447 tprev = SystemClock::nowSystem();
448 //bool ok = nvman.ask(&transceiver, id32s, values, 3.0);
449 bool ok = getRemoteValues(id32s, values, 3.0);
450 delta = SystemClock::nowSystem() - tprev;
451
452 yDebug() << "parallel mode: after" << delta << "seconds";
453 yDebug() << "value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major << "value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
454 yDebug() << "value_applconfig.cycletime = " << value_applconfig.cycletime << "value_applconfig.txratedivider" << value_applconfig.txratedivider << "etc";
455 yDebug() << "txratedivider = " << txratedivider;
456
457 memset(&value_commstatus, 0, sizeof(value_commstatus));
458 memset(&value_applconfig, 0, sizeof(value_applconfig));
459 txratedivider = 0;
460
461
462 tprev = SystemClock::nowSystem();
463 nvman.ask(&transceiver, id32_commstatus, &value_commstatus, 3.0);
464 nvman.ask(&transceiver, id32_applconfig, &value_applconfig, 3.0);
465 nvman.ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 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 delta = SystemClock::nowSystem() - tprev;
474
475 yDebug() << "serial mode: after" << delta << "seconds";
476 yDebug() << "value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major << "value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
477 yDebug() << "value_applconfig.cycletime = " << value_applconfig.cycletime << "value_applconfig.txratedivider" << value_applconfig.txratedivider << "etc";
478 yDebug() << "txratedivider = " << txratedivider;
479
480 for(;;);
481
482
483 return true;
484
485#endif
486}
487
488bool EthResource::verifyEPprotocol(eOprot_endpoint_t ep)
489{
490 if((uint8_t)ep >= eoprot_endpoints_numberof)
491 {
492 yError() << "EthResource::verifyEPprotocol() called with wrong ep = " << ep << ": cannot proceed any further";
493 return(false);
494 }
495
496 if(true == verifiedEPprotocol[ep])
497 {
498 return(true);
499 }
500
501 if(false == verifyBoard())
502 {
503 yError() << "EthResource::verifyEPprotocol() cannot verify BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
504 return(false);
505 }
506
507 if(false == askBoardVersion())
508 {
509 yError() << "EthResource::verifyEPprotocol() cannot ask the version to BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
510 return(false);
511 }
512
513 testMultipleASK();
514
515
516 // 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>
517 // the opc to send is eomn_opc_query_array_EPdes which will trigger a opc in reception eomn_opc_reply_array_EPdes
518 // 2. the resulting array will contains a eoprot_endpoint_descriptor_t item for the specifeid ep with the protocol version of the ems.
519
520
521
522 const double timeout = 0.100;
523
524 eOprotID32_t id2send = eo_prot_ID32dummy;
525 eOprotID32_t id2wait = eo_prot_ID32dummy;
526 eOmn_command_t command = {0};
527
528
529 // step 1: ask all the EP descriptors. from them we can extract protocol version of MN and of the target ep
530 id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_queryarray);
531 memset(&command, 0, sizeof(command));
532 command.cmd.opc = eomn_opc_query_array_EPdes;
533 command.cmd.queryarray.opcpar.opc = eomn_opc_query_array_EPdes;
534 command.cmd.queryarray.opcpar.endpoint = eoprot_endpoint_all;
535 command.cmd.queryarray.opcpar.setnumber = 0;
536 command.cmd.queryarray.opcpar.setsize = 0;
537
538 // the semaphore must be retrieved using the id of the variable which is waited. in this case, it is the array of descriptors
539 id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray);
540
542
543 if(false == nvman.command(properties.ipv4addr, id2send, &command, id2wait, &command, timeout))
544 {
545 yError() << "EthResource::verifyEPprotocol() retrieve the endpoint descriptors from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << ": cannot proceed any further";
546 return(false);
547 }
548
549 // the array is ...
550 eOmn_cmd_replyarray_t* cmdreplyarray = (eOmn_cmd_replyarray_t*)&command.cmd.replyarray;
551 EOarray* array = (EOarray*)cmdreplyarray->array;
552
553
554 uint8_t sizeofarray = eo_array_Size(array);
555
556
557 for(int i=0; i<sizeofarray; i++)
558 {
559 eoprot_endpoint_descriptor_t *epd = (eoprot_endpoint_descriptor_t*)eo_array_At(array, i);
560
561 if(epd->endpoint == eoprot_endpoint_management)
562 {
563 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
564 if(pc104versionMN->major != epd->version.major)
565 {
566 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.major =" << pc104versionMN->major << "and board.version.major =" << epd->version.major;
567 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.major in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "for eoprot_endpoint_management: cannot proceed any further.";
568 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
569 return(false);
570 }
571 if(pc104versionMN->minor != epd->version.minor)
572 {
573 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.minor =" << pc104versionMN->minor << "and board.version.minor =" << epd->version.minor;
574 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.minor BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "for eoprot_endpoint_management: cannot proceed any further.";
575 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update.";
576 return false;
577 }
578 }
579 if(epd->endpoint == ep)
580 {
581 const eoprot_version_t * pc104versionEP = eoprot_version_of_endpoint_get(ep);
582 if(pc104versionEP->major != epd->version.major)
583 {
584 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.major =" << pc104versionEP->major << "and board.version.major =" << epd->version.major;
585 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.major in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << " for" << eoprot_EP2string(ep) << ": cannot proceed any further.";
586 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update to offer services for" << eoprot_EP2string(ep);
587 return(false);
588 }
589 if(pc104versionEP->minor != epd->version.minor)
590 {
591 yError() << "EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) << "detected: pc104.version.minor =" << pc104versionEP->minor << "and board.version.minor =" << epd->version.minor;
592 yError() << "EthResource::verifyEPprotocol() detected mismatching protocol version.minor in BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << " for" << eoprot_EP2string(ep) << ": annot proceed any further";
593 yError() << "ACTION REQUIRED: BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "needs a FW update to offer services for" << eoprot_EP2string(ep);
594 return(false);
595 }
596 }
597 }
598
599 verifiedEPprotocol[ep] = true;
600
601 return(true);
602
603}
604
605
606
607bool EthResource::verifyBoard(void)
608{
609 if((true == verifyBoardPresence()) &&
610 (true == verifyBoardTransceiver()) &&
611 (true == cleanBoardBehaviour()) &&
612 (true == setTimingOfRunningCycle()) )
613 {
614 return(true);
615 }
616
617 return(false);
618}
619
620
621bool EthResource::verifyBoardPresence(void)
622{
623 if(verifiedBoardPresence)
624 {
625 return(true);
626 }
627
628 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
629 const int retries = 20; // the number of retries depends on the above timeout and on link-up time of the EMS.
630
631 double start_time = yarp::os::Time::now();
632
634 verifiedBoardPresence = nvman.ping(properties.ipv4addr, boardMNprotocolversion, timeout, retries);
635
636 double end_time = yarp::os::Time::now();
637
638 if(true == verifiedBoardPresence)
639 {
640 verifiedBoardPresence = true;
641 if(verbosewhenok)
642 {
643 yDebug() << "EthResource::verifyBoardPresence() found BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "after" << end_time-start_time << "seconds";
644 }
645 }
646 else
647 {
648 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";
649 }
650
651 return(verifiedBoardPresence);
652}
653
654
655bool EthResource::askBoardVersion(void)
656{
657
658 if(askedBoardVersion)
659 {
660 return(true);
661 }
662
663 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
664
665 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status);
666 eOmn_appl_status_t applstatus = {0};
667
669
670 askedBoardVersion = nvman.ask(properties.ipv4addr, id32, &applstatus, timeout);
671
672 if(false == askedBoardVersion)
673 {
674 yError() << "EthResource::askBoardVersion() cannot reach BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "w/ timeout of" << timeout << "seconds";
675 return false;
676 }
677
678
679 // now i store the ....
680
681 properties.firmwareversion.major = applstatus.version.major;
682 properties.firmwareversion.minor = applstatus.version.minor;
683
684
685 properties.firmwaredate.year = applstatus.buildate.year;
686 properties.firmwaredate.month = applstatus.buildate.month;
687 properties.firmwaredate.day = applstatus.buildate.day;
688 properties.firmwaredate.hour = applstatus.buildate.hour;
689 properties.firmwaredate.min = applstatus.buildate.min;
690
691 char versstr[32] = {0};
692 snprintf(versstr, sizeof(versstr), "ver %d.%d built on ", properties.firmwareversion.major, properties.firmwareversion.minor);
693 char datestr[32] = {0};
694 eo_common_date_to_string(properties.firmwaredate, datestr, sizeof(datestr));
695
696 properties.firmwareString = string(versstr) + string(datestr);
697
698
699 if(eobool_true == eoboards_is_eth((eObrd_type_t)applstatus.boardtype))
700 {
701 detectedBoardType = (eObrd_ethtype_t) applstatus.boardtype;
702 }
703 else
704 {
705 detectedBoardType = eobrd_ethtype_unknown;
706 }
707
708 if(detectedBoardType != properties.boardtype)
709 {
710 yWarning() << "EthResource::askBoardVersion(): detected wrong board. expecting" << properties.boardtypeString << "and detected" << eoboards_type2string2(eoboards_ethtype2type(detectedBoardType), eobool_true);
711 }
712
713
714 yInfo() << "EthResource::askBoardVersion() found BOARD" << properties.boardnameString << "@ IP" << properties.ipv4addrString << "of type" << properties.boardtypeString<< "with FW =" << properties.firmwareString;
715
716
717 return(askedBoardVersion);
718}
719
720
721bool EthResource::getRemoteValue(const eOprotID32_t id32, void *value, const double timeout, const unsigned int retries)
722{
723 bool replied = false;
724
725 double start_time = yarp::os::Time::now();
726
728
729 for(unsigned int numOfattempts=0; numOfattempts<(retries+1); numOfattempts++)
730 {
731 if(true == nvman.ask(&transceiver, id32, value, timeout))
732 {
733 replied = true;
734 // stop attempts
735 break;
736 }
737
738 if(!replied)
739 {
740 yWarning() << "EthResource::getRemoteValue() cannot have a reply from BOARD" << getProperties().boardnameString << "with IP" << getProperties().ipv4addrString << "at attempt #" << numOfattempts+1 << "w/ timeout of" << timeout << "seconds";
741 }
742
743 }
744
745 double end_time = yarp::os::Time::now();
746
747 if(false == replied)
748 {
749 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";
750 }
751
752 return replied;
753}
754
755
756bool EthResource::getRemoteValues(const std::vector<eOprotID32_t> &id32s, const std::vector<void*> &values, const double timeout)
757{
759
760 double start_time = yarp::os::Time::now();
761
762 bool replied = nvman.ask(&transceiver, id32s, values, timeout);
763
764 double end_time = yarp::os::Time::now();
765
766 if(false == replied)
767 {
768 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";
769 }
770
771 return replied;
772}
773
774
775bool EthResource::setRemoteValue(const eOprotID32_t id32, void *value)
776{
778 return nvman.set(properties.ipv4addr, id32, value);
779}
780
781bool EthResource::setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries, const double waitbeforecheck, const double timeout)
782{
784 return nvman.setcheck(properties.ipv4addr, id32, value, retries, waitbeforecheck, timeout);
785}
786
787bool EthResource::CANPrintHandler(eOmn_info_basic_t *infobasic)
788{
789 char str[256];
790 char canfullmessage[128];
791
792 static const char * sourcestrings[] =
793 {
794 "LOCAL",
795 "CAN1",
796 "CAN2",
797 "UNKNOWN"
798 };
799 int source = EOMN_INFO_PROPERTIES_FLAGS_get_source(infobasic->properties.flags);
800 const char * str_source = (source > eomn_info_source_can2) ? (sourcestrings[3]) : (sourcestrings[source]);
801 uint16_t address = EOMN_INFO_PROPERTIES_FLAGS_get_address(infobasic->properties.flags);
802 uint8_t *p64 = (uint8_t*)&(infobasic->properties.par64);
803
804 int msg_id = (p64[1]&0xF0) >> 4;
805
806 uint32_t sec = infobasic->timestamp / 1000000;
807 uint32_t msec = (infobasic->timestamp % 1000000) / 1000;
808 uint32_t usec = infobasic->timestamp % 1000;
809
810 const char *boardstr = properties.boardnameString.c_str();
811
812 // Validity check
813 if(address > 15)
814 {
815 snprintf(canfullmessage, sizeof(canfullmessage), "Error while parsing the message: CAN address detected is out of allowed range");
816 snprintf(str, sizeof(str), "from BOARD %s (%s), src %s, adr %d, time %ds %dm %du: CAN PRINT MESSAGE[id %d] -> %s",
817 properties.ipv4addrString.c_str(),
818 boardstr,
819 str_source,
820 address,
821 sec,
822 msec,
823 usec,
824 msg_id,
825 canfullmessage
826 );
827 feat_PrintError(str);
828 }
829 else
830 {
831 // Initialization needed?
832 if (c_string_handler[address] == NULL)
833 c_string_handler[address] = new can_string_eth();
834
835 CanFrame can_msg;
836 can_msg.setCanData(infobasic->properties.par64);
837 can_msg.setId(msg_id);
838 can_msg.setSize(infobasic->properties.par16);
839 int ret = c_string_handler[address]->add_string(&can_msg);
840
841 // String finished?
842 if (ret != -1)
843 {
844 char* themsg = c_string_handler[address]->get_string(ret);
845 memcpy(canfullmessage, themsg, sizeof(canfullmessage));
846 canfullmessage[63] = 0;
847 c_string_handler[address]->clear_string(ret);
848
849 snprintf(str,sizeof(str), "from BOARD %s (%s), src %s, adr %d, time %ds %dm %du: CAN PRINT MESSAGE[id %d] -> %s",
850 properties.ipv4addrString.c_str(),
851 boardstr,
852 str_source,
853 address,
854 sec,
855 msec,
856 usec,
857 msg_id,
858 canfullmessage
859 );
860 feat_PrintInfo(str);
861 }
862 }
863 return true;
864}
865
866
867bool EthResource::serviceCommand(eOmn_serv_operation_t operation, eOmn_serv_category_t category, const eOmn_serv_parameter_t* param, double timeout, int times)
868{
869 eOprotID32_t id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_cmmnds_command);
870 eOprotID32_t id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_status_commandresult);
871
872 eOmn_service_cmmnds_command_t command = {0};
873 eOmn_service_command_result_t result = {0};
874
875 command.operation = operation;
876 command.category = category;
877 if(NULL != param)
878 {
879 memcpy(&command.parameter, param, sizeof(eOmn_serv_parameter_t));
880 }
881 else
882 {
883 memset(&command.parameter, 0, sizeof(eOmn_serv_parameter_t));
884 if((eomn_serv_operation_regsig_load == operation) || ((eomn_serv_operation_regsig_clear == operation)))
885 { // we send an empty array
886 eo_array_New(eOmn_serv_capacity_arrayof_id32, 4, &command.parameter.arrayofid32);
887 }
888 else
889 {
890 command.parameter.configuration.type = eomn_serv_NONE;
891 }
892 }
893
895
896
897 bool replied = false;
898 for(int i=0; i<times; i++)
899 {
900 if(true == nvman.command(properties.ipv4addr, id2send, &command, id2wait, &result, timeout))
901 {
902 replied = true;
903 break;
904 }
905 }
906
907 if(false == replied)
908 {
909 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";
910 return false;
911 }
912
913 //yDebug() << "result is:" << result.latestcommandisok;
914
915 return(result.latestcommandisok);
916}
917
918
919bool EthResource::serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t* param, double timeout)
920{
921 return(serviceCommand(eomn_serv_operation_verifyactivate, category, param, timeout, 3));
922}
923
924
925bool EthResource::serviceSetRegulars(eOmn_serv_category_t category, vector<eOprotID32_t> &id32vector, double timeout)
926{
927 eOmn_serv_parameter_t param = {0};
928 EOarray *array = eo_array_New(eOmn_serv_capacity_arrayof_id32, 4, &param.arrayofid32);
929 for(int i=0; i<id32vector.size(); i++)
930 {
931 eOprotID32_t id32 = id32vector.at(i);
932 eo_array_PushBack(array, &id32);
933 }
934
935 regularsAreSet = serviceCommand(eomn_serv_operation_regsig_load, category, &param, timeout, 3);
936
937 return regularsAreSet;
938}
939
940
941
942bool EthResource::serviceStart(eOmn_serv_category_t category, double timeout)
943{
944 bool ret = serviceCommand(eomn_serv_operation_start, category, NULL, timeout, 3);
945
946 if(ret)
947 {
948 isInRunningMode = true;
949 }
950
951 return ret;
952}
953
954
955bool EthResource::serviceStop(eOmn_serv_category_t category, double timeout)
956{
957 bool ret = serviceCommand(eomn_serv_operation_stop, category, NULL, timeout, 3);
958
959 if(ret && (category == eomn_serv_category_all))
960 {
961 regularsAreSet = false;
962 }
963
964 //#warning TODO: the result for command stop shall also tell if the the board is in running mode or not.
965 return ret;
966}
967
968
969// new methods from host transceiver
970
971bool EthResource::getLocalValue(const eOprotID32_t id32, void *data)
972{
973 return transceiver.read(id32, data);
974}
975
976
977bool EthResource::setLocalValue(eOprotID32_t id32, const void *value, bool overrideROprotection)
978{
979 return transceiver.write(id32, value, overrideROprotection);
980}
981
982
984{
985 return false;
986}
987
989{
990 return &transceiver;
991}
992
993// eof
994
995
996
997
998
999
1000
@ data
void setSize(uint8_t size)
void setId(uint16_t id)
void setCanData(uint64_t data)
char * get_string(int buffer_num)
int add_string(CanFrame *can_packet)
Process a string can packet.
void clear_string(int buffer_num)
Resets the string buffer.
void config(const Config &cfg)
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()
const eOipv4addressing_t & getLocalIPV4addressing(void)
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)
bool write(const eOprotID32_t id32, const void *data, bool forcewriteOfReadOnly)
bool init2(AbstractEthResource *owner, yarp::os::Searchable &cfgtotal, eOipv4addressing_t &localIPaddressing, eOipv4addr_t remoteIP, uint16_t rxpktsize=maxSizeOfRXpacket)
const void * getUDP(size_t &size, uint16_t &numofrops)
bool parseUDP(const void *data, const uint16_t size)
bool read(const eOprotID32_t id32, void *data)
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