iCub-main
Loading...
Searching...
No Matches
embObjPOS.cpp
Go to the documentation of this file.
1
2/*
3 * Copyright (C) 2020 iCub Tech - Istituto Italiano di Tecnologia
4 * Author: Marco Accame
5 * email: marco.accame@iit.it
6*/
7
8
9// general purpose stuff.
10#include <string>
11#include <iostream>
12#include <string.h>
13
14// Yarp Includes
15#include <yarp/os/Time.h>
16#include <yarp/os/Log.h>
17#include <yarp/os/LogStream.h>
18#include <stdarg.h>
19#include <stdio.h>
20#include <yarp/dev/PolyDriver.h>
21#include <ace/config.h>
22#include <ace/Log_Msg.h>
23
24
25// specific to this device driver.
26#include <embObjPOS.h>
27#include <ethManager.h>
28#include <yarp/os/LogStream.h>
29#include "EoAnalogSensors.h"
30#include "EOnv_hid.h"
31
32#include "EoProtocol.h"
33#include "EoProtocolMN.h"
34#include "EoProtocolAS.h"
35
36#include <yarp/os/NetType.h>
37
38#ifdef WIN32
39#pragma warning(once:4355)
40#endif
41
42
43
44using namespace yarp;
45using namespace yarp::os;
46using namespace yarp::dev;
47
48
49
50bool embObjPOS::fromConfig(yarp::os::Searchable &config, servConfigPOS_t &serviceConfig)
51{
52 ServiceParser parser;
53 if(false == parser.parseService(config, serviceConfig))
54 {
55 yError() << m_PDdevice.getBoardInfo() << ": missing some configuration parameter. Check logs and your config file.";
56 return false;
57 }
58
59 return true;
60}
61
62
63embObjPOS::embObjPOS(): m_PDdevice("embObjPOS")
64{
65}
66
67
72
73
75{
76 return m_PDdevice.isOpen();
77}
78
79
80bool embObjPOS::open(yarp::os::Searchable &config)
81{
82 // 1) prepare eth service verifing if the eth manager is available and parsing info about the eth board.
83
84 yInfo() << "embObjPOS::open(): preparing ETH resource";
85
86 if(! m_PDdevice.prerareEthService(config, this))
87 return false;
88
89 yInfo() << "embObjPOS::open(): browsing xml files which describe the service";
90
91 // 2) read stuff from config file
92 servConfigPOS_t serviceConfig;
93 if(!fromConfig(config, serviceConfig))
94 {
95 yError() << "embObjPOS missing some configuration parameter. Check logs and your config file.";
96 return false;
97 }
98
99 // 3) prepare data vector
100 {
101 m_data.resize(eOas_pos_data_maxnumber, 0.0);
102 }
103
104
105 yInfo() << "embObjPOS::open(): verify the presence of the board and if its protocol version is correct";
106
107 // 4) verify analog sensor protocol and then verify-Activate the POS service
108 if(!m_PDdevice.res->verifyEPprotocol(eoprot_endpoint_analogsensors))
109 {
110 cleanup();
111 return false;
112 }
113
114
115 yInfo() << "embObjPOS::open(): verify and activate the POS service";
116
117 const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
118
119 if(!m_PDdevice.res->serviceVerifyActivate(eomn_serv_category_pos, servparam, 5.0))
120 {
121 yError() << m_PDdevice.getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate() ";
122 cleanup();
123 return false;
124 }
125
126
127 //printServiceConfig();
128
129
130 yInfo() << "embObjPOS::open(): configure the POS service";
131
132 if(false == sendConfig2boards(serviceConfig))
133 {
134 cleanup();
135 return false;
136 }
137
138 yInfo() << "embObjPOS::open(): impose the network variable which the ETH bord must stream up";
139
140 // Set variable to be signaled
141 if(false == initRegulars())
142 {
143 cleanup();
144 return false;
145 }
146
147
148 yInfo() << "embObjPOS::open(): start the POS service";
149
150 if(!m_PDdevice.res->serviceStart(eomn_serv_category_pos))
151 {
152 yError() << m_PDdevice.getBoardInfo() << "open() fails to start as service.... cannot continue";
153 cleanup();
154 return false;
155 }
156 else
157 {
158 if(m_PDdevice.isVerbose())
159 {
160 yDebug() << m_PDdevice.getBoardInfo() << "open() correctly starts service";
161 }
162 }
163
164 yInfo() << "embObjPOS::open(): start streaming of POS data";
165
166 sendStart2boards();
167
168 m_PDdevice.setOpen(true);
169
170 return true;
171}
172
173
174bool embObjPOS::sendConfig2boards(servConfigPOS_t &serviceConfig)
175{
176 eOprotID32_t id32 = eo_prot_ID32dummy;
177
178 eOas_pos_config_t cfg;
179 cfg.datarate = serviceConfig.acquisitionrate;
180 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_config);
181
182 if(false == m_PDdevice.res->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
183 {
184 yError() << m_PDdevice.getBoardInfo() << "FATAL error in sendConfig2boards() while try to configure datarate=" << cfg.datarate;
185 return false;
186 }
187
188 if(m_PDdevice.isVerbose())
189 {
190 yDebug() << m_PDdevice.getBoardInfo() << ": sendConfig2boards() correctly configured boards with datarate=" << cfg.datarate;
191 }
192
193 return true;
194
195}
196
197bool embObjPOS::sendStart2boards(void)
198{
199 eOprotID32_t id32 = eo_prot_ID32dummy;
200
201 uint8_t enable=1;
202
203 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_cmmnds_enable);
204
205 if(false == m_PDdevice.res->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
206 {
207 yError() << m_PDdevice.getBoardInfo() << "FATAL error in sendStart2boards() while try to enable the boards transmission";
208 return false;
209 }
210
211 if(m_PDdevice.isVerbose())
212 {
213 yDebug() << m_PDdevice.getBoardInfo() << ": sendStart2boards() correctly enabled the boards transmission";
214 }
215
216 return true;
217
218}
219
220
221bool embObjPOS::initRegulars(void)
222{
223 // configure regular rops
224
225 vector<eOprotID32_t> id32v(0);
226 eOprotID32_t id32 = eo_prot_ID32dummy;
227
228 // we need to choose the id32 to put inside the vector
229
230 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_pos, 0, eoprot_tag_as_pos_status);;
231
232 // and put it inside vector
233
234 id32v.push_back(id32);
235
236 // now we send the vector
237
238 if(false == m_PDdevice.res->serviceSetRegulars(eomn_serv_category_pos, id32v))
239 {
240 yError() << m_PDdevice.getBoardInfo() << "initRegulars() fails to add its variables to regulars: cannot proceed any further";
241 return false;
242 }
243
244 if(m_PDdevice.isVerbose())
245 {
246 yDebug() << m_PDdevice.getBoardInfo() << "initRegulars() added" << id32v.size() << "regular rops ";
247 char nvinfo[128];
248 for (size_t r = 0; r<id32v.size(); r++)
249 {
250 uint32_t item = id32v.at(r);
251 eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
252 yDebug() << "\t it added regular rop for" << nvinfo;
253 }
254 }
255
256
257 return true;
258}
259
260
266int embObjPOS::read(yarp::sig::Vector &out)
267{
268 // This method gives the data, received from embedded boards, to the analogServer
269
270 if(!m_PDdevice.isOpen())
271 return AS_ERROR ;
272
273 std::lock_guard<std::mutex> lock(m_mutex);
274
275 // errors are not handled for now... it'll always be OK!!
276 out=m_data;
277
278 return AS_OK;
279}
280
281
282
284{
285 printf("getstate\n");
286 return AS_OK;
287}
288
289
291{
292 return static_cast<int>(eOas_pos_data_maxnumber);
293}
294
295
297{
298 return AS_OK;
299}
300
301
302int embObjPOS::calibrateSensor(const yarp::sig::Vector& value)
303{
304 return AS_OK;
305}
306
307
309{
310 return AS_OK;
311}
312
313
314int embObjPOS::calibrateChannel(int ch, double v)
315{
316 return AS_OK;
317}
318
319
324
325
326bool embObjPOS::update(eOprotID32_t id32, double timestamp, void* rxdata)
327{
328 // called by feat_manage_analogsensors_data() which is called by:
329 // eoprot_fun_UPDT_as_pos_status
330 if(!m_PDdevice.isOpen())
331 return false;
332
333 eOas_pos_status_t *pos_st_ptr = (eOas_pos_status_t*)rxdata;
334 uint32_t sizeOfData = pos_st_ptr->arrayofdata.head.size;
335
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!";
338
339 std::lock_guard<std::mutex> lock(m_mutex);
340
341 for(uint32_t i=0; i<sizeOfData; i++)
342 {
343 eOas_pos_data_t posdata = pos_st_ptr->arrayofdata.data[i];
344 //pos value is in deci-degree
345 m_data[i]= posdata.value*0.1;
346 }
347
348 return true;
349}
350
351
352
353
355{
356 cleanup();
357 return true;
358}
359
360
361// void embObjPOS::printServiceConfig(void)
362// {
363// char loc[20] = {0};
364// char fir[20] = {0};
365// char pro[20] = {0};
366//
367// const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
368// const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
369//
370// parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc, sizeof(loc));
371// parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir, sizeof(fir));
372// parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro, sizeof(pro));
373//
374// yInfo() << "The embObjPOS device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
375// yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
376// yInfo() << "- MAIS named" << serviceConfig.nameOfMais << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
377// }
378
379
380void embObjPOS::cleanup(void)
381{
382 m_PDdevice.cleanup(static_cast <eth::IethResource*> (this));
383}
384
385// eof
386
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
bool prerareEthService(yarp::os::Searchable &config, eth::IethResource *interface)
void cleanup(eth::IethResource *interface)
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()
Definition embObjPOS.cpp:74
virtual int getState(int ch)
bool open(yarp::os::Searchable &config)
Definition embObjPOS.cpp:80
virtual int calibrateChannel(int ch, double v)
iethresType_t
@ iethres_analogpos
Copyright (C) 2008 RobotCub Consortium.
out
Definition sine.m:8
eOmn_serv_parameter_t ethservice