15#include <yarp/os/Time.h>
16#include <yarp/os/Log.h>
17#include <yarp/os/LogStream.h>
20#include <yarp/dev/PolyDriver.h>
21#include <ace/config.h>
22#include <ace/Log_Msg.h>
28#include <yarp/os/LogStream.h>
29#include "EoAnalogSensors.h"
32#include "EoProtocol.h"
33#include "EoProtocolMN.h"
34#include "EoProtocolAS.h"
36#include <yarp/os/NetType.h>
39#pragma warning(once:4355)
45using namespace yarp::os;
50bool embObjPOS::fromConfig(yarp::os::Searchable &config,
servConfigPOS_t &serviceConfig)
55 yError() << m_PDdevice.
getBoardInfo() <<
": missing some configuration parameter. Check logs and your config file.";
76 return m_PDdevice.
isOpen();
84 yInfo() <<
"embObjPOS::open(): preparing ETH resource";
89 yInfo() <<
"embObjPOS::open(): browsing xml files which describe the service";
93 if(!fromConfig(config, serviceConfig))
95 yError() <<
"embObjPOS missing some configuration parameter. Check logs and your config file.";
101 m_data.resize(eOas_pos_data_maxnumber, 0.0);
105 yInfo() <<
"embObjPOS::open(): verify the presence of the board and if its protocol version is correct";
115 yInfo() <<
"embObjPOS::open(): verify and activate the POS service";
117 const eOmn_serv_parameter_t* servparam = &serviceConfig.
ethservice;
121 yError() << m_PDdevice.
getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate() ";
130 yInfo() <<
"embObjPOS::open(): configure the POS service";
132 if(
false == sendConfig2boards(serviceConfig))
138 yInfo() <<
"embObjPOS::open(): impose the network variable which the ETH bord must stream up";
141 if(
false == initRegulars())
148 yInfo() <<
"embObjPOS::open(): start the POS service";
152 yError() << m_PDdevice.
getBoardInfo() <<
"open() fails to start as service.... cannot continue";
160 yDebug() << m_PDdevice.
getBoardInfo() <<
"open() correctly starts service";
164 yInfo() <<
"embObjPOS::open(): start streaming of POS data";
176 eOprotID32_t id32 = eo_prot_ID32dummy;
178 eOas_pos_config_t cfg;
180 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_config);
184 yError() << m_PDdevice.
getBoardInfo() <<
"FATAL error in sendConfig2boards() while try to configure datarate=" << cfg.datarate;
190 yDebug() << m_PDdevice.
getBoardInfo() <<
": sendConfig2boards() correctly configured boards with datarate=" << cfg.datarate;
197bool embObjPOS::sendStart2boards(
void)
199 eOprotID32_t id32 = eo_prot_ID32dummy;
203 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_cmmnds_enable);
207 yError() << m_PDdevice.
getBoardInfo() <<
"FATAL error in sendStart2boards() while try to enable the boards transmission";
213 yDebug() << m_PDdevice.
getBoardInfo() <<
": sendStart2boards() correctly enabled the boards transmission";
221bool embObjPOS::initRegulars(
void)
225 vector<eOprotID32_t> id32v(0);
226 eOprotID32_t id32 = eo_prot_ID32dummy;
230 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_status);;
234 id32v.push_back(id32);
240 yError() << m_PDdevice.
getBoardInfo() <<
"initRegulars() fails to add its variables to regulars: cannot proceed any further";
246 yDebug() << m_PDdevice.
getBoardInfo() <<
"initRegulars() added" << id32v.size() <<
"regular rops ";
248 for (
size_t r = 0; r<id32v.size(); r++)
250 uint32_t item = id32v.at(r);
251 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
252 yDebug() <<
"\t it added regular rop for" << nvinfo;
273 std::lock_guard<std::mutex> lock(m_mutex);
285 printf(
"getstate\n");
292 return static_cast<int>(eOas_pos_data_maxnumber);
333 eOas_pos_status_t *pos_st_ptr = (eOas_pos_status_t*)rxdata;
334 uint32_t sizeOfData = pos_st_ptr->arrayofdata.head.size;
336 if(sizeOfData>m_data.size())
337 yWarning() << m_PDdevice.
getBoardInfo() <<
"In update function I received more data than I had been configured to store.My size is " << m_data.size() <<
"while I received " << sizeOfData <<
"values!";
339 std::lock_guard<std::mutex> lock(m_mutex);
341 for(uint32_t i=0; i<sizeOfData; i++)
343 eOas_pos_data_t posdata = pos_st_ptr->arrayofdata.data[i];
345 m_data[i]= posdata.value*0.1;
380void embObjPOS::cleanup(
void)
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=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
virtual bool isOpen() const
bool prerareEthService(yarp::os::Searchable &config, eth::IethResource *interface)
void cleanup(eth::IethResource *interface)
std::string getBoardInfo(void) const
eth::AbstractEthResource * res
virtual int calibrateSensor()
virtual int getChannels()
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual int read(yarp::sig::Vector &out)
Read a vector from the sensor.
virtual eth::iethresType_t type()
virtual bool initialised()
virtual int getState(int ch)
bool open(yarp::os::Searchable &config)
virtual int calibrateChannel(int ch, double v)
Copyright (C) 2008 RobotCub Consortium.
eOmn_serv_parameter_t ethservice