17 #include <yarp/os/Time.h>
18 #include <yarp/os/Log.h>
19 #include <yarp/os/LogStream.h>
22 #include <yarp/dev/PolyDriver.h>
23 #include <ace/config.h>
24 #include <ace/Log_Msg.h>
30 #include <yarp/os/LogStream.h>
31 #include "EoAnalogSensors.h"
34 #include "EoProtocol.h"
35 #include "EoProtocolMN.h"
36 #include "EoProtocolAS.h"
38 #include <yarp/os/NetType.h>
39 #include <yarp/conf/environment.h>
42 #pragma warning(once:4355)
48 using namespace yarp::os;
54 yWarning() << std::string(txt) <<
" not yet implemented for embObjMais\n";
63 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
66 yError (
"%s not found\n", key1.c_str());
72 yError(
"%s incorrect number of entries\n", key1.c_str());
82 bool embObjMais::fromConfig(yarp::os::Searchable &_config)
84 #if defined(EMBOBJMAIS_USESERVICEPARSER)
87 if(
false == parser->parseService(_config, serviceConfig))
101 Bottle config = _config.findGroup(
"GENERAL");
102 if (!
extractGroup(config, xtmp,
"Period",
"transmitting period of the sensors", 1))
104 yError() <<
"embObjMais Using default value = 0 (disabled)";
110 _period = xtmp.get(1).asInt32();
111 yDebug() <<
"embObjMais::fromConfig() detects embObjMais Using value of" << _period;
114 serviceConfig.acquisitionrate = _period;
121 embObjMais::embObjMais()
123 serviceConfig.acquisitionrate = 0;
124 memset(&serviceConfig.ethservice, 0,
sizeof(serviceConfig.ethservice));
132 status = MAS_status::MAS_OK;
137 analogdata.resize(0);
140 std::string
tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
143 verbosewhenok = (bool)(yarp::conf::numeric::from_string(
tmp, 0U));
147 verbosewhenok =
false;
155 embObjMais::~embObjMais()
157 analogdata.resize(0);
168 bool embObjMais::initialised()
176 bool embObjMais::open(yarp::os::Searchable &config)
181 if(NULL == ethManager)
183 yFatal() <<
"embObjMais::open() fails to instantiate ethManager";
188 if(
false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
190 yError() <<
"embObjMais::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
213 if(!fromConfig(config))
215 yError() <<
"embObjMais missing some configuration parameter. Check logs and your config file.";
222 analogdata.resize(mais_Channels, 0.0);
229 res = ethManager->requestResource2(
this, config);
232 yError() <<
"embObjMais::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring <<
" ... unable to continue";
236 printServiceConfig();
239 if(!res->verifyEPprotocol(eoprot_endpoint_analogsensors))
245 #if defined(TEST_MAIS_PLUS_MC)
249 const eOmn_serv_parameter_t* mcservparam = NULL;
251 if(
false == res->serviceVerifyActivate(eomn_serv_category_mc, mcservparam, 5.0))
253 yError() <<
"embObjMais::open() has an error in call of ethResources::serviceVerifyActivate(MC) for BOARD" << res->getProperties().boardnameString <<
"IP" << res->getProperties().ipv4addrString;
257 if(
false == res->serviceStart(eomn_serv_category_mc))
259 yError() <<
"embObjMais::open() fails to start MC service for BOARD" << res->getProperties().boardnameString <<
"IP" << res->getProperties().ipv4addrString <<
": cannot continue";
267 yDebug() <<
"embObjMais::open() correctly starts MC service of BOARD" << res->getProperties().boardnameString <<
"IP" << res->getProperties().ipv4addrString;
279 #if defined(EMBOBJMAIS_USESERVICEPARSER)
280 const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
282 const eOmn_serv_parameter_t* servparam = NULL;
287 if(
false == res->serviceVerifyActivate(eomn_serv_category_mais, servparam, 5.0))
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;
306 if(
false == sendConfig2Mais())
313 if(
false == initRegulars())
320 if(
false == res->serviceStart(eomn_serv_category_mais))
322 yError() <<
"embObjMais::open() fails to start as service for BOARD" << res->getProperties().boardnameString <<
"IP" << res->getProperties().ipv4addrString <<
": cannot continue";
330 yDebug() <<
"embObjMais::open() correctly starts as service of BOARD" << res->getProperties().boardnameString <<
"IP" << res->getProperties().ipv4addrString;
340 bool embObjMais::sendConfig2Mais(
void)
344 eOprotID32_t id32 = eo_prot_ID32dummy;
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);
351 if(
false == res->setcheckRemoteValue(id32, &datarate, 10, 0.010, 0.050))
353 yError() <<
"FATAL: embObjMais::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais datarate in BOARD" << res->getProperties().boardnameString <<
"with IP" << res->getProperties().ipv4addrString;
360 yDebug() <<
"embObjMais::sendConfig2Mais() correctly configured mais datarate at value" << datarate <<
"in BOARD" << res->getProperties().boardnameString <<
"with IP" << res->getProperties().ipv4addrString;
366 eOenum08_t maismode = eoas_maismode_txdatacontinuously;
367 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
369 if(
false == res->setcheckRemoteValue(id32, &maismode, 10, 0.010, 0.050))
371 yError() <<
"FATAL: embObjMais::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais mode in BOARD" << res->getProperties().boardnameString <<
"with IP" << res->getProperties().ipv4addrString;
378 yDebug() <<
"embObjMais::sendConfig2Mais() correctly configured mais mode at value" << maismode <<
"in BOARD" << res->getProperties().boardnameString <<
"with IP" << res->getProperties().ipv4addrString;
388 bool embObjMais::initRegulars()
392 vector<eOprotID32_t> id32v(0);
393 eOprotID32_t id32 = eo_prot_ID32dummy;
397 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_status_the15values);;
401 id32v.push_back(id32);
405 if(
false == res->serviceSetRegulars(eomn_serv_category_mais, id32v))
407 yError() <<
"embObjMais::initRegulars() fails to add its variables to regulars: cannot proceed any further";
414 yDebug() <<
"embObjMais::initRegulars() added" << id32v.size() <<
"regular rops to BOARD" << res->getProperties().boardnameString <<
"with IP" << res->getProperties().ipv4addrString;
416 for (
size_t r = 0; r<id32v.size(); r++)
418 uint32_t item = id32v.at(r);
419 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
420 yDebug() <<
"\t it added regular rop for" << nvinfo;
429 void embObjMais::resetCounters()
437 void embObjMais::getCounters(
unsigned int &
sat,
unsigned int &err,
unsigned int &to)
444 size_t embObjMais::getNrOfEncoderArrays()
const {
448 yarp::dev::MAS_status embObjMais::getEncoderArrayStatus(
size_t sens_index)
const {
449 if (sens_index >= 1)
return yarp::dev::MAS_UNKNOWN;
450 return yarp::dev::MAS_OK;
453 bool embObjMais::getEncoderArrayName(
size_t sens_index, std::string &name)
const {
454 if (sens_index >= 1)
return false;
455 name = serviceConfig.nameOfMais;
459 bool embObjMais::getEncoderArrayMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const {
460 if (sens_index >= 1)
return false;
461 timestamp = this->timeStamp;
462 out.resize(analogdata.size());
467 size_t embObjMais::getEncoderArraySize(
size_t sens_index)
const {
468 if (sens_index >= 1)
return 0;
469 return analogdata.size();
479 bool embObjMais::update(eOprotID32_t id32,
double timestamp,
void* rxdata)
482 this->timeStamp = timestamp;
495 EOarray *array = (EOarray*)rxdata;
496 uint8_t size = eo_array_Size(array);
497 uint8_t itemsize = eo_array_ItemSize(array);
498 if((0 == size) || (1 != itemsize))
503 std::lock_guard<std::mutex> lck(mtx);
505 for (
size_t k = 0; k<analogdata.size(); k++)
507 uint8_t*
tmp = (uint8_t*) eo_array_At(array, k);
512 analogdata[k] = (double)val;
522 bool embObjMais::close()
531 void embObjMais::printServiceConfig(
void)
537 const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : (
"NOT-ASSIGNED-YET");
538 const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : (
"NOT-ASSIGNED-YET");
540 parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc,
sizeof(loc));
541 parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir,
sizeof(fir));
542 parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro,
sizeof(pro));
544 yInfo() <<
"The embObjMais device using BOARD" << boardname <<
"w/ IP" << ipv4 <<
"has the following service config:";
545 yInfo() <<
"- acquisitionrate =" << serviceConfig.acquisitionrate;
546 yInfo() <<
"- MAIS named" << serviceConfig.nameOfMais <<
"@" << loc <<
"with required protocol version =" << pro <<
"and required firmware version =" << fir;
550 void embObjMais::cleanup(
void)
552 if(ethManager == NULL)
return;
554 int ret = ethManager->releaseResource2(res,
this);
557 ethManager->killYourself();
static TheEthManager * instance()
bool NOT_YET_IMPLEMENTED(const char *txt)
double sat(const double val, const double min, const double max)
Copyright (C) 2008 RobotCub Consortium.
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)