iCub-main
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 
37 using namespace std;
38 using namespace iCub::skin::diagnostics;
39 
40 bool 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 
56 EmbObjSkin::EmbObjSkin() : _isDiagnosticPresent(false)
57 {
58  res = NULL;
59  ethManager = NULL;
60  opened = false;
61  sensorsNum = 0;
62  _skCfg.numOfPatches = 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 
92 bool 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 
272 bool EmbObjSkin::fromConfig(yarp::os::Searchable& config)
273 {
274  Bottle bPatches, bPatchList, xtmp;
275  //reset total num of cards
276  _skCfg.totalCardsNum = 0;
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;
312  _skCfg.patchInfoList[j-1].indexNv = convertIdPatch2IndexNv(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*/
415  if(!_cfgReader.readDefaultBoardCfg(config, &_brdCfg))
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 
446 bool 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 
528  if(!configPeriodicMessage())
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 
579 int 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 
601 int EmbObjSkin::calibrateChannel(int ch, double v)
602 {
603  //NOT YET IMPLEMENTED
604  return calibrateSensor();
605 }
606 
607 int 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 
776 {
777  return eth::iethres_skin;
778 }
779 
780 #undef DEBUG_PRINT_RX_STATS
781 #if defined(DEBUG_PRINT_RX_STATS)
782 static uint32_t receivedpatches[2][16] = {0};
783 static uint32_t counterpa = 0;
784 #endif
785 
786 bool 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)
Definition: embObjSkin.cpp:579
bool fromConfig(yarp::os::Searchable &config)
Definition: embObjSkin.cpp:272
eth::AbstractEthResource * res
Definition: embObjSkin.h:89
SkinConfigReader _cfgReader
Definition: embObjSkin.h:100
SkinConfig _skCfg
Definition: embObjSkin.h:101
bool init()
Definition: embObjSkin.cpp:687
Vector skindata
Definition: embObjSkin.h:95
virtual int calibrateSensor()
Definition: embObjSkin.cpp:596
string boardName
Definition: embObjSkin.h:85
SkinBoardCfgParam _brdCfg
Definition: embObjSkin.h:97
virtual bool initialised()
Definition: embObjSkin.cpp:770
bool start()
Definition: embObjSkin.cpp:617
@ SPECIAL_TRIANGLE_CFG_MAX_NUM
Definition: embObjSkin.h:78
virtual int calibrateChannel(int ch, double v)
Definition: embObjSkin.cpp:601
void cleanup(void)
Definition: embObjSkin.cpp:561
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)
Definition: embObjSkin.cpp:92
SkinTriangleCfgParam _triangCfg
Definition: embObjSkin.h:98
bool opened
Definition: embObjSkin.h:80
virtual bool open(yarp::os::Searchable &config)
Definition: embObjSkin.cpp:446
size_t sensorsNum
Definition: embObjSkin.h:94
eOipv4addr_t ipv4addr
Definition: embObjSkin.h:86
virtual bool close()
Definition: embObjSkin.cpp:571
virtual eth::iethresType_t type()
Definition: embObjSkin.cpp:775
virtual int getState(int ch)
Definition: embObjSkin.cpp:586
virtual int getChannels()
Definition: embObjSkin.cpp:591
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
Definition: embObjSkin.cpp:786
bool configPeriodicMessage(void)
Definition: embObjSkin.cpp:646
std::mutex mtx
Definition: embObjSkin.h:91
eth::TheEthManager * ethManager
Definition: embObjSkin.h:88
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
uint8_t skinType
Definition: skinParams.h:36
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
int checkCardAddrIsInList(int cardAddr)
Definition: embObjSkin.cpp:46
void setDefaultValues(void)
Definition: skinParams.h:60
SkinTriangleCfgParam cfg
virtual const Properties & getProperties()=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=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)
Definition: ethManager.cpp:409
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
Definition: ethManager.cpp:283
static bool killYourself()
Definition: ethManager.cpp:178
static TheEthManager * instance()
Definition: ethManager.cpp:159
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
Definition: ethManager.cpp:336
int n
bool isCANaddressValid(int adr)
Definition: embObjSkin.cpp:40
iethresType_t
Definition: IethResource.h:62
@ iethres_skin
Definition: IethResource.h:68
std::string printErrorCode(int code)
const FSC max
Definition: strain.h:48
out
Definition: sine.m:8
eObrd_cantype_t type
Definition: serviceParser.h:97
eObrd_protocolversion_t protocol
Definition: serviceParser.h:98
eObrd_firmwareversion_t firmware
Definition: serviceParser.h:99
servCanBoard_t canboard