iCub-main
3dm_gx3.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 iCub Facility, Istituto Italiano di Tecnologia
3  * Authors: Alberto Cardellino
4  * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
5  *
6  */
7 
8 #include <yarp/os/Bottle.h>
9 #include <yarp/os/Value.h>
10 #include <iostream>
11 #include <3dm_gx3.h>
12 
13 
14 
15 #include <fcntl.h> // File control definitions
16 #include <errno.h> // Error number definitions
17 
18 
19 using namespace yarp::dev;
20 using namespace yarp::os;
21 
22 
23 imu3DM_GX3::imu3DM_GX3() : PeriodicThread(0.006)
24 {
25  /* data to be transfered are:
26  1 (request Euler) +
27  19 (answer Euler) +
28  1 (acc, request gyro, magn) +
29  43 (answer) = 64 bytes
30  serial rate is 115.200 Kbits/s -> max rate is 181Hz - 5.52ms (if device support it... check)
31  choose 6ms as period to let device have a breath.
32  */
33 
34  // does nchannels has to be a parameter? I don't think so since ServerInertial has this fixed interface
35  nchannels = 12;
36 
37  // only CB and CE are used, but keep here some more definition in case of future use
38 
39  CB_cmd.cmd = CMD_ACCEL_ANGRATE_MAG;
40  CB_cmd.expSize = 43;
41  CB_cmd.process = process_CB;
42 
43  CE_cmd.cmd = CMD_EULER;
44  CE_cmd.expSize = 19;
45  CE_cmd.process = process_CE;
46 /*
47  C2_cmd.cmd = CMD_ACCEL_ANGRATE;
48  C2_cmd.expSize = 31;
49  C2_cmd.process = process_C2;
50 
51  C8_cmd.cmd = CMD_ACCEL_ANGRATE_ORIENT;
52  C8_cmd.expSize = 67;
53  C8_cmd.process = process_C8;
54 
55  CC_cmd.cmd = CMD_ACCEL_ANGRATE_MAG_ORIENT;
56  CC_cmd.expSize = 79;
57  CC_cmd.process = process_CC;
58 */
59 
60 
61  // if no continuous_mode is set ... imu worker thread cycle over these ....
62 
63  cmd_ptr_map[CMD_ACCEL_ANGRATE_MAG] = &CB_cmd;
64  cmd_ptr_map[CMD_EULER] = &CE_cmd;
65 };
66 
68 {
69  close();
70 };
71 
72 
73 // IGenericSensor interface.
74 bool imu3DM_GX3::open(yarp::os::Searchable &config)
75 {
76  bool ret = true;
77 
78  Value *baudrate, *serial;
79 
80  if(!config.check("serial", serial))
81  {
82  std::cout << "Can't find 'serial' name in config file";
83  return false;
84  }
85 
86  comPortName = serial->toString();
87 
88  int errNum = 0;
89  /* open serial */
90  printf("\n\nSerial opening %s\n\n\n", comPortName.c_str());
91  fd_ser = ::open(comPortName.c_str(), O_RDWR | O_NOCTTY );
92  if (fd_ser < 0) {
93  printf("can't open %s, %s\n", comPortName.c_str(), strerror(errno));
94  return false;
95  }
96 
97  //Get the current options for the port...
98  struct termios options;
99  tcgetattr(fd_ser, &options);
100 
101  //set the baud rate to 115200
102  int baudRate = B115200;
103  cfsetospeed(&options, baudRate);
104  cfsetispeed(&options, baudRate);
105 
106  //set the number of data bits.
107  options.c_cflag &= ~CSIZE; // Mask the character size bits
108  options.c_cflag |= CS8;
109 
110  //set the number of stop bits to 1
111  options.c_cflag &= ~CSTOPB;
112 
113  //Set parity to None
114  options.c_cflag &=~PARENB;
115 
116  //set for non-canonical (raw processing, no echo, etc.)
117  options.c_iflag = IGNPAR; // ignore parity check close_port(int
118  options.c_oflag = 0; // raw output
119  options.c_lflag = 0; // raw input
120 
121  //Time-Outs -- won't work with NDELAY option in the call to open
122  options.c_cc[VMIN] = 0; // block reading until RX x characers. If x = 0, it is non-blocking.
123  options.c_cc[VTIME] = 100; // Inter-Character Timer -- i.e. timeout= x*.1 s
124 
125  //Set local mode and enable the receiver
126  options.c_cflag |= (CLOCAL | CREAD);
127 
128  tcflush(fd_ser,TCIOFLUSH);
129 
130  //Set the new options for the port...
131  if ( tcsetattr(fd_ser, TCSANOW, &options) != 0)
132  { //For error message
133  printf("Configuring comport failed\n");
134  return false;
135  }
136  this->start();
137  return true;
138 }
139 
140 
142 {
143  int nbytes;
144  uint8_t reply[20];
145  uint8_t buff[20] =
146  {
148  0xA8, 0xB9, // confirm user intent
149  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
150  };
151 
152  nbytes = ::write(fd_ser, (void*)buff, 20);
153  nbytes = ::read(fd_ser, (void*)reply, 19);
154 
155  memcpy(buff+4, reply+1, 10);
156 
157  // Function selector : change parameter but do not send a treply
158  buff[3] = 3;
159  // Data conditioning function selector byte 7-8 : bit 12 enable quaternion ... remember big endian
160  buff[6] |= 16;
161 
162  //printHex(buff, 20);
163 
164  nbytes = write(fd_ser, (void*)buff, 20);
165 
166  usleep(100000);
167 }
168 
169 
171 {
172  this->stop();
173  if(fd_ser != 0)
174  ::close(fd_ser);
175  fd_ser = 0;
176  printf("Closed %s\n", comPortName.c_str());
177 
178  return true;
179 }
180 
181 
183 {
184  *nc = nchannels;
185  return true;
186 }
187 
188 
189 bool imu3DM_GX3::calibrate(int ch, double v)
190 {
191  // Send command to zero gyro bias?? Be careful it must be used only if the device is absolutely stationary!!
192  printf("Not implemented yet\n");
193  return false;
194 }
195 
196 
197 bool imu3DM_GX3::read(yarp::sig::Vector &out)
198 {
199  float tmp_eul[3];
200  float tmp_acc[3];
201  float tmp_gyro[3];
202  float tmp_mag[3];
203 
204  uint64_t imu_timeStamp;
205 
206  get_Euler(tmp_eul, &imu_timeStamp);
207  get_Acc_Ang_Mag(tmp_acc, tmp_gyro, tmp_mag, &imu_timeStamp);
208 
209  int out_idx = 0;
210  for(int i=0; i<3; i++, out_idx++)
211  out[out_idx] = (double) tmp_eul[i];
212 
213  for(int i=0; i<3; i++, out_idx++)
214  out[out_idx] = (double) tmp_acc[i];
215 
216  for(int i=0; i<3; i++, out_idx++)
217  out[out_idx] = (double) tmp_gyro[i];
218 
219  for(int i=0; i<3; i++, out_idx++)
220  out[out_idx] = (double) tmp_mag[i];
221 
222  lastStamp.update();
223  return true;
224 }
225 
227 {
228  return lastStamp;
229 }
230 
232 // Thread //
234 
235 bool imu3DM_GX3::threadInit()
236 {
237  // if board was configured to automatically send data on wakeup, this will make it silent
238  stop_continuous();
239  sample_setting();
240  printf("Started imu-3DM-GX3-25 thread\n");
241  return true;
242 }
243 
244 void imu3DM_GX3::run()
245 {
246  int nbytes;
247  static data_3DM_GX3_t th_data;
248  imu_cmd_t * tmp_cmd = NULL;
249 
250  // ask data to device in polling, cycling within the map
251  cmd_map_t::iterator it = cmd_ptr_map.begin();
252  while(it != cmd_ptr_map.end())
253  {
254  tmp_cmd = it->second;
255  // transmit command
256  nbytes = ::write(fd_ser, (void*)&tmp_cmd->cmd, sizeof(uint8_t));
257 
258  // receive response
259  nbytes = ::read(fd_ser, (void*)&th_data.buffer, (size_t)tmp_cmd->expSize);
260 
261  if ( nbytes < 0 )
262  {
263  printf("Error while reading imu response to command XXX\n");
264  // skip parsing and try with the next command
265  }
266  else if ( nbytes != tmp_cmd->expSize )
267  {
268  printf("read %d instead of %d\n", nbytes, tmp_cmd->expSize);
269  // skip parsing and try with the next command
270  }
271  else
272  {
273  // process data ... at least swap bytes !!!
274  if ( tmp_cmd->process ) {
275  // compute checksum and swap bytes
276  if ( ! (bool)tmp_cmd->process(th_data) )
277  {
278  printf("Checksum FAILURE\n");
279  continue;
280  }
281  }
282 
283  // copy for consumer ....
284  lock_guard<mutex> lck(data_mutex);
285  memcpy((void*)&tmp_cmd->data.buffer, &th_data.buffer, tmp_cmd->expSize);
286  }
287  it++;
288  }
289 }
290 
291 void imu3DM_GX3::threadRelease()
292 {
293  stop_continuous();
294 }
295 
297 // Device specific //
299 
300 void imu3DM_GX3::stop_continuous(void)
301 {
302 
303  int nbytes;
304  const uint8_t buff[] = {
306  0x75, 0xB4 // confirms user intent
307  };
308 
309  nbytes = write(fd_ser, (void*)buff, sizeof(buff));
310  // !!! need to clean serial buffer ...
311  sleep(1);
312  flush();
313 }
314 
315 void imu3DM_GX3::get_Acc_Ang(float acc[3], float angRate[3], uint64_t *time) {
316 
317  acc_angRate_t * aa;
318 
319  lock_guard<mutex> lck(data_mutex);
320  aa = &C2_cmd.data.aa;
321  if (acc) {
322  memcpy((void*)acc, aa->acc._bytes, sizeof(float)*3);
323  }
324  if (angRate) {
325  memcpy((void*)angRate, aa->angRate._bytes, sizeof(float)*3);
326  }
327 }
328 
329 void imu3DM_GX3::get_Acc_Ang_Orient(float acc[3], float angRate[3], float orientMat[9], uint64_t *time) {
330 
331  acc_ang_orient_t * aaom;
332 
333  lock_guard<mutex> lck(data_mutex);
334  aaom = &C8_cmd.data.aaom;
335  if (acc) {
336  memcpy((void*)acc, aaom->acc._bytes, sizeof(float)*3);
337  }
338  if (angRate) {
339  memcpy((void*)angRate, aaom->angRate._bytes, sizeof(float)*3);
340  }
341  if (orientMat) {
342  memcpy((void*)orientMat, aaom->orientMat._bytes, sizeof(float)*9);
343  }
344 }
345 
346 void imu3DM_GX3::get_Acc_Ang_Mag(float acc[3], float angRate[3], float mag[3], uint64_t *time) {
347  acc_ang_mag_t * aam;
348 
349  lock_guard<mutex> lck(data_mutex);
350  aam = &CB_cmd.data.aam;
351  if (acc) {
352  memcpy((void*)acc, aam->acc._bytes, sizeof(float)*3);
353  }
354  if (angRate) {
355  memcpy((void*)angRate, aam->angRate._bytes, sizeof(float)*3);
356  }
357  if (mag) {
358  memcpy((void*)mag, aam->mag._bytes, sizeof(float)*3);
359  }
360 }
361 
362 void imu3DM_GX3::get_Acc_Ang_Mag_Orient(float acc[3], float angRate[3], float mag[3], float orientMat[9], uint64_t *time) {
363 
364  acc_ang_mag_orient_t * aamom;
365 
366  lock_guard<mutex> lck(data_mutex);
367  aamom = &CC_cmd.data.aamom;
368  if (acc) {
369  memcpy((void*)acc, aamom->acc._bytes, sizeof(float)*3);
370  }
371  if (angRate) {
372  memcpy((void*)angRate, aamom->angRate._bytes, sizeof(float)*3);
373  }
374  if (mag) {
375  memcpy((void*)mag, aamom->mag._bytes, sizeof(float)*3);
376  }
377  if (orientMat) {
378  memcpy((void*)orientMat, aamom->orientMat._bytes, sizeof(float)*9);
379  }
380 }
381 
382 void imu3DM_GX3::get_Euler(float euler[3], uint64_t *time) {
383 
384  eul_t * eu;
385 
386  lock_guard<mutex> lck(data_mutex);
387  eu = &CE_cmd.data.eu;
388  if (euler) {
389  memcpy((void*)euler, eu->eul._bytes, sizeof(float)*3);
390  }
391 }
392 
393 
394 void imu3DM_GX3::get_Euler_AngularRate(float euler[3], float angRate[3], uint64_t *time) {
395 
396  eul_angRate_t * ea;
397 
398  lock_guard<mutex> lck(data_mutex);
399  ea = &CE_cmd.data.ea;
400  if (euler) {
401  memcpy((void*)euler, ea->eul._bytes, sizeof(float)*3);
402  }
403  if (angRate) {
404  memcpy((void*)angRate, ea->angRate._bytes, sizeof(float)*3);
405  }
406 }
407 
408 void imu3DM_GX3::get_Quaternion(float quat[4], uint64_t *time) {
409 
410  quat_t * q;
411 
412  lock_guard<mutex> lck(data_mutex);
413  q = &DF_cmd.data.quat;
414  if (quat) {
415  memcpy((void*)quat, q->quat._bytes, sizeof(float)*4);
416  }
417 }
virtual bool calibrate(int ch, double v)
Definition: 3dm_gx3.cpp:189
virtual bool getChannels(int *nc)
Definition: 3dm_gx3.cpp:182
void sample_setting(void)
Definition: 3dm_gx3.cpp:141
virtual yarp::os::Stamp getLastInputStamp()
Definition: 3dm_gx3.cpp:226
virtual bool close()
Definition: 3dm_gx3.cpp:170
virtual bool open(yarp::os::Searchable &config)
Definition: 3dm_gx3.cpp:74
virtual bool read(yarp::sig::Vector &out)
Definition: 3dm_gx3.cpp:197
_3f_vect_t mag
Definition: dataTypes.h:3
@ CMD_EULER
Definition: dataTypes.h:44
@ CMD_ACCEL_ANGRATE_MAG
Definition: dataTypes.h:41
@ CMD_SAMPLING_SETTING
Definition: dataTypes.h:49
@ CMD_STOP_CONTINUOUS
Definition: dataTypes.h:52
void * process_CB(data_3DM_GX3_t &data)
Definition: dataTypes.h:285
_4f_vect_t quat
Definition: dataTypes.h:1
_3f_matx_t orientMat
Definition: dataTypes.h:1
_3f_vect_t angRate
Definition: dataTypes.h:2
void * process_CE(data_3DM_GX3_t &data)
Definition: dataTypes.h:362
_3f_vect_t acc
Definition: dataTypes.h:1
static int stop
Definition: iCub_Sim.cpp:41
static int v
Definition: iCub_Sim.cpp:42
bool write(const std::string filename, const FullRegulation &reg)
out
Definition: sine.m:8
degrees time
Definition: sine.m:5
Definition: dataTypes.h:98
_3f_vect_t acc
Definition: dataTypes.h:100
_3f_vect_t angRate
Definition: dataTypes.h:101
_3f_matx_t orientMat
Definition: dataTypes.h:123
_3f_vect_t acc
Definition: dataTypes.h:121
_3f_vect_t angRate
Definition: dataTypes.h:122
_3f_vect_t mag
Definition: dataTypes.h:135
_3f_vect_t angRate
Definition: dataTypes.h:134
_3f_vect_t acc
Definition: dataTypes.h:133
_3f_matx_t orientMat
Definition: dataTypes.h:148
_3f_vect_t mag
Definition: dataTypes.h:147
_3f_vect_t angRate
Definition: dataTypes.h:146
_3f_vect_t acc
Definition: dataTypes.h:145
_3f_vect_t eul
Definition: dataTypes.h:158
_3f_vect_t eul
Definition: dataTypes.h:168
_3f_vect_t angRate
Definition: dataTypes.h:169
_4f_vect_t quat
Definition: dataTypes.h:179
data_3DM_GX3_t data
Definition: dataTypes.h:204
funptr_t process
Definition: dataTypes.h:205
uint8_t cmd
Definition: dataTypes.h:202
uint8_t expSize
Definition: dataTypes.h:203
uint32_t _bytes[9]
Definition: dataTypes.h:88
uint32_t _bytes[3]
Definition: dataTypes.h:61
uint32_t _bytes[4]
Definition: dataTypes.h:72
acc_ang_mag_orient_t aamom
Definition: dataTypes.h:189
acc_angRate_t aa
Definition: dataTypes.h:187
eul_angRate_t ea
Definition: dataTypes.h:192
acc_ang_orient_t aaom
Definition: dataTypes.h:188
uint8_t buffer[80]
Definition: dataTypes.h:195
acc_ang_mag_t aam
Definition: dataTypes.h:190