iCub-main
embObjInertials.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 <embObjInertials.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 #if defined(EMBOBJINERTIALS_PUBLISH_OLDSTYLE)
47 
48 typedef enum
49 {
50  eoas_inertial1_type_none = 0,
51  eoas_inertial1_type_accelerometer = 1,
52  eoas_inertial1_type_gyroscope = 2,
53  eoas_inertial1_type_emsgyro = 3
54 } eOas_inertial1_type_t;
55 
56 enum { eoas_inertial1_pos_max_numberof = 63 };
57 
58 enum { eoas_inertial1_pos_offsetleft = 0, eoas_inertial1_pos_offsetright = 24, eoas_inertial1_pos_offsetcentral = 48 };
59 
60 typedef enum
61 {
62  eoas_inertial1_pos_none = 0,
63 
64  // left arm
65  eoas_inertial1_pos_l_hand = 1+eoas_inertial1_pos_offsetleft, // label 1B7 canloc = (CAN2, 14)
66  eoas_inertial1_pos_l_forearm_1 = 2+eoas_inertial1_pos_offsetleft, // label 1B8 canloc = (CAN2, 12)
67  eoas_inertial1_pos_l_forearm_2 = 3+eoas_inertial1_pos_offsetleft, // label 1B9 canloc = (CAN2, 13)
68  eoas_inertial1_pos_l_upper_arm_1 = 4+eoas_inertial1_pos_offsetleft, // label 1B10 canloc = (CAN2, 9)
69  eoas_inertial1_pos_l_upper_arm_2 = 5+eoas_inertial1_pos_offsetleft, // label 1B11 canloc = (CAN2, 11)
70  eoas_inertial1_pos_l_upper_arm_3 = 6+eoas_inertial1_pos_offsetleft, // label 1B12 canloc = (CAN2, 10)
71  eoas_inertial1_pos_l_upper_arm_4 = 7+eoas_inertial1_pos_offsetleft, // label 1B13 canloc = (CAN2, 8)
72  // left leg
73  eoas_inertial1_pos_l_foot_1 = 8+eoas_inertial1_pos_offsetleft, // label 10B12 canloc = (CAN2, 13)
74  eoas_inertial1_pos_l_foot_2 = 9+eoas_inertial1_pos_offsetleft, // label 10B13 canloc = (CAN2, 12)
75  eoas_inertial1_pos_l_lower_leg_1 = 10+eoas_inertial1_pos_offsetleft, // label 10B8 canloc = (CAN2, 8)
76  eoas_inertial1_pos_l_lower_leg_2 = 11+eoas_inertial1_pos_offsetleft, // label 10B9 canloc = (CAN2, 9)
77  eoas_inertial1_pos_l_lower_leg_3 = 12+eoas_inertial1_pos_offsetleft, // label 10B10 canloc = (CAN2, 10)
78  eoas_inertial1_pos_l_lower_leg_4 = 13+eoas_inertial1_pos_offsetleft, // label 10B11 canloc = (CAN2, 11)
79  eoas_inertial1_pos_l_upper_leg_1 = 14+eoas_inertial1_pos_offsetleft, // label 10B1 canloc = (CAN1, 1)
80  eoas_inertial1_pos_l_upper_leg_2 = 15+eoas_inertial1_pos_offsetleft, // label 10B2 canloc = (CAN1, 2)
81  eoas_inertial1_pos_l_upper_leg_3 = 16+eoas_inertial1_pos_offsetleft, // label 10B3 canloc = (CAN1, 3)
82  eoas_inertial1_pos_l_upper_leg_4 = 17+eoas_inertial1_pos_offsetleft, // label 10B4 canloc = (CAN1, 4)
83  eoas_inertial1_pos_l_upper_leg_5 = 18+eoas_inertial1_pos_offsetleft, // label 10B5 canloc = (CAN1, 5)
84  eoas_inertial1_pos_l_upper_leg_6 = 19+eoas_inertial1_pos_offsetleft, // label 10B6 canloc = (CAN1, 6)
85  eoas_inertial1_pos_l_upper_leg_7 = 20+eoas_inertial1_pos_offsetleft, // label 10B7 canloc = (CAN1, 7)
86 
87  // right arm
88  eoas_inertial1_pos_r_hand = 1+eoas_inertial1_pos_offsetright, // label 2B7 canloc = (CAN2, 14)
89  eoas_inertial1_pos_r_forearm_1 = 2+eoas_inertial1_pos_offsetright, // label 2B8 canloc = (CAN2, 12)
90  eoas_inertial1_pos_r_forearm_2 = 3+eoas_inertial1_pos_offsetright, // label 2B9 canloc = (CAN2, 13)
91  eoas_inertial1_pos_r_upper_arm_1 = 4+eoas_inertial1_pos_offsetright, // label 2B10 canloc = (CAN2, 9)
92  eoas_inertial1_pos_r_upper_arm_2 = 5+eoas_inertial1_pos_offsetright, // label 2B11 canloc = (CAN2, 11)
93  eoas_inertial1_pos_r_upper_arm_3 = 6+eoas_inertial1_pos_offsetright, // label 2B12 canloc = (CAN2, 10)
94  eoas_inertial1_pos_r_upper_arm_4 = 7+eoas_inertial1_pos_offsetright, // label 2B13 canloc = (CAN2, 8)
95  // right leg
96  eoas_inertial1_pos_r_foot_1 = 8+eoas_inertial1_pos_offsetright, // label 11B12 canloc = (CAN2, 13)
97  eoas_inertial1_pos_r_foot_2 = 9+eoas_inertial1_pos_offsetright, // label 11B13 canloc = (CAN2, 12)
98  eoas_inertial1_pos_r_lower_leg_1 = 10+eoas_inertial1_pos_offsetright, // label 11B8 canloc = (CAN2, 8)
99  eoas_inertial1_pos_r_lower_leg_2 = 11+eoas_inertial1_pos_offsetright, // label 11B9 canloc = (CAN2, 9)
100  eoas_inertial1_pos_r_lower_leg_3 = 12+eoas_inertial1_pos_offsetright, // label 11B10 canloc = (CAN2, 10)
101  eoas_inertial1_pos_r_lower_leg_4 = 13+eoas_inertial1_pos_offsetright, // label 11B11 canloc = (CAN2, 11)
102  eoas_inertial1_pos_r_upper_leg_1 = 14+eoas_inertial1_pos_offsetright, // label 11B1 canloc = (CAN1, 1)
103  eoas_inertial1_pos_r_upper_leg_2 = 15+eoas_inertial1_pos_offsetright, // label 11B2 canloc = (CAN1, 2)
104  eoas_inertial1_pos_r_upper_leg_3 = 16+eoas_inertial1_pos_offsetright, // label 11B3 canloc = (CAN1, 3)
105  eoas_inertial1_pos_r_upper_leg_4 = 17+eoas_inertial1_pos_offsetright, // label 11B5 canloc = (CAN1, 5)
106  eoas_inertial1_pos_r_upper_leg_5 = 18+eoas_inertial1_pos_offsetright, // label 11B4 canloc = (CAN1, 4)
107  eoas_inertial1_pos_r_upper_leg_6 = 19+eoas_inertial1_pos_offsetright, // label 11B6 canloc = (CAN1, 6)
108  eoas_inertial1_pos_r_upper_leg_7 = 20+eoas_inertial1_pos_offsetright, // label 11B7 canloc = (CAN1, 7)
109 
110  // central parts
111  eoas_inertial1_pos_chest_1 = 1+eoas_inertial1_pos_offsetcentral, // 9B7
112  eoas_inertial1_pos_chest_2 = 2+eoas_inertial1_pos_offsetcentral, // 9B8
113  eoas_inertial1_pos_chest_3 = 3+eoas_inertial1_pos_offsetcentral, // 9B9
114  eoas_inertial1_pos_chest_4 = 4+eoas_inertial1_pos_offsetcentral, // 9B10
115 
116  eOas_inertial1_pos_jolly_1 = 60,
117  eOas_inertial1_pos_jolly_2 = 61,
118  eOas_inertial1_pos_jolly_3 = 62,
119  eOas_inertial1_pos_jolly_4 = 63
120 
121 } eOas_inertial1_position_t;
122 
123 const uint8_t fromip2indexof_thepositions[28] = { 0, 0, 1, 0, 2, 0, 0, 0, 0, 0, 3, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 0, 1, 0, 0, 2 };
124 
125 const eOas_inertial1_position_t thepositions[6][2][16] =
126 {
127  { // none
128  { // can1
129  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
130  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
131  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
132  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
133  },
134  { // can2
135  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
136  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
137  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
138  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
139  }
140  },
141  { // eb2
142  { // can1
143  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
144  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
145  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
146  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
147  },
148  { // can2
149  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
150  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
151  eoas_inertial1_pos_l_upper_arm_4, eoas_inertial1_pos_l_upper_arm_1, eoas_inertial1_pos_l_upper_arm_3, eoas_inertial1_pos_l_upper_arm_2,
152  eoas_inertial1_pos_l_forearm_1, eoas_inertial1_pos_l_forearm_2, eoas_inertial1_pos_l_hand, eoas_inertial1_pos_none
153  }
154  },
155  { // eb4
156  { // can1
157  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
158  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
159  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
160  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
161  },
162  { // can2
163  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
164  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
165  eoas_inertial1_pos_r_upper_arm_4, eoas_inertial1_pos_r_upper_arm_1, eoas_inertial1_pos_r_upper_arm_3, eoas_inertial1_pos_r_upper_arm_2,
166  eoas_inertial1_pos_r_forearm_1, eoas_inertial1_pos_r_forearm_2, eoas_inertial1_pos_r_hand, eoas_inertial1_pos_none
167  }
168  },
169  { // eb10
170  { // can1
171  eoas_inertial1_pos_none, eoas_inertial1_pos_l_upper_leg_1, eoas_inertial1_pos_l_upper_leg_2, eoas_inertial1_pos_l_upper_leg_3,
172  eoas_inertial1_pos_l_upper_leg_4, eoas_inertial1_pos_l_upper_leg_5, eoas_inertial1_pos_l_upper_leg_6, eoas_inertial1_pos_l_upper_leg_7,
173  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
174  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
175  },
176  { // can2
177  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
178  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
179  eoas_inertial1_pos_l_lower_leg_1, eoas_inertial1_pos_l_lower_leg_2, eoas_inertial1_pos_l_lower_leg_3, eoas_inertial1_pos_l_lower_leg_4,
180  eoas_inertial1_pos_l_foot_2, eoas_inertial1_pos_l_foot_1, eoas_inertial1_pos_none, eoas_inertial1_pos_none
181  }
182  },
183  { // eb11
184  { // can1
185  eoas_inertial1_pos_none, eoas_inertial1_pos_r_upper_leg_1, eoas_inertial1_pos_r_upper_leg_2, eoas_inertial1_pos_r_upper_leg_3,
186  eoas_inertial1_pos_r_upper_leg_5, eoas_inertial1_pos_r_upper_leg_4, eoas_inertial1_pos_r_upper_leg_6, eoas_inertial1_pos_r_upper_leg_7,
187  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
188  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
189  },
190  { // can2
191  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
192  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
193  eoas_inertial1_pos_r_lower_leg_1, eoas_inertial1_pos_r_lower_leg_2, eoas_inertial1_pos_r_lower_leg_3, eoas_inertial1_pos_r_lower_leg_4,
194  eoas_inertial1_pos_r_foot_2, eoas_inertial1_pos_r_foot_1, eoas_inertial1_pos_none, eoas_inertial1_pos_none
195  }
196  },
197  { // eb22
198  { // can1
199  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
200  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_chest_1,
201  eoas_inertial1_pos_chest_2, eoas_inertial1_pos_chest_3, eoas_inertial1_pos_chest_4, eoas_inertial1_pos_none,
202  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
203  },
204  { // can2
205  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
206  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
207  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none,
208  eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none, eoas_inertial1_pos_none
209  }
210  }
211 };
212 
213 #endif
214 
215 
216 
217 using namespace yarp;
218 using namespace yarp::os;
219 using namespace yarp::dev;
220 
221 
222 inline bool NOT_YET_IMPLEMENTED(const char *txt)
223 {
224  yWarning() << std::string(txt) << " not yet implemented for embObjInertials\n";
225  return false;
226 }
227 
228 
229 bool embObjInertials::fromConfig(yarp::os::Searchable &_config)
230 {
231 
232  if(false == parser->parseService(_config, serviceConfig))
233  {
234  return false;
235  }
236  //TODO prepare local data where cache the periodic sent ifo
237 
238 #if defined(EMBOBJINERTIALS_PUBLISH_OLDSTYLE)
239 
240  vector<eOas_inertial1_position_t> positions;
241  vector<eOas_inertial1_type_t> types;
242 
243  int size = serviceConfig.inertials.size();
244  positions.resize(size, eoas_inertial1_pos_none);
245  types.resize(size, eoas_inertial1_type_none);
246 
247 
248  for(int i=0; i<size; i++)
249  {
250  eOas_inertial1_type_t type = eoas_inertial1_type_none;
251  switch(serviceConfig.inertials.at(i).type)
252  {
253  case eoas_inertial_accel_mtb_int:
254  case eoas_inertial_accel_mtb_ext:
255  {
256  type = eoas_inertial1_type_accelerometer;
257  } break;
258 
259  case eoas_inertial_gyros_mtb_ext:
260  {
261  type = eoas_inertial1_type_gyroscope;
262  } break;
263 
264  case eoas_gyros_st_l3g4200d:
265  {
266  type = eoas_inertial1_type_emsgyro;
267  } break;
268 
269  default:
270  {
271  type = eoas_inertial1_type_none;
272  } break;
273  }
274 
275  types[i] = type;
276 
277  if(eoas_inertial1_type_emsgyro == type)
278  {
279  positions[i] = eOas_inertial1_pos_jolly_1;
280  }
281  else
282  {
283  // i decide to repeat the code in here because it must give error only for mtb-based positions.
284  uint8_t ip4 = 10;
285  eo_common_ipv4addr_to_decimal(ipv4addr, NULL, NULL, NULL, &ip4);
286  if(ip4 > sizeof(fromip2indexof_thepositions))
287  {
288  yError() << "embObjInertials::fromConfig() is using a non supported IP address:" << boardIPstring;
289  return false;
290  }
291  uint8_t index = fromip2indexof_thepositions[ip4];
292 
293 
294  eObrd_location_t on = serviceConfig.inertials.at(i).on;
295 
296  eOas_inertial1_position_t pos = eoas_inertial1_pos_none;
297  pos = thepositions[index][on.can.port][on.can.addr];
298 
299  positions[i] = pos;
300  }
301  }
302 
303  // prepare analogdata
304  {
305  // must be of size: 2+ inertials_Channels*erviceConfig.inertials.size(), and 0.0-initted
306  analogdata.resize(2 + inertials_Channels*serviceConfig.inertials.size(), -1.0);
307  // format is (n, 6) { (pos, type), (t, x, y, z) }_n
308  // now: (t, x, y, z) stay -1 if nothing comes up.
309  // the others must be initted now because they dont change
310  analogdata[0] = serviceConfig.inertials.size();
311  analogdata[1] = inertials_Channels;
312  for(int i=0; i<serviceConfig.inertials.size(); i++)
313  {
314  int indexpos = 2 + i*inertials_Channels;
315  int indextype = 2 + i*inertials_Channels + 1;
316  analogdata[indexpos] = positions[i];
317  analogdata[indextype] = types[i];
318  }
319 
320  }
321 
322 #else
323 
324  // prepare analogdata
325  {
326  // must be of size: inertials_Channels*erviceConfig.inertials.size(), and -1.0-initted
327  // format is { (t, x, y, z) }_n
328  // now: (t, x, y, z) stay -1 if nothing comes up.
329  analogdata.resize(inertials_Channels*serviceConfig.inertials.size(), -1.0);
330  }
331 
332 #endif
333 
334  return true;
335 }
336 
337 
338 embObjInertials::embObjInertials()
339 {
340  analogdata.resize(0);
341 
342  timeStamp = 0;
343  counterSat = 0;
344  counterError = 0;
345  counterTimeout = 0;
346 
347  status = IAnalogSensor::AS_OK;
348 
349  opened = false;
350 
351  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
352  if (tmp != "")
353  {
354  verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
355  }
356  else
357  {
358  verbosewhenok = false;
359  }
360 
361  parser = NULL;
362  res = NULL;
363  gyrSensors.resize(0);
364  accSensors.resize(0);
365 }
366 
367 
368 embObjInertials::~embObjInertials()
369 {
370  analogdata.resize(0);
371 
372  if(NULL != parser)
373  {
374  delete parser;
375  parser = NULL;
376  }
377 }
378 
379 bool embObjInertials::initialised()
380 {
381  return opened;
382 }
383 
384 bool embObjInertials::open(yarp::os::Searchable &config)
385 {
386  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
387 
388  ethManager = eth::TheEthManager::instance();
389  if(NULL == ethManager)
390  {
391  yFatal() << "embObjInertials::open() fails to instantiate ethManager";
392  return false;
393  }
394 
395 
396  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
397  {
398  yError() << "embObjInertials::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
399  return false;
400  }
401  // add specific info about this device ...
402 
403 
404  // - now all other things
405 
406  if(NULL == parser)
407  {
408  parser = new ServiceParser;
409  }
410 
411  // read stuff from config file
412  if(!fromConfig(config))
413  {
414  yError() << "embObjInertials missing some configuration parameter. Check logs and your config file.";
415  return false;
416  }
417 
418 
419  // -- instantiate EthResource etc.
420 
421  res = ethManager->requestResource2(this, config);
422  if(NULL == res)
423  {
424  yError() << "embObjInertials::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
425  return false;
426  }
427 
428  printServiceConfig();
429 
430 
431  if(!res->verifyEPprotocol(eoprot_endpoint_analogsensors))
432  {
433  cleanup();
434  return false;
435  }
436 
437  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
438 
439  if(false == res->serviceVerifyActivate(eomn_serv_category_inertials, servparam, 5.0))
440  {
441  yError() << "embObjInertials::open() has an error in call of ethResources::serviceVerifyActivate() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
442  printServiceConfig();
443  cleanup();
444  return false;
445  }
446 
447 
448  // configure the sensor(s)
449  for(int i=0; i<serviceConfig.inertials.size(); i++)
450  {
451  switch(serviceConfig.inertials[i].type)
452  {
453  case eoas_inertial_accel_mtb_int:
454  case eoas_inertial_accel_mtb_ext:
455  case eoas_inertial_accel_ems_st_lis3x:
456  {
457  accSensors.push_back(i);
458  }break;
459 
460 
461  case eoas_inertial_gyros_mtb_ext:
462  case eoas_inertial_gyros_ems_st_l3g4200d:
463  {
464  gyrSensors.push_back(i);
465 // yDebug() << "-------- I-" << res->getProperties().boardnameString << "num " << i << " is a GYRO";
466  }break;
467 
468  default:
469  //eoas_inertial_unknown
470  //eoas_inertial_none
471  break;
472 
473  };
474  }
475  if(false == sendConfig2MTBboards(config))
476  {
477  cleanup();
478  return false;
479  }
480 
481 
482  if(false == initRegulars())
483  {
484  cleanup();
485  return false;
486  }
487 
488 
489  if(false == res->serviceStart(eomn_serv_category_inertials))
490  {
491  yError() << "embObjInertials::open() fails to start as service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
492  cleanup();
493  return false;
494  }
495  else
496  {
497  if(verbosewhenok)
498  {
499  yDebug() << "embObjInertials::open() correctly starts service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
500  }
501  }
502 
503 
504  { // start the configured sensors. so far, we must keep it in here. later on we can remove this command
505 
506  uint8_t enable = 1;
507 
508  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_cmmnds_enable);
509  if(false == res->setRemoteValue(id32, &enable))
510  {
511  yError() << "embObjInertials::open() fails to command the start transmission of the configured inertials";
512  cleanup();
513  return false;
514  }
515  }
516 
517  opened = true;
518  return true;
519 }
520 
521 
522 
523 bool embObjInertials::sendConfig2MTBboards(Searchable& globalConfig)
524 {
525  eOas_inertial_config_t config = {0};
526  config.datarate = serviceConfig.acquisitionrate;
527  if(config.datarate > 200)
528  {
529  config.datarate = 200;
530  }
531  if(config.datarate < 10)
532  {
533  config.datarate = 10;
534  }
535  if(config.datarate != serviceConfig.acquisitionrate)
536  {
537  yWarning() << "embObjInertials::sendConfig2MTBboards() has detected a wrong acquisition rate =" << serviceConfig.acquisitionrate << "and clipped it to be" << config.datarate;
538  }
539 
540  config.enabled=0;
541  for(size_t i=0; i<serviceConfig.inertials.size(); i++)
542  {
543  eo_common_dword_bitset(&config.enabled, i);
544  }
545 
546  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_config);
547  if(false == res->setcheckRemoteValue(id32, &config, 10, 0.010, 0.050))
548  {
549  yError() << "FATAL: embObjInertials::sendConfig2MTBboards() had an error while calling setcheckRemoteValue() for config in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
550  return false;
551  }
552  else
553  {
554  if(verbosewhenok)
555  {
556  yDebug() << "embObjInertials::sendConfig2MTBboards() correctly configured enabled sensors with period" << serviceConfig.acquisitionrate << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
557  }
558  }
559 
560  return true;
561 }
562 
563 
564 
565 bool embObjInertials::initRegulars()
566 {
567  // configure regular rops
568 
569  vector<eOprotID32_t> id32v(0);
570  eOprotID32_t id32 = eo_prot_ID32dummy;
571 
572  // we need to choose the protoid to put inside the vector
573 
574  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_status);
575 
576  // put it inside vector
577 
578  id32v.push_back(id32);
579 
580 
581  if(false == res->serviceSetRegulars(eomn_serv_category_inertials, id32v))
582  {
583  yError() << "embObjInertials::initRegulars() fails to add its variables to regulars: cannot proceed any further";
584  return false;
585  }
586  else
587  {
588  if(verbosewhenok)
589  {
590  yDebug() << "embObjInertials::initRegulars() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
591  char nvinfo[128];
592  for (size_t r = 0; r<id32v.size(); r++)
593  {
594  uint32_t item = id32v.at(r);
595  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
596  yDebug() << "\t it added regular rop for" << nvinfo;
597  }
598  }
599  }
600 
601  return true;
602 }
603 
604 
609 int embObjInertials::read(yarp::sig::Vector &out)
610 {
611  // This method gives analogdata to the analogServer
612 
613  if(false == opened)
614  {
615  return false;
616  }
617 
618  std::lock_guard<std::mutex> lck(mtx);
619 
620  // errors are not handled for now... it'll always be OK!!
621  if (status != IAnalogSensor::AS_OK)
622  {
623  switch (status)
624  {
625  case IAnalogSensor::AS_OVF:
626  {
627  counterSat++;
628  } break;
629  case IAnalogSensor::AS_ERROR:
630  {
631  counterError++;
632  } break;
633  case IAnalogSensor::AS_TIMEOUT:
634  {
635  counterTimeout++;
636  } break;
637  default:
638  {
639  counterError++;
640  } break;
641  }
642  return status;
643  }
644 
645  out.resize(analogdata.size());
646  for (size_t k = 0; k<analogdata.size(); k++)
647  {
648  out[k] = analogdata[k];
649  }
650 
651  return status;
652 
653 }
654 
655 
656 void embObjInertials::resetCounters()
657 {
658  counterSat = 0;
659  counterError = 0;
660  counterTimeout = 0;
661 }
662 
663 
664 void embObjInertials::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
665 {
666  sat = counterSat;
667  err = counterError;
668  to = counterTimeout;
669 }
670 
671 
672 int embObjInertials::getState(int ch)
673 {
674  printf("getstate\n");
675  return AS_OK;
676 }
677 
678 int embObjInertials::getChannels()
679 {
680  return analogdata.size();
681 }
682 
683 
684 int embObjInertials::calibrateSensor()
685 {
686  return AS_OK;
687 }
688 
689 
690 int embObjInertials::calibrateSensor(const yarp::sig::Vector& value)
691 {
692  return AS_OK;
693 }
694 
695 int embObjInertials::calibrateChannel(int ch)
696 {
697  return AS_OK;
698 }
699 
700 
701 int embObjInertials::calibrateChannel(int ch, double v)
702 {
703  return AS_OK;
704 }
705 
706 
707 eth::iethresType_t embObjInertials::type()
708 {
710 }
711 
712 
713 bool embObjInertials::update(eOprotID32_t id32, double timestamp, void* rxdata)
714 {
715  id32 = id32;
716  timestamp = timestamp;
717  // timestamp is the time of reception inside EthReceiver, whereas status->data.timestamp is the time of the remote ETH board
718 
719  if(false == opened)
720  {
721  return false;
722  }
723 
724  eOas_inertial_status_t *status = (eOas_inertial_status_t*) rxdata;
725 
726  if(status->data.id >= serviceConfig.inertials.size())
727  { // we dont have any info to manage the received position ... or the remote board did not have to send up anything meaningful
728  return(true);
729  }
730 
731  std::lock_guard<std::mutex> lck(mtx);
732 
733 #if defined(EMBOBJINERTIALS_PUBLISH_OLDSTYLE)
734 
735  int firstpos = 2 + inertials_Channels*status->data.id;
736  // firstpos+0 is pos, firstpos+1 is type. they dont change over time
737 
738  analogdata[firstpos+2] = (double) status->data.timestamp;
739 
740  analogdata[firstpos+3] = (double) status->data.x;
741  analogdata[firstpos+4] = (double) status->data.y;
742  analogdata[firstpos+5] = (double) status->data.z;
743 
744 #else
745 
746  int firstpos = inertials_Channels*status->data.id;
747 
748  analogdata[firstpos+0] = (double) status->data.timestamp;
749 
750  analogdata[firstpos+1] = (double) status->data.x;
751  analogdata[firstpos+2] = (double) status->data.y;
752  analogdata[firstpos+3] = (double) status->data.z;
753 
754 #endif
755 
756  return true;
757 }
758 
759 
760 bool embObjInertials::close()
761 {
762  opened = false;
763 
764  cleanup();
765  return true;
766 }
767 
768 void embObjInertials::cleanup(void)
769 {
770  if(ethManager == NULL) return;
771 
772  int ret = ethManager->releaseResource2(res, this);
773  res = NULL;
774  if(ret == -1)
775  ethManager->killYourself();
776 }
777 
778 
779 void embObjInertials::printServiceConfig(void)
780 {
781  char loc[20] = {0};
782  char fir[20] = {0};
783  char pro[20] = {0};
784 
785  const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
786  const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
787 
788 
789  yInfo() << "The embObjInertials device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
790  yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
791  yInfo() << "- number of sensors =" << serviceConfig.inertials.size() << "defined as follows:";
792  for (size_t i = 0; i<serviceConfig.inertials.size(); i++)
793  {
794  eOas_inertial_descriptor_t des = serviceConfig.inertials.at(i);
795  string id = serviceConfig.id.at(i);
796  string strtype = string(eoas_sensor2string((eOas_sensor_t)des.type)); // from sensor type to string
797 
798  parser->convert(des.on, loc, sizeof(loc));
799  parser->convert(serviceConfig.ethservice.configuration.data.as.inertial.mtbversion.firmware, fir, sizeof(fir));
800  parser->convert(serviceConfig.ethservice.configuration.data.as.inertial.mtbversion.protocol, pro, sizeof(pro));
801 
802  yInfo() << " - id =" << id << "type =" << strtype << "on MTB w/ loc =" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
803  }
804 }
805 
806 size_t embObjInertials::getNrOfThreeAxisGyroscopes() const
807 {
808  return gyrSensors.size();
809 }
810 
811 yarp::dev::MAS_status embObjInertials::getThreeAxisGyroscopeStatus(size_t sens_index) const
812 {
813  return yarp::dev::MAS_OK;
814 }
815 bool embObjInertials::getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const
816 {
817  if(sens_index>= gyrSensors.size())
818  {
819  return false;
820  }
821 
822 
823  name = serviceConfig.id[gyrSensors[sens_index]];
824  return true;
825 
826 }
827 bool embObjInertials::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const
828 {
829  return getThreeAxisGyroscopeName(sens_index, frameName);
830 }
831 bool embObjInertials::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
832 {
833  if(false == opened)
834  {
835  return false;
836  }
837 
838  if(sens_index>= gyrSensors.size())
839  {
840  return false;
841  }
842 
843  std::lock_guard<std::mutex> lck(mtx);
844 
845  int firstpos = 2 + inertials_Channels*(gyrSensors[sens_index]);
846  timestamp = analogdata[firstpos+2];
847  out.resize(3);
848  out[0] = analogdata[firstpos+3];
849  out[1] = analogdata[firstpos+4];
850  out[2] = analogdata[firstpos+5];
851 
852  return true;
853 }
854 
855 /* IThreeAxisLinearAccelerometers methods */
856 size_t embObjInertials::getNrOfThreeAxisLinearAccelerometers() const
857 {
858  return accSensors.size();
859 }
860 yarp::dev::MAS_status embObjInertials::getThreeAxisLinearAccelerometerStatus(size_t sens_index) const
861 {
862  return yarp::dev::MAS_OK;
863 }
864 bool embObjInertials::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const
865 {
866  if(sens_index>= accSensors.size())
867  {
868  return false;
869  }
870 
871  name = serviceConfig.id[accSensors[sens_index]];
872  return true;
873 
874 }
875 
876 bool embObjInertials::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const
877 {
878  return getThreeAxisLinearAccelerometerName(sens_index, frameName);
879 
880 }
881 bool embObjInertials::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
882 {
883  if(false == opened)
884  {
885  return false;
886  }
887 
888  if(sens_index>= accSensors.size())
889  {
890  return false;
891  }
892 
893  std::lock_guard<std::mutex> lck(mtx);
894 
895  int firstpos = 2 + inertials_Channels*(accSensors[sens_index]);
896  timestamp = analogdata[firstpos+2];
897  out.resize(3);
898  out[0] = analogdata[firstpos+3];
899  out[1] = analogdata[firstpos+4];
900  out[2] = analogdata[firstpos+5];
901 
902  return true;
903 }
904 
905 
906 // eof
907 
static TheEthManager * instance()
Definition: ethManager.cpp:159
bool NOT_YET_IMPLEMENTED(const char *txt)
const dReal * pos
Definition: iCub_Sim.cpp:62
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_analoginertial
Definition: IethResource.h:69
Copyright (C) 2008 RobotCub Consortium.
grid on
Definition: show_eyes_axes.m:5
out
Definition: sine.m:8