iCub-main
Loading...
Searching...
No Matches
embObjSkin.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 * Authors: Alberto Cardellino
6 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
7 *
8 */
9
10// system includes
11#include <iostream>
12#include <string.h>
13
14// Ace & Yarp includes
15#include <yarp/os/Time.h>
16
17#include <embObjSkin.h>
18
19#include "EoProtocol.h"
20#include "EoProtocolMN.h"
21#include "EoProtocolSK.h"
22
23#include "EoSkin.h"
24#include "iCubCanProtocol.h"
25
26
27#include <ethResource.h>
28#include "../embObjLib/hostTransceiver.hpp"
29
30#include "EOnv.h"
31
32#include <yarp/os/NetType.h>
33#include <yarp/conf/environment.h>
34
35#include "EoCommon.h"
36
37using namespace std;
38using namespace iCub::skin::diagnostics;
39
40bool isCANaddressValid(int adr)
41{
42 return ((adr>0) && (adr<15));
43}
44
45
47{
48 for (size_t i = 0; i< cardAddrList.size(); i++)
49 {
50 if(cardAddrList[i] == cardAddr)
51 return i;
52 }
53 return -1;
54}
55
56EmbObjSkin::EmbObjSkin() : _isDiagnosticPresent(false)
57{
58 res = NULL;
59 ethManager = NULL;
60 opened = false;
61 sensorsNum = 0;
64
65 std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
66 if (tmp != "")
67 {
68 verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
69 }
70 else
71 {
72 verbosewhenok = false;
73 }
74
75 parser = NULL;
76
77 memset(&ethservice.configuration, 0, sizeof(ethservice.configuration));
78 ethservice.configuration.type = eomn_serv_NONE;
79}
80
81
83{
84 if(NULL != parser)
85 {
86 delete parser;
87 parser = NULL;
88 }
89}
90
91
92bool EmbObjSkin::initWithSpecialConfig(yarp::os::Searchable& config)
93{
94 Bottle bNumOfset;
95 unsigned int numOfSets;
96 eOprotID32_t protoid;
97 unsigned int p, j;
98 SpecialSkinBoardCfgParam* boardCfgList = new SpecialSkinBoardCfgParam[_skCfg.totalCardsNum]; //please add cleanup!
99 unsigned int numofcfg;
100
101 if(!_newCfg)
102 {
103 return true; //if we use old style config then return
104 }
105
106 //-----------------------------------------------------------------------------------------------------
107 //------------ read special cfg board --------------------------------------------------------------
108
109 numofcfg = _skCfg.totalCardsNum;//set size of my vector boardCfgList;
110 //in output the function return number of special board cfg are in file xml
111 bool ret = _cfgReader.readSpecialBoardCfg(config, boardCfgList, &numofcfg);
112
113 if(!ret)
114 return false;
115
116
117 for(j=0; j<numofcfg; j++) //for each special board config
118 {
119 //check if patch exist
120 for(p=0; p< _skCfg.patchInfoList.size(); p++)
121 {
122 if(_skCfg.patchInfoList[p].idPatch == boardCfgList[j].patch)
123 break;
124 }
125 if(p>=_skCfg.patchInfoList.size())
126 {
127 yError() << "EmbObjSkin::initWithSpecialConfig(): in skin of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": patch " << boardCfgList[j].patch << "not exists";
128 return false;
129 }
130 //now p is the index of patch.
131
132 eOcanport_t canport = _skCfg.patchInfoList[p].canport;
133
134 //check if card address are in patch
135 int boardIdx = -1;
136 for(int a=boardCfgList[j].boardAddrStart; a<=boardCfgList[j].boardAddrEnd; a++)
137 {
138 boardIdx = _skCfg.patchInfoList[p].checkCardAddrIsInList(a);
139 if(-1 == boardIdx)
140 {
141 yError() << "EmbObjSkin::initWithSpecialConfig(): in skin of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " card with address " << a << "is not present in patch " << _skCfg.patchInfoList[p].idPatch;
142 return(false);
143 }
144 }
145
146 // prepare data to send to ems
147 eOsk_cmd_boardsCfg_t bcfg;
148 // this message is for some addresses only
149 bcfg.candestination[0] = bcfg.candestination[1] = 0;
150 for(int adr=boardCfgList[j].boardAddrStart; adr<=boardCfgList[j].boardAddrEnd; adr++)
151 {
152 if(isCANaddressValid(adr))
153 {
154 eo_common_hlfword_bitset(&bcfg.candestination[canport], adr);
155 }
156 }
157
158 bcfg.cfg.skintype = boardCfgList[j].cfg.skinType;
159 bcfg.cfg.period = boardCfgList[j].cfg.period;
160 bcfg.cfg.noload = boardCfgList[j].cfg.noLoad;
161
162 // Init the data vector with special config values from "noLoad" param in config file.
163 // This is to have a correct initilization for the data sent through yarp port
164 for (size_t sensorId = 0; sensorId < 16; sensorId++)
165 {
166 size_t index = 16 * 12 * boardIdx + sensorId * 12;
167
168 // Message head
169 for(int k = 0; k < 12; k++)
170 {
171// yDebug() << "readNewSpecialConfiguration size is: " << data.size() << " index is " << (index+k) << " value is: " << boardCfgList[j].cfg.noLoad;
172 if((index+k) >= skindata.size())
173 {
174 yError() << "readNewSpecialConfiguration: index too big";
175 }
176 skindata[index + k] = boardCfgList[j].cfg.noLoad;
177 }
178 }
179// //uncomment for debug only
180// yDebug() << "\n Special board cfg num " << j;
181// boardCfgList[j].debugPrint();
182
183 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin, _skCfg.patchInfoList[p].indexNv, eoprot_tag_sk_skin_cmmnds_boardscfg);
184
185 if(false == res->setRemoteValue(protoid, &bcfg))
186 {
187 yError() << "EmbObjSkin::initWithSpecialConfig(): in skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " Error in send special board config for mtb with addr from"<< boardCfgList[j].boardAddrStart << " to addr " << boardCfgList[j].boardAddrEnd;
188 return false;
189 }
190
191 } //end for for each special board cfg
192
193 SystemClock::delaySystem(0.010); // 10 ms (m.a.a-delay: before it was 0.01)
194
195 //-----------------------------------------------------------------------------------------------------
196 //------------ read special cfg triangle --------------------------------------------------------------
198 numofcfg = SPECIAL_TRIANGLE_CFG_MAX_NUM; //set size of my vector boardCfgList;
199 //in output the function return number of special board cfg are in file xml
200 ret = _cfgReader.readSpecialTriangleCfg(config, triangleCfg, &numofcfg);
201 if(!ret)
202 return false;
203
204
205 for(j=0; j<numofcfg; j++)
206 {
207 for(p=0; p< _skCfg.patchInfoList.size(); p++)
208 {
209 if(_skCfg.patchInfoList[p].idPatch == triangleCfg[j].patch)
210 break;
211 }
212 if(p >= _skCfg.patchInfoList.size())
213 {
214 yError() << "EmbObjSkin::initWithSpecialConfig(): in skin of bBOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": patch " << triangleCfg[j].patch << "not exists";
215 return false;
216 }
217 //now p is index patch
218
219 eOcanport_t canport = _skCfg.patchInfoList[p].canport;
220
221 //check if bcfg.boardAddr is in my patches list
222 if(-1 == _skCfg.patchInfoList[p].checkCardAddrIsInList(triangleCfg[j].boardAddr))
223 {
224 yError() << "EmbObjSkin::initWithSpecialConfig(): in skin of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " card with address " << triangleCfg[j].boardAddr << "is not present in patch " << _skCfg.patchInfoList[p].idPatch;
225 return(false);
226 }
227
228 //prepare data to send to ems
229 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin, _skCfg.patchInfoList[p].indexNv, eoprot_tag_sk_skin_cmmnds_trianglescfg);
230
231
232 eOsk_cmd_trianglesCfg_t tcfg = {0};
233
234 // this message is for one address only
235 tcfg.candestination[0] = tcfg.candestination[1] = 0;
236 int adr = triangleCfg[j].boardAddr;
237 if(isCANaddressValid(adr))
238 {
239 eo_common_hlfword_bitset(&tcfg.candestination[canport], adr);
240 }
241
242 tcfg.idstart = triangleCfg[j].triangleStart;
243 tcfg.idend = triangleCfg[j].triangleEnd;
244 tcfg.cfg.CDCoffset = triangleCfg[j].cfg.cdcOffset;
245 tcfg.cfg.enable = triangleCfg[j].cfg.enabled;
246 tcfg.cfg.shift = triangleCfg[j].cfg.shift;
247
248
249// //uncomment for debug only
250// yDebug() << "\n Special triangle cfg num " << j;
251// boardCfgList[j].debugPrint();
252
253 // allow the remote board to process one command at a time.
254 // otherwise the board sends up a diagnostics message telling that it cannot send a message in can bus
255 // we do that by ... sic ... adding a small delay.
256 // the above "yDebug() << "\n Special triangle cfg num " << j;" used to test was probably doing the same effect as a delay of a few ms
257 SystemClock::delaySystem(0.010);
258
259 if(false == res->setRemoteValue(protoid, &tcfg))
260 {
261 yError() << "EmbObjSkin::initWithSpecialConfig(): in skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " Error in send special triangle config for board CAN" << canport+1 << ":" << triangleCfg[j].boardAddr;
262 return false;
263 }
264 }
265
266 SystemClock::delaySystem(0.010); // 10 ms (m.a.a-delay: before it was 0.01)
267
268 return true;
269}
270
271
272bool EmbObjSkin::fromConfig(yarp::os::Searchable& config)
273{
274 Bottle bPatches, bPatchList, xtmp;
275 //reset total num of cards
277
278
279 servConfigSkin_t skinconfig;
280 parser->parseService(config, skinconfig);
281
282 bPatches = config.findGroup("patches", "skin patches connected to this device");
283 if(bPatches.isNull())
284 {
285 yError() << "EmbObjSkin::fromConfig(): in skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << "patches group is missing";
286 return(false);
287 }
288
290 char tmp[80];
291 for (int i=1; i<=2; i++)
292 {
293 sprintf(tmp,"skinCanAddrsPatch%d",i);
294 if (bPatches.check(tmp))
295 {
297 bPatchList.addInt32(i);
298 }
299 }
300
301 _skCfg.patchInfoList.clear();
302 _skCfg. patchInfoList.resize(_skCfg.numOfPatches);
303 for(int j=1; j<_skCfg.numOfPatches+1; j++)
304 {
305 int id = bPatchList.get(j-1).asInt32();
306 if((id!=1) && (id!=2))
307 {
308 yError() << "EmbObjSkin::fromConfig(): in skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << "expecting at most 2 patches";
309 return false;
310 }
311 _skCfg.patchInfoList[j-1].idPatch = id;
313 _skCfg.patchInfoList[j-1].canport = (1 == id) ? eOcanport1 : eOcanport2;
314 //yDebug("fromConfig: found CAN%d", _skCfg.patchInfoList[j-1].canport+1);
315 }
316
317
318 for(int i=0; i<_skCfg.numOfPatches; i++)
319 {
320 char tmp[80];
321 int id = _skCfg.patchInfoList[i].idPatch;
322 snprintf(tmp, sizeof(tmp), "skinCanAddrsPatch%d", id);
323
324 xtmp = bPatches.findGroup(tmp);
325 if(xtmp.isNull())
326 {
327 yError() << "EmbObjSkin::fromConfig(): skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << "doesn't find " << tmp << "in xml file";
328 return false;
329 }
330
331 _skCfg.patchInfoList[i].cardAddrList.resize(xtmp.size()-1);
332
333 for(int j=1; j<xtmp.size(); j++)
334 {
335 int addr = xtmp.get(j).asInt32();
337 _skCfg.patchInfoList[i].cardAddrList[j-1] = addr;
338 }
339 }
340
341 // impose the number of sensors (triangles found in config file)
342 sensorsNum = 16*12*_skCfg.totalCardsNum; // max num of card
343
344 // resize the skindata holder
345 mtx.lock();
346
347 this->skindata.resize(sensorsNum);
348 int ttt = this->skindata.size();
349 for (int i=0; i < ttt; i++)
350 {
351 this->skindata[i]=(double)240;
352 }
353
354 mtx.unlock();
355
356 // fill the ethservice ...
357
358 ethservice.configuration.type = eomn_serv_SK_skin;
359
360
361 ethservice.configuration.data.sk.skin.boardinfo.type = skinconfig.canboard.type;
362
363 ethservice.configuration.data.sk.skin.boardinfo.protocol.major = skinconfig.canboard.protocol.major;
364 ethservice.configuration.data.sk.skin.boardinfo.protocol.minor = skinconfig.canboard.protocol.minor;
365
366 ethservice.configuration.data.sk.skin.boardinfo.firmware.major = skinconfig.canboard.firmware.major;
367 ethservice.configuration.data.sk.skin.boardinfo.firmware.minor = skinconfig.canboard.firmware.minor;
368 ethservice.configuration.data.sk.skin.boardinfo.firmware.build = skinconfig.canboard.firmware.build;
369
370
371 ethservice.configuration.data.sk.skin.numofpatches = _skCfg.numOfPatches;
372 if(ethservice.configuration.data.sk.skin.numofpatches > eomn_serv_skin_maxpatches)
373 {
374 yError() << "cannot have so many skin patches. detected" << ethservice.configuration.data.sk.skin.numofpatches << "max is" << eomn_serv_skin_maxpatches;
375 return false;
376 }
377 // tagliato e cucito per le schede eb2 eb4 eb10 ed eb11. da migliorare sia i file xml che il parser.
378 for(int np=0; np<ethservice.configuration.data.sk.skin.numofpatches; np++)
379 {
380 // patch np-th, can1 ... for each address put a bit using eo_common_hlfword_bitset
381 ethservice.configuration.data.sk.skin.canmapskin[np][0] = 0;
382 //eo_common_hlfword_bitset(&ethservice.configuration.data.sk.skin.canmapskin[np][0], 3);
383
384 // patch np-th, can2 ... for each address put a bit using
385 ethservice.configuration.data.sk.skin.canmapskin[np][1] = 0;
386
387 eOcanport_t canport = _skCfg.patchInfoList[np].canport;
388
389 int max = _skCfg.patchInfoList[np].cardAddrList.size();
390 for(int n=0; n<max; n++)
391 {
392 int adr = _skCfg.patchInfoList[np].cardAddrList.at(n);
393 adr = adr;
394 if(isCANaddressValid(adr))
395 {
396 eo_common_hlfword_bitset(&ethservice.configuration.data.sk.skin.canmapskin[np][canport], adr);
397 //yDebug("config of service: initting mask for bdr @ CAN%d:%d", canport+1, adr);
398 }
399 }
400 }
401
402
404 {
405 _newCfg = true;
406 }
407 else
408 {
409 _newCfg = false;
410 return true;
411 }
412
413 /*read skin board default configuration*/
416 return false;
417
418 // Fill the data vector with default values from "noLoad" param in config file.
419 for (int board_idx = 0; board_idx < _skCfg.totalCardsNum; board_idx++)
420 {
421 for (int triangleId = 0; triangleId < 16; triangleId++)
422 {
423 int index = 16*12*board_idx + triangleId*12;
424
425 // Message head
426 for (size_t k = 0; k < 12; k++)
427 {
428// yDebug() << "EO readNewConfiguration (default) size is: " << data.size()
429// << " index is " << (index+k) << " value is: " << _brdCfg.noLoad;
430 if((index+k) >= skindata.size())
431 yError() << "readNewConfiguration: index too big";
432 skindata[index + k] = _brdCfg.noLoad;
433 }
434 }
435 }
436
437 /*read skin triangle default configuration*/
440 return false;
441
442 return true;
443}
444
445
446bool EmbObjSkin::open(yarp::os::Searchable& config)
447{
448 // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
449
451 if(NULL == ethManager)
452 {
453 yFatal() << "EmbObjSkin::open() fails to instantiate ethManager";
454 return false;
455 }
456
458 {
459 yError() << "embObjSkin::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
460 return false;
461 }
462 // add specific info about this device ...
463
464
465
466 // - now all other things
467
468 if(NULL == parser)
469 {
470 parser = new ServiceParser;
471 }
472
473 // read config file
474 if(false == fromConfig(config))
475 {
476 yError() << "embObjSkin::init() fails in function fromConfig() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": CANNOT PROCEED ANY FURTHER";
477 cleanup();
478 return false;
479 }
480
481
482
483 // -- instantiate EthResource etc.
484
485 res = ethManager->requestResource2(this, config);
486 if(NULL == res)
487 {
488 yError() << "embObjSkin::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
489 return false;
490 }
491
492 // now we have an ip address, thus we can set the name in object SkinConfigReader
493 char name[80];
494 snprintf(name, sizeof(name), "embObjSkin on BOARD %s IP %s", res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
495 _cfgReader.setName(name);
496
497
498 if(!res->verifyEPprotocol(eoprot_endpoint_skin))
499 {
500 cleanup();
501 return false;
502 }
503
504
505 const eOmn_serv_parameter_t* servparam = &ethservice;
506
507 if(false == res->serviceVerifyActivate(eomn_serv_category_skin, servparam, 5.0))
508 {
509 yError() << "embObjSkin::open() has an error in call of ethResources::serviceVerifyActivate() for board" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
510 cleanup();
511 return false;
512 }
513
514
515
516 if(!init())
517 return false;
518
519 /* Following delay is necessary in order to give enough time to skin boards to configure all its triangles */
520 SystemClock::delaySystem(0.500);
521
522 if(!initWithSpecialConfig(config))
523 {
524 cleanup();
525 return false;
526 }
527
529 {
530 cleanup();
531 return false;
532 }
533
534 if(!start())
535 {
536 cleanup();
537 return false;
538 }
539
540
541
542 if(false == res->serviceStart(eomn_serv_category_skin))
543 {
544 yError() << "embObjSkin::open() fails to start skin service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
545 cleanup();
546 return false;
547 }
548 else
549 {
550 if(verbosewhenok)
551 {
552 yDebug() << "embObjSkin::open() correctly starts skin service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
553 }
554 }
555
556 return true;
557}
558
559
560
562{
563 if(ethManager == NULL) return;
564
565 int ret = ethManager->releaseResource2(res, this);
566 res = NULL;
567 if(ret == -1)
569}
570
572{
573 cleanup();
574 return true;
575}
576
577
578
579int EmbObjSkin::read(yarp::sig::Vector &out)
580{
581 std::lock_guard<std::mutex> lck(mtx);
582 out = this->skindata; //old - this needs the running thread
583 return yarp::dev::IAnalogSensor::AS_OK;
584}
585
587{
588 return yarp::dev::IAnalogSensor::AS_OK;;
589}
590
592{
593 return sensorsNum;
594}
595
597{
598 return true;
599}
600
601int EmbObjSkin::calibrateChannel(int ch, double v)
602{
603 //NOT YET IMPLEMENTED
604 return calibrateSensor();
605}
606
607int EmbObjSkin::calibrateSensor(const yarp::sig::Vector& v)
608{
609 return 0;
610}
611
613{
614 return 0;
615}
616
618{
619 eOprotID32_t protoid;
620 uint8_t dat;
621 bool ret = true;
622 int i;
623
624 if(_newCfg)
625 {
626 dat = eosk_sigmode_signal;
627 }
628 else
629 {
630 dat = eosk_sigmode_signal_oldway;
631 }
632
633 for(i=0; i<_skCfg.numOfPatches;i++)
634 {
635 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin, _skCfg.patchInfoList[i].indexNv, eoprot_tag_sk_skin_config_sigmode);
636 ret = res->setRemoteValue(protoid, &dat);
637 if(!ret)
638 {
639 yError() << "EmbObjSkin::start(): unable to start skin for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " on port " << _skCfg.patchInfoList[i].idPatch;
640 return false;
641 }
642 }
643 return ret;
644}
645
647{
648 // - configure regular rops
649
650 vector<eOprotID32_t> id32v(0);
651 eOprotID32_t protoid = eo_prot_ID32dummy;
652
653 // choose the variables and put them inside vector
654
655
656 for(int i=0; i<_skCfg.numOfPatches; i++)
657 {
658 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin, _skCfg.patchInfoList[i].indexNv, eoprot_tag_sk_skin_status_arrayofcandata);
659 id32v.push_back(protoid);
660 }
661
662
663 if(false == res->serviceSetRegulars(eomn_serv_category_skin, id32v))
664 {
665 yError() << "EmbObjSkin::configPeriodicMessage() fails to add its variables to regulars: cannot proceed any further";
666 return false;
667 }
668 else
669 {
670 if(verbosewhenok)
671 {
672 yDebug() << "embObjSkin::configPeriodicMessage() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
673 char nvinfo[128];
674 for (size_t r = 0; r<id32v.size(); r++)
675 {
676 uint32_t id32 = id32v.at(r);
677 eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
678 yDebug() << "\t it added regular rop for" << nvinfo;
679 }
680 }
681 }
682 SystemClock::delaySystem(0.005); // 5 ms (m.a.a-delay: before it was 0)
683
684 return true;
685}
686
688{
689 int j = 0;
690 eOprotID32_t protoid;
691
692 // if old configuration style returns
693 if(!_newCfg)
694 {
695 return true;
696 }
697
698 // send default board and triangle configuration (new configuration style)
699 eOsk_cmd_boardsCfg_t defBoardCfg = {0};
700 eOsk_cmd_trianglesCfg_t defTriangleCfg = {0};
701 size_t i, k;
702
703 defBoardCfg.cfg.skintype = _brdCfg.skinType;
704 defBoardCfg.cfg.period = _brdCfg.period;
705 defBoardCfg.cfg.noload = _brdCfg.noLoad;
706
707 defTriangleCfg.idstart = 0;
708 defTriangleCfg.idend = 15;
709 defTriangleCfg.cfg.enable = _triangCfg.enabled;
710 defTriangleCfg.cfg.shift = _triangCfg.shift;
711 defTriangleCfg.cfg.CDCoffset = _triangCfg.cdcOffset;
712
713 for(i=0; i<_skCfg.numOfPatches;i++)
714 {
715 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin, _skCfg.patchInfoList[i].indexNv, eoprot_tag_sk_skin_cmmnds_boardscfg);
716
717 // get min and max address
718 uint8_t minAddr = 16;
719 uint8_t maxAddr = 0;
720
721 for(k=0; k<_skCfg.patchInfoList[i].cardAddrList.size(); k++)
722 {
723 int adr = _skCfg.patchInfoList[i].cardAddrList[k];
724
725 if(isCANaddressValid(adr))
726 {
727 if(_skCfg.patchInfoList[i].cardAddrList[k] < minAddr)
728 minAddr = _skCfg.patchInfoList[i].cardAddrList[k];
729
730 if(_skCfg.patchInfoList[i].cardAddrList[k] > maxAddr)
731 maxAddr = _skCfg.patchInfoList[i].cardAddrList[k];
732 }
733 }
734
735
736
737 // we send the config to the whole patch, hence 0xffff
738 defBoardCfg.candestination[0] = defBoardCfg.candestination[1] = 0xffff;
739
740 if(false == res->setRemoteValue(protoid, &defBoardCfg))
741 {
742 yError() << "EmbObjSkin::init(): in skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " Error in send default board config for patch #"<< i;
743 return false;
744 }
745
746 }
747 SystemClock::delaySystem(0.010);
748
749 for(i=0; i<_skCfg.numOfPatches;i++)
750 {
751 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin, _skCfg.patchInfoList[i].indexNv, eoprot_tag_sk_skin_cmmnds_trianglescfg);
752
753
754 // we send the config to the whole patch, hence 0xffff
755 defTriangleCfg.candestination[0] = defTriangleCfg.candestination[1] = 0xffff;
756
757 if(false == res->setRemoteValue(protoid, &defTriangleCfg))
758 {
759 yError() << "EmbObjSkin::init(): in skin BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << " Error in send default triangle config for patch # "<< i;
760 return false;
761 }
762
763 }
764
765 opened = true;
766
767 return true;
768}
769
771{
772 return opened;
773}
774
779
780#undef DEBUG_PRINT_RX_STATS
781#if defined(DEBUG_PRINT_RX_STATS)
782static uint32_t receivedpatches[2][16] = {0};
783static uint32_t counterpa = 0;
784#endif
785
786bool EmbObjSkin::update(eOprotID32_t id32, double timestamp, void *rxdata)
787{
788 uint8_t msgtype = 0;
789 uint8_t i, triangle = 0;
790 static int error = 0;
791 int p;
792 EOarray* arrayof = (EOarray*)rxdata;
793 uint8_t sizeofarray = eo_array_Size(arrayof);
794
795 eOprotIndex_t indexpatch = eoprot_ID2index(id32);
796
797 for(p=0; p<_skCfg.numOfPatches; p++)
798 {
799 if(_skCfg.patchInfoList[p].indexNv == indexpatch)
800 break;
801 }
802 if(p >= _skCfg.numOfPatches)
803 {
804 yError() << "EmbObjSkin::update(): skin of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": received data of patch with nvindex= " << indexpatch;
805 return false;
806 }
807
808 // yDebug() << "received data from " << patchInfoList[p].idPatch << "port";
809
810 errors.resize(sizeofarray);
811
812 for(i=0; i<sizeofarray; i++)
813 {
814 eOsk_candata_t *candata = (eOsk_candata_t*) eo_array_At(arrayof, i);
815
816 if(NULL == candata)
817 {
818 break;
819 }
820
821 uint16_t canframeid11 = EOSK_CANDATA_INFO2IDCAN(candata->info);
822 uint8_t canframesize = EOSK_CANDATA_INFO2SIZE(candata->info);
823 uint8_t *canframedata = candata->data;
824
825 uint8_t mtbId = 255; // unknown mtb card addr
826 uint8_t cardAddr = 0;
827 uint8_t valid = 0;
828 uint8_t skinClass;
829
830 if(_newCfg)
831 skinClass = ICUBCANPROTO_CLASS_PERIODIC_SKIN;
832 else
833 skinClass = ICUBCANPROTO_CLASS_PERIODIC_ANALOGSENSOR;
834
835
836
837 valid = (((canframeid11 & 0x0f00) >> 8) == skinClass) ? 1 : 0;
838
839 if(valid)
840 {
841 cardAddr = (canframeid11 & 0x00f0) >> 4;
842 //get index of start of data of board with addr cardId.
843 for (size_t cId_index = 0; cId_index< _skCfg.patchInfoList[p].cardAddrList.size(); cId_index++)
844 {
845 if(_skCfg.patchInfoList[p].cardAddrList[cId_index] == cardAddr)
846 {
847 mtbId = cId_index;
848 if(_skCfg.numOfPatches==2 && p==0)
849 mtbId += _skCfg.patchInfoList[1].cardAddrList.size(); //add max num of boards on patch number 2 because they are sorted in decreasing order by can addr
850 break;
851 }
852 }
853
854 if(mtbId == 255)
855 {
856 //yError() << "Unknown cardId from skin\n";
857 return false;
858 }
859
860 //printf("mtbId=%d\n", mtbId);
861 triangle = (canframeid11 & 0x000f);
862 msgtype = (int) canframedata[0];
863
864 int index=16*12*mtbId + triangle*12;
865
866 // marco.accame: added lock to avoid concurrent access to this->skindata. i lock at triangle resolution ...
867 mtx.lock();
868
869 if (msgtype == 0x40)
870 {
871#if defined(DEBUG_PRINT_RX_STATS)
872 receivedpatches[p][cardAddr]++;
873 counterpa ++;
874#endif
875 // Message head
876 for(int k = 0; k < 7; k++)
877 {
878 skindata[index + k] = canframedata[k + 1];
879 }
880 }
881 else if (msgtype == 0xC0)
882 {
883 // Message tail
884 for(int k = 0; k < 5; k++)
885 {
886 skindata[index + k + 7] = canframedata[k + 1];
887 }
888
889 // Skin diagnostics
890 if (_brdCfg.useDiagnostic) // if user requests to check the diagnostic
891 {
892 if (canframesize == 8)
893 {
894 // Skin diagnostics is active
895 _isDiagnosticPresent = true;
896
897 // Get error code head and tail
898 short head = canframedata[6];
899 short tail = canframedata[7];
900 int fullMsg = (head << 8) | (tail & 0xFF);
901
902 // Store error message
903 errors[i].net = indexpatch;
904 errors[i].board = cardAddr;
905 errors[i].sensor = triangle;
906 errors[i].error = fullMsg;
907
908 if (fullMsg != SkinErrorCode::StatusOK)
909 {
910 yError() << "embObjSkin error code: " <<
911 "BOARD: " << res->getProperties().boardnameString <<
912 "IP:" << res->getProperties().ipv4addrString <<
913 "canDeviceNum: " << errors[i].net <<
914 "board: " << errors[i].board <<
915 "sensor: " << errors[i].sensor <<
916 "error: " << iCub::skin::diagnostics::printErrorCode(errors[i].error).c_str();
917 }
918 }
919 else
920 {
921 _isDiagnosticPresent = false;
922 }
923 }
924 }
925 mtx.unlock();
926 }
927 else if(canframeid11 == 0x100)
928 {
929 /* Can frame with id =0x100 contains Debug info. SO I skip it.*/
930 return true;
931 }
932 else
933 {
934 if(error == 0)
935 yError() << "EMS: " << res->getProperties().ipv4addrString << " Unknown Message received from skin (" << i<<"/"<< sizeofarray <<"): frameID=" << canframeid11<< " len="<<canframesize << "canframe.data="<<canframedata[0] << " " <<canframedata[1] << " " <<canframedata[2] << " " <<canframedata[3] <<"\n" ;
936 error++;
937 if (error == 10000)
938 error = 0;
939 }
940 }
941
942#if defined(DEBUG_PRINT_RX_STATS)
943 if(counterpa >= 10000)
944 {
945 counterpa = 0;
946 yDebug("pa = {{%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d} {%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d}}",
947 receivedpatches[0][0], receivedpatches[0][1], receivedpatches[0][2], receivedpatches[0][3], receivedpatches[0][4], receivedpatches[0][5], receivedpatches[0][6], receivedpatches[0][7],
948 receivedpatches[0][8], receivedpatches[0][9], receivedpatches[0][10], receivedpatches[0][11], receivedpatches[0][12], receivedpatches[0][13], receivedpatches[0][14], receivedpatches[0][15],
949 receivedpatches[1][0], receivedpatches[1][1], receivedpatches[1][2], receivedpatches[1][3], receivedpatches[1][4], receivedpatches[1][5], receivedpatches[1][6], receivedpatches[1][7],
950 receivedpatches[1][8], receivedpatches[1][9], receivedpatches[1][10], receivedpatches[1][11], receivedpatches[1][12], receivedpatches[1][13], receivedpatches[1][14], receivedpatches[1][15]
951 );
952
953 }
954#endif
955
956 return true;
957}
958
959/* *********************************************************************************************************************** */
960/* ******* Diagnose skin errors. ********************************************** */
961//bool EmbObjSkin::diagnoseSkin(void) {
962// using iCub::skin::diagnostics::DetectedError; // FG: Skin diagnostics errors
963// using yarp::sig::Vector;
964
965// if (useDiagnostics) {
966// // Write errors to port
967// for (size_t i = 0; i < errors.size(); ++i) {
968// Vector &out = portSkinDiagnosticsOut.prepare();
969// out.clear();
970
971// out.push_back(errors[i].net);
972// out.push_back(errors[i].board);
973// out.push_back(errors[i].sensor);
974// out.push_back(errors[i].error);
975
976// portSkinDiagnosticsOut.write(true);
977// }
978// }
979
980// return true;
981//}
982/* *********************************************************************************************************************** */
983// eof
984
985
virtual int read(yarp::sig::Vector &out)
bool fromConfig(yarp::os::Searchable &config)
eth::AbstractEthResource * res
Definition embObjSkin.h:89
SkinConfigReader _cfgReader
Definition embObjSkin.h:100
SkinConfig _skCfg
Definition embObjSkin.h:101
Vector skindata
Definition embObjSkin.h:95
virtual int calibrateSensor()
string boardName
Definition embObjSkin.h:85
SkinBoardCfgParam _brdCfg
Definition embObjSkin.h:97
virtual bool initialised()
@ SPECIAL_TRIANGLE_CFG_MAX_NUM
Definition embObjSkin.h:78
virtual int calibrateChannel(int ch, double v)
void cleanup(void)
bool _newCfg
Definition embObjSkin.h:99
string boardIPstring
Definition embObjSkin.h:84
eOprotIndex_t convertIdPatch2IndexNv(int idPatch)
Definition embObjSkin.h:108
bool initWithSpecialConfig(yarp::os::Searchable &config)
SkinTriangleCfgParam _triangCfg
Definition embObjSkin.h:98
bool opened
Definition embObjSkin.h:80
virtual bool open(yarp::os::Searchable &config)
size_t sensorsNum
Definition embObjSkin.h:94
eOipv4addr_t ipv4addr
Definition embObjSkin.h:86
virtual bool close()
virtual eth::iethresType_t type()
virtual int getState(int ch)
virtual int getChannels()
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
bool configPeriodicMessage(void)
std::mutex mtx
Definition embObjSkin.h:91
eth::TheEthManager * ethManager
Definition embObjSkin.h:88
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
void setDefaultValues(void)
Definition skinParams.h:39
void setName(char *name)
bool readDefaultBoardCfg(yarp::os::Searchable &config, SkinBoardCfgParam *boardCfg)
bool isDefaultBoardCfgPresent(yarp::os::Searchable &config)
bool readSpecialBoardCfg(yarp::os::Searchable &config, SpecialSkinBoardCfgParam *boardCfg, unsigned int *numofcfg)
bool readDefaultTriangleCfg(yarp::os::Searchable &config, SkinTriangleCfgParam *triangCfg)
bool readSpecialTriangleCfg(yarp::os::Searchable &config, SpecialSkinTriangleCfgParam *triangleCfg, unsigned int *numofcfg)
bool isDefaultTriangleCfgPresent(yarp::os::Searchable &config)
uint8_t numOfPatches
Definition embObjSkin.h:64
int totalCardsNum
Definition embObjSkin.h:62
std::vector< SkinPatchInfo > patchInfoList
Definition embObjSkin.h:63
std::vector< int > cardAddrList
Definition embObjSkin.h:55
int checkCardAddrIsInList(int cardAddr)
void setDefaultValues(void)
Definition skinParams.h:60
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=0
virtual const Properties & getProperties()=0
virtual bool setRemoteValue(const eOprotID32_t id32, void *value)=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
virtual bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)=0
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
static bool killYourself()
static TheEthManager * instance()
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
int n
bool error
bool isCANaddressValid(int adr)
iethresType_t
@ iethres_skin
std::string printErrorCode(int code)
out
Definition sine.m:8
eObrd_cantype_t type
eObrd_protocolversion_t protocol
eObrd_firmwareversion_t firmware
servCanBoard_t canboard