iCub-main
embObjMais.cpp
Go to the documentation of this file.
1 
2 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
3 
4 /*
5 * Copyright (C) 2012 Robotcub Consortium
6 * Author: Alberto Cardellino
7 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
8 *
9 */
10 
11 // general purpose stuff.
12 #include <string>
13 #include <iostream>
14 #include <string.h>
15 
16 // Yarp Includes
17 #include <yarp/os/Time.h>
18 #include <yarp/os/Log.h>
19 #include <yarp/os/LogStream.h>
20 #include <stdarg.h>
21 #include <stdio.h>
22 #include <yarp/dev/PolyDriver.h>
23 #include <ace/config.h>
24 #include <ace/Log_Msg.h>
25 
26 
27 // specific to this device driver.
28 #include <embObjMais.h>
29 #include <ethManager.h>
30 #include <yarp/os/LogStream.h>
31 #include "EoAnalogSensors.h"
32 #include "EOnv_hid.h"
33 
34 #include "EoProtocol.h"
35 #include "EoProtocolMN.h"
36 #include "EoProtocolAS.h"
37 
38 #include <yarp/os/NetType.h>
39 #include <yarp/conf/environment.h>
40 
41 #ifdef WIN32
42 #pragma warning(once:4355)
43 #endif
44 
45 
46 
47 using namespace yarp;
48 using namespace yarp::os;
49 using namespace yarp::dev;
50 
51 
52 inline bool NOT_YET_IMPLEMENTED(const char *txt)
53 {
54  yWarning() << std::string(txt) << " not yet implemented for embObjMais\n";
55  return false;
56 }
57 
58 // generic function that checks is key1 is present in input bottle and that the result has size elements
59 // return true/false
60 bool embObjMais::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
61 {
62  size++; // size includes also the name of the parameter
63  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
64  if (tmp.isNull())
65  {
66  yError ("%s not found\n", key1.c_str());
67  return false;
68  }
69 
70  if(tmp.size()!=size)
71  {
72  yError("%s incorrect number of entries\n", key1.c_str());
73  return false;
74  }
75 
76  out=tmp;
77 
78  return true;
79 }
80 
81 
82 bool embObjMais::fromConfig(yarp::os::Searchable &_config)
83 {
84 #if defined(EMBOBJMAIS_USESERVICEPARSER)
85 
86 
87  if(false == parser->parseService(_config, serviceConfig))
88  {
89  return false;
90  }
91 
92  return true;
93 
94 #else
95 
96  Bottle xtmp;
97 
98  int _period = 0;
99 
100  // Analog Sensor stuff
101  Bottle config = _config.findGroup("GENERAL");
102  if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
103  {
104  yError() << "embObjMais Using default value = 0 (disabled)";
105  _period = 0;
106 
107  }
108  else
109  {
110  _period = xtmp.get(1).asInt32();
111  yDebug() << "embObjMais::fromConfig() detects embObjMais Using value of" << _period;
112  }
113 
114  serviceConfig.acquisitionrate = _period;
115 
116  return true;
117 #endif
118 }
119 
120 
121 embObjMais::embObjMais()
122 {
123  serviceConfig.acquisitionrate = 0;
124  memset(&serviceConfig.ethservice, 0, sizeof(serviceConfig.ethservice));
125 
126  timeStamp = 0;
127 
128  counterSat=0;
129  counterError=0;
130  counterTimeout=0;
131 
132  status = IAnalogSensor::AS_OK;
133 
134 
135  opened = false;
136 
137  analogdata.resize(0);
138 
139 
140  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
141  if (tmp != "")
142  {
143  verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
144  }
145  else
146  {
147  verbosewhenok = false;
148  }
149 
150  parser = NULL;
151  res = NULL;
152 }
153 
154 
155 embObjMais::~embObjMais()
156 {
157  analogdata.resize(0);
158 
159  if(NULL != parser)
160  {
161  delete parser;
162  parser = NULL;
163  }
164 
165 }
166 
167 
168 bool embObjMais::initialised()
169 {
170  return opened;
171 }
172 
173 //#define TEST_MAIS_PLUS_MC
174 
175 
176 bool embObjMais::open(yarp::os::Searchable &config)
177 {
178  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
179 
180  ethManager = eth::TheEthManager::instance();
181  if(NULL == ethManager)
182  {
183  yFatal() << "embObjMais::open() fails to instantiate ethManager";
184  return false;
185  }
186 
187 
188  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
189  {
190  yError() << "embObjMais::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
191  return false;
192  }
193  // add specific info about this device ...
194 
195 
196  // - now all other things
197 
198 
199 // std::string str;
200 // if(config.findGroup("GENERAL").find("verbose").asBool())
201 // str=config.toString().c_str();
202 // else
203 // str="\n";
204 // yTrace() << str;
205 
206 
207  if(NULL == parser)
208  {
209  parser = new ServiceParser;
210  }
211 
212  // read stuff from config file
213  if(!fromConfig(config))
214  {
215  yError() << "embObjMais missing some configuration parameter. Check logs and your config file.";
216  return false;
217  }
218 
219  // and prepare analogdata
220  {
221  // must be of size: mais_Channels, and 0.0-initted
222  analogdata.resize(mais_Channels, 0.0);
223  }
224 
225 
226 
227  // -- instantiate EthResource etc.
228 
229  res = ethManager->requestResource2(this, config);
230  if(NULL == res)
231  {
232  yError() << "embObjMais::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
233  return false;
234  }
235 
236  printServiceConfig();
237 
238 
239  if(!res->verifyEPprotocol(eoprot_endpoint_analogsensors))
240  {
241  cleanup();
242  return false;
243  }
244 
245 #if defined(TEST_MAIS_PLUS_MC)
246 
247  // sarebbe bene chiamare requestResource2(ptr2fakemccontroller, config) ma pazienza
248 
249  const eOmn_serv_parameter_t* mcservparam = NULL;
250 
251  if(false == res->serviceVerifyActivate(eomn_serv_category_mc, mcservparam, 5.0))
252  {
253  yError() << "embObjMais::open() has an error in call of ethResources::serviceVerifyActivate(MC) for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
254  cleanup();
255  return false;
256  }
257  if(false == res->serviceStart(eomn_serv_category_mc))
258  {
259  yError() << "embObjMais::open() fails to start MC service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
260  cleanup();
261  return false;
262  }
263  else
264  {
265  if(verbosewhenok)
266  {
267  yDebug() << "embObjMais::open() correctly starts MC service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
268  }
269  }
270 
271 
272 #endif
273 
274 
275 
276 
277 
278 
279 #if defined(EMBOBJMAIS_USESERVICEPARSER)
280  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
281 #else
282  const eOmn_serv_parameter_t* servparam = NULL;
283 #endif
284 
285  //servparam = NULL;
286 
287  if(false == res->serviceVerifyActivate(eomn_serv_category_mais, servparam, 5.0))
288  {
289  SystemClock::delaySystem(1);
290  yError() << "embObjMais::open() has an error in call of ethResources::serviceVerifyActivate() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
291  //printServiceConfig();
292  cleanup();
293  return false;
294  }
295 
296  //printServiceConfig();
297 
298 
299  // configure the service: aka, send to the remote board information about the whereabouts of the can boards mais, strain, mtb which offers the service.
300  // so far nothing to do
301 
302 
303  // configure the sensor(s)
304 
305 
306  if(false == sendConfig2Mais())
307  {
308  cleanup();
309  return false;
310  }
311 
312  // Set variable to be signalled
313  if(false == initRegulars())
314  {
315  cleanup();
316  return false;
317  }
318 
319 
320  if(false == res->serviceStart(eomn_serv_category_mais))
321  {
322  yError() << "embObjMais::open() fails to start as service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
323  cleanup();
324  return false;
325  }
326  else
327  {
328  if(verbosewhenok)
329  {
330  yDebug() << "embObjMais::open() correctly starts as service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
331  }
332  }
333 
334 
335  opened = true;
336  return true;
337 }
338 
339 
340 bool embObjMais::sendConfig2Mais(void)
341 {
342  // version with read-back
343 
344  eOprotID32_t id32 = eo_prot_ID32dummy;
345 
346  // -- mais datarate
347 
348  uint8_t datarate = serviceConfig.acquisitionrate;
349  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_datarate);
350 
351  if(false == res->setcheckRemoteValue(id32, &datarate, 10, 0.010, 0.050))
352  {
353  yError() << "FATAL: embObjMais::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais datarate in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
354  return false;
355  }
356  else
357  {
358  if(verbosewhenok)
359  {
360  yDebug() << "embObjMais::sendConfig2Mais() correctly configured mais datarate at value" << datarate << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
361  }
362  }
363 
364  // -- mais tx mode
365 
366  eOenum08_t maismode = eoas_maismode_txdatacontinuously; // use eOas_maismode_t for value BUT USE for type (their sizes can be different !!)
367  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
368 
369  if(false == res->setcheckRemoteValue(id32, &maismode, 10, 0.010, 0.050))
370  {
371  yError() << "FATAL: embObjMais::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais mode in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
372  return false;
373  }
374  else
375  {
376  if(verbosewhenok)
377  {
378  yDebug() << "embObjMais::sendConfig2Mais() correctly configured mais mode at value" << maismode << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
379  }
380  }
381 
382  return true;
383 
384 }
385 
386 
387 
388 bool embObjMais::initRegulars()
389 {
390  // configure regular rops
391 
392  vector<eOprotID32_t> id32v(0);
393  eOprotID32_t id32 = eo_prot_ID32dummy;
394 
395  // we need to choose the id32 to put inside the vector
396 
397  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_status_the15values);;
398 
399  // and put it inside vector
400 
401  id32v.push_back(id32);
402 
403  // now we send the vector
404 
405  if(false == res->serviceSetRegulars(eomn_serv_category_mais, id32v))
406  {
407  yError() << "embObjMais::initRegulars() fails to add its variables to regulars: cannot proceed any further";
408  return false;
409  }
410  else
411  {
412  if(verbosewhenok)
413  {
414  yDebug() << "embObjMais::initRegulars() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
415  char nvinfo[128];
416  for (size_t r = 0; r<id32v.size(); r++)
417  {
418  uint32_t item = id32v.at(r);
419  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
420  yDebug() << "\t it added regular rop for" << nvinfo;
421  }
422  }
423  }
424 
425  return true;
426 }
427 
428 
434 int embObjMais::read(yarp::sig::Vector &out)
435 {
436  // This method gives analogdata to the analogServer
437 
438  if(false == opened)
439  {
440  return false;
441  }
442 
443  std::lock_guard<std::mutex> lck(mtx);
444 
445  // errors are not handled for now... it'll always be OK!!
446  if (status != IAnalogSensor::AS_OK)
447  {
448  switch (status)
449  {
450  case IAnalogSensor::AS_OVF:
451  {
452  counterSat++;
453  } break;
454  case IAnalogSensor::AS_ERROR:
455  {
456  counterError++;
457  } break;
458  case IAnalogSensor::AS_TIMEOUT:
459  {
460  counterTimeout++;
461  } break;
462  default:
463  {
464  counterError++;
465  } break;
466  }
467  return status;
468  }
469 
470  out.resize(analogdata.size());
471  for (size_t k = 0; k<analogdata.size(); k++)
472  {
473  out[k] = analogdata[k];
474  }
475 
476  return status;
477 }
478 
479 
480 void embObjMais::resetCounters()
481 {
482  counterSat=0;
483  counterError=0;
484  counterTimeout=0;
485 }
486 
487 
488 void embObjMais::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
489 {
490  sat=counterSat;
491  err=counterError;
492  to=counterTimeout;
493 }
494 
495 
496 int embObjMais::getState(int ch)
497 {
498  printf("getstate\n");
499  return AS_OK;
500 }
501 
502 
503 int embObjMais::getChannels()
504 {
505  return analogdata.size();
506 }
507 
508 
509 int embObjMais::calibrateSensor()
510 {
511  return AS_OK;
512 }
513 
514 
515 int embObjMais::calibrateSensor(const yarp::sig::Vector& value)
516 {
517  return AS_OK;
518 }
519 
520 
521 int embObjMais::calibrateChannel(int ch)
522 {
523  return AS_OK;
524 }
525 
526 
527 int embObjMais::calibrateChannel(int ch, double v)
528 {
529  return AS_OK;
530 }
531 
532 
533 eth::iethresType_t embObjMais::type()
534 {
536 }
537 
538 
539 bool embObjMais::update(eOprotID32_t id32, double timestamp, void* rxdata)
540 {
541  id32 = id32;
542  timestamp = timestamp;
543 
544  if(false == opened)
545  {
546  return false;
547  }
548 
549  // called by feat_manage_analogsensors_data() which is called by:
550  // eoprot_fun_UPDT_as_mais_status_the15values()
551  // the void* parameter inside this function is a eOas_arrayofupto36bytes_t*
552  // and can be treated as a EOarray
553 
554 
555  EOarray *array = (EOarray*)rxdata;
556  uint8_t size = eo_array_Size(array);
557  uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 1, as the code after uses this convention
558  if((0 == size) || (1 != itemsize))
559  {
560  return false;
561  }
562 
563  std::lock_guard<std::mutex> lck(mtx);
564 
565  for (size_t k = 0; k<analogdata.size(); k++)
566  {
567  uint8_t* tmp = (uint8_t*) eo_array_At(array, k);
568  if(NULL != tmp)
569  {
570  uint8_t val = *tmp; // marco.accame: i see that we treat the array as if containing items of 1 byte.
571  // Get the kth element of the array
572  analogdata[k] = (double)val;
573  }
574  }
575 
576  return true;
577 }
578 
579 
580 
581 
582 bool embObjMais::close()
583 {
584  opened = false;
585 
586  cleanup();
587  return true;
588 }
589 
590 
591 void embObjMais::printServiceConfig(void)
592 {
593  char loc[20] = {0};
594  char fir[20] = {0};
595  char pro[20] = {0};
596 
597  const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
598  const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
599 
600  parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc, sizeof(loc));
601  parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir, sizeof(fir));
602  parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro, sizeof(pro));
603 
604  yInfo() << "The embObjMais device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
605  yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
606  yInfo() << "- MAIS named" << serviceConfig.nameOfMais << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
607 }
608 
609 
610 void embObjMais::cleanup(void)
611 {
612  if(ethManager == NULL) return;
613 
614  int ret = ethManager->releaseResource2(res, this);
615  res = NULL;
616  if(ret == -1)
617  ethManager->killYourself();
618 }
619 
620 // eof
621 
static TheEthManager * instance()
Definition: ethManager.cpp:159
bool NOT_YET_IMPLEMENTED(const char *txt)
Definition: embObjMais.cpp:52
static int v
Definition: iCub_Sim.cpp:42
double sat(const double val, const double min, const double max)
Definition: utils.h:183
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
iethresType_t
Definition: IethResource.h:61
@ iethres_analogmais
Definition: IethResource.h:64
Copyright (C) 2008 RobotCub Consortium.
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
out
Definition: sine.m:8