iCub-main
ST_M1.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 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 <iostream>
9 #include <fcntl.h> // File control definitions
10 #include <errno.h> // Error number definitions
11 
12 #include <yarp/os/Time.h>
13 #include <yarp/os/Bottle.h>
14 #include <yarp/os/Value.h>
15 #include <yarp/os/LogStream.h>
16 #include <ST_M1.h>
17 
18 using namespace yarp::dev;
19 using namespace yarp::os;
20 
21 
22 imuST_M1::imuST_M1() : PeriodicThread(0.006)
23 {
24  verbose = false;
25  nchannels = 12;
26  opened = false;
27  progressiv_num = -1;
28  temp_euler = &temp_data[0];
29  temp_acc = &temp_data[3];
30  temp_gyro = &temp_data[6];
31  temp_mag = &temp_data[9];
32 }
33 
35 {
36  close();
37 };
38 
39 
40 // IGenericSensor interface.
41 bool imuST_M1::open(yarp::os::Searchable &config)
42 {
43  yTrace();
44  bool ret = true;
45 
46  Value *baudrate, *serial;
47 
48  if(!config.check("serial", serial))
49  {
50  std::cout << "Can't find 'serial' name in config file";
51  return false;
52  }
53 
54  if(config.check("verbose"))
55  {
56  verbose = true;
57  }
58 
59  comPortName = serial->toString();
60 
61  int errNum = 0;
62  /* open serial */
63  printf("\n\nSerial opening %s\n\n\n", comPortName.c_str());
64  fd_ser = ::open(comPortName.c_str(), O_RDWR | O_NOCTTY );
65  if (fd_ser < 0) {
66  printf("can't open %s, %s\n", comPortName.c_str(), strerror(errno));
67  return false;
68  }
69 
70  //Get the current options for the port...
71  struct termios options;
72  tcgetattr(fd_ser, &options);
73 //
74 // //set the baud rate to 115200
75  int baudRate = B115200;
76  cfsetospeed(&options, baudRate);
77  cfsetispeed(&options, baudRate);
78 
79  //set the number of data bits.
80 // options.c_cflag &= ~CSIZE; // Mask the character size bits
81 // options.c_cflag |= CS8;
82 //
83 // //set the number of stop bits to 1
84 // options.c_cflag &= ~CSTOPB;
85 //
86 // //Set parity to None
87 // options.c_cflag &=~PARENB;
88 
89  //set for non-canonical (raw processing, no echo, etc.)
90  options.c_iflag = IGNPAR; // ignore parity check
91  options.c_oflag = 0; // raw output
92  options.c_lflag = 0; // raw input
93 
94  //Time-Outs -- won't work with NDELAY option in the call to open
95  options.c_cc[VMIN] = 53; // block reading until RX x characers. If x = 0, it is non-blocking.
96  options.c_cc[VTIME] = 10; // Inter-Character Timer -- i.e. timeout= x*.1 s
97 
98  //Set local mode and enable the receiver
99  options.c_cflag |= (CLOCAL | CREAD);
100 
101  tcflush(fd_ser,TCIOFLUSH);
102 
103  //Set the new options for the port...
104  if ( tcsetattr(fd_ser, TCSANOW, &options) != 0)
105  { //For error message
106  printf("Configuring comport failed\n");
107  return false;
108  }
109 
110  opened = true;
111  char buff[20];
112  buff[0] = 0x20;
113  buff[1] = 0x01;
114  buff[2] = 0x00;
115  int nbytes_w = ::write(fd_ser, (void*)buff, 3);
116 
117  memset(buff, 0x00, 20);
118  int nbytes_r = ::read(fd_ser, (void*)buff, 3);
119 
120  printf("Line %d: Received response 0x%0X, 0x%0X, 0x%0X\n", __LINE__, buff[0], buff[1], buff[2]);
121 
122  // turn led on
123  buff[0] = 0x20;
124  buff[1] = 0x02;
125  buff[2] = 0x08;
126  buff[3] = 0x01;
127  nbytes_w = ::write(fd_ser, (void*)buff, 4);
128 
129  memset(buff, 0x00, 20);
130  nbytes_r = ::read(fd_ser, (void*)buff, 3);
131 
132  printf("Line %d: Received response 0x%0X, 0x%0X, 0x%0X\n", __LINE__, buff[0], buff[1], buff[2]);
133 
134  sleep(1);
135  // turn led off
136  buff[0] = 0x20;
137  buff[1] = 0x02;
138  buff[2] = 0x08;
139  buff[3] = 0x00;
140  nbytes_w = ::write(fd_ser, (void*)buff, 4);
141 
142  memset(buff, 0x00, 20);
143  nbytes_r = ::read(fd_ser, (void*)buff, 3);
144 
145  printf("Line %d: Received response 0x%0X, 0x%0X, 0x%0X\n", __LINE__, buff[0], buff[1], buff[2]);
146 
147  buff[0] = 0x20;
148  buff[1] = 0x01;
149  buff[2] = 0x19;
150  nbytes_w = ::write(fd_ser, (void*)buff, 3);
151 
152  memset(buff, 0x00, 20);
153  nbytes_r = ::read(fd_ser, (void*)buff, 4);
154 
155  printf("Line %d: Received response 0x%0X, 0x%0X, 0x%0X\n, 0x%0X\n", __LINE__, buff[0], buff[1], buff[2], buff[3]);
156 
157  sleep(2);
158 
159  buff[0] = 0x20;
160  buff[1] = 0x03;
161  buff[2] = 0x21;
162  buff[3] = 0x02;
163  buff[4] = 0x01;
164 
165  nbytes_w = ::write(fd_ser, (void*)buff, 5);
166 
167  memset(buff, 0x00, 20);
168  nbytes_r = ::read(fd_ser, (void*)buff, 6);
169 
170  printf("Line %d: gyro parameter fullscale 0x%0X.%0X.%0X.%0X.%0X.%0X\n", __LINE__, buff[0], buff[1], buff[2], buff[3], buff[4], buff[5]); // 0x01 --> 500 dps
171 
172 
173  buff[0] = 0x20;
174  buff[1] = 0x03;
175  buff[2] = 0x21;
176  buff[3] = 0x02;
177  buff[4] = 0x06;
178 
179  nbytes_w = ::write(fd_ser, (void*)buff, 5);
180 
181  memset(buff, 0x00, 20);
182  nbytes_r = ::read(fd_ser, (void*)buff, 7);
183 
184  printf("Line %d: gyro parameter scale factor 0x%0X.%0X.%0X.%0X.%0X.%0X.%0X\n", __LINE__, buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6]);
185 
186 
187  this->start();
188 
189  return true;
190 }
191 
192 int euler_base = 25;
193 
195 {
196  printf("Setting sample config\n");
197  uint8_t buff[7];
198  buff[0] = 0x20;
199  buff[1] = 0x05;
200  buff[2] = 0x50;
201  buff[3] = 0x9D; // enable AHRS algorithm for "Euler angles", acclerometer, gyroscope, temperature data (data calibrated) (anche pressione)
202  buff[4] = 0x18; // set frequency to 50HZ (the only one usable if AHRS is set), set output port to USB
203  buff[5] = 0x00; // bytes 5&6 to zeros means continuous mode
204  buff[6] = 0x00;
205 
206  int nbytes_w = ::write(fd_ser, (void*)buff, 7);
207 
208  memset(buff, 0x00, 7);
209  int nbytes_r = ::read(fd_ser, (void*)buff, 4);
210  printf("Received response 0x%02X, 0x%02X, 0x%02X, 0x%02X\n", buff[0], buff[1], buff[2], buff[3]);
211 
212  sleep(1);
213  printf("Reading back sample config\n");
214  // read back the settings
215  buff[0] = 0x20;
216  buff[1] = 0x01;
217  buff[2] = 0x51;
218  nbytes_w = ::write(fd_ser, (void*)buff, 3);
219  memset(buff, 0x00, 7);
220  nbytes_r = ::read(fd_ser, (void*)buff, 7);
221  printf("Received response 0x%0X, 0x%0X, 0x%0X, 0x%0X, 0x%0X, 0x%0X, 0x%0X\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6]);
222 
223  printf("Reading libbraries config\n");
224  // read back the settings
225  buff[0] = 0x20;
226  buff[1] = 0x01;
227  buff[2] = 0x18;
228  nbytes_w = ::write(fd_ser, (void*)buff, 3);
229  memset(buff, 0x00, 7);
230  nbytes_r = ::read(fd_ser, (void*)buff, 4);
231  printf("Line %d: Received response 0x%02X, 0x%02X, 0x%02X, 0x%02X\n", __LINE__, buff[0], buff[1], buff[2], buff[3]);
232 
233  /* data to be transfered are:
234  3 header +
235  2 frame progressive number +
236  6 accelerometer data +
237  6 gyroscope data +
238  6 magnetometer data +
239  2 temperature +
240  12 RPY data +
241  16 quaternion =
242  ------------------
243  53 total
244 
245  serial rate is 115.200 Kbits/s -> max rate is 50Hz when RPY or quaternion is reequested
246  */
247  expected_packet_size = (1 + 1 + 1) + (2 + 6 + 6 + 6 + 2 + 12 + 16);
248  expected_payload_size = expected_packet_size - 2;
249 
250  buffer = new char [100];
251  pippo = (Pippo*) &buffer[accel_base];
252  euler_float = reinterpret_cast<float*>(&(outVals.euler_raw[0]));
253 }
254 
255 
257 {
258  yTrace();
259  if(opened)
260  {
261  this->stop();
262 
263  char buff[20];
264  buff[0] = 0x20;
265  buff[1] = 0x01;
266  buff[2] = 0x01;
267  int nbytes_w = ::write(fd_ser, (void*)buff, 3);
268 
269  memset(buff, 0x00, 20);
270  int nbytes_r = ::read(fd_ser, (void*)buff, 3);
271 
272  printf("Received response 0x%0X, 0x%0X, 0x%0X\n", buff[0], buff[1], buff[2]);
273 
274  if(fd_ser != 0)
275  ::close(fd_ser);
276  fd_ser = 0;
277  printf("Closed %s\n", comPortName.c_str());
278  }
279  opened = false;
280  return true;
281 }
282 
283 
285 {
286  *nc = nchannels;
287  return true;
288 }
289 
290 
291 bool imuST_M1::calibrate(int ch, double v)
292 {
293  // Send command to zero gyro bias?? Be careful it must be used only if the device is absolutely stationary!!
294  printf("Not implemented yet\n");
295  return false;
296 }
297 
298 
299 bool imuST_M1::read(yarp::sig::Vector &out)
300 {
301  if(out.size() != 12)
302  out.resize(12);
303 
304  data_mutex.lock();
305  uint64_t imu_timeStamp;
306 
307  int out_idx = 0;
308  for(int i=0; i<3; i++, out_idx++)
309  out[out_idx] = (double) euler_float[i];
310 
311  for(int i=0; i<3; i++, out_idx++)
312  out[out_idx] = (double) outVals.accel[i] * 9.81 / 1000.0f;
313 
314  for(int i=0; i<3; i++, out_idx++)
315  out[out_idx] = (double) outVals.gyro[i] * 500.0f / (2<<15);
316 
317  for(int i=0; i<3; i++, out_idx++)
318  out[out_idx] = (double) outVals.magn[i];
319 
320  data_mutex.unlock();
321 
322  lastStamp.update();
323  return true;
324 }
325 
327 {
328  return lastStamp;
329 }
330 
332 // Thread //
334 
335 bool imuST_M1::threadInit()
336 {
337  // configure the type of data we want from the boards
338  sample_setting();
339  uint8_t buff[10];
340 
341  // start acquisition
342  buff[0] = 0x20;
343  buff[1] = 0x01;
344  buff[2] = 0x52;
345 
346  int nbytes_w = ::write(fd_ser, (void*)buff, 3);
347 
348  memset(buff, 0x00, 7);
349  int nbytes_r = ::read(fd_ser, (void*)buff, 3);
350 
351  printf("Line %d: Received response 0x%0X, 0x%0X, 0x%0X \n", __LINE__, buff[0], buff[1], buff[2]);
352 
353  printf("Started imu-3DM-GX3-25 thread\n");
354  return true;
355 }
356 
357 void imuST_M1::run()
358 {
359 // memset(buffer, 0x00, expected_packet_size);
360 
361  int nbytes_r = ::read(fd_ser, (void*)buffer, expected_packet_size);
362 
363 // buffer = &data.packet[0];
364  // check message is correct constructed
365  if(buffer[0] == 0xC0)
366  {
367  yError("Device return error code in the frame header!! \n");
368  // cleanup(?)
369  return;
370  }
371 
372  if(nbytes_r != expected_packet_size)
373  {
374  yError("Number of bytes read is different from expected size: read %d vs expected %d\n", nbytes_r, expected_packet_size);
375  // cleanup(?)
376  return;
377  }
378 
379  if(buffer[0] != 0x40)
380  {
381  yError("Wrong starting byte in the header\n");
382 // cleanup(?)
383  return;
384  }
385 
386  if(buffer[1] != expected_payload_size)
387  {
388  yError("Payload size doen't match the expected one\n");
389 // cleanup(?)
390  return;
391  }
392 
393  if(buffer[2] != 0x52)
394  {
395  yError("Message ID is wrong\n");
396 // cleanup(?)
397  return;
398  }
399 
400 // uint16_t progressiv_num = ;
401  int16_t tmp = *(int16_t*) &(buffer[3]); // << 8 + buffer[4];
402 // int16_t tmp2 = *tmp;
403  tmp = ntohs(tmp);
404 // printf("progressiv num = %d\n", tmp);
405  if( tmp != progressiv_num+1 )
406  {
407  yError("Progressive number check doesn't match\n");
408  progressiv_num = tmp;
409  // cleanup(?)
410  return;
411  }
412 
413  progressiv_num = tmp;
414 
415  // here the message should be fine
416 
417  int16_t temperature;
418 
419  data_mutex.lock();
420  pippo->accel[0] = ntohs(pippo->accel[0]);
421  pippo->accel[1] = ntohs(pippo->accel[1]);
422  pippo->accel[2] = ntohs(pippo->accel[2]);
423 
424  pippo->gyro[0] = ntohs(pippo->gyro[0]);
425  pippo->gyro[1] = ntohs(pippo->gyro[1]);
426  pippo->gyro[2] = ntohs(pippo->gyro[2]);
427 
428  pippo->magn[0] = ntohs(pippo->magn[0]);
429  pippo->magn[1] = ntohs(pippo->magn[1]);
430  pippo->magn[2] = ntohs(pippo->magn[2]);
431 
432  pippo->temp = ntohs(pippo->temp);
433 
434  pippo->euler_raw[0] = ntohl(pippo->euler_raw[0]);
435  pippo->euler_raw[1] = ntohl(pippo->euler_raw[1]);
436  pippo->euler_raw[2] = ntohl(pippo->euler_raw[2]);
437 
438  mempcpy(&outVals, pippo, sizeof(Pippo));
439  data_mutex.unlock();
440 
441  if(verbose)
442  {
443  printf("euler %f <--> %f <--> %f\n\n", euler_float[0], euler_float[1], euler_float[2]);
444  printf("accel: %4d <--> %4d <--> %4d\n", pippo->accel[0], pippo->accel[1], pippo->accel[2]);
445  printf("gyro: %4d <--> %4d <--> %4d\n", pippo->gyro[0], pippo->gyro[1], pippo->gyro[2]);
446  printf("magn: %4d <--> %4d <--> %4d\n", pippo->magn[0], pippo->magn[1], pippo->magn[2]);
447  printf("temp: %4d \n", pippo->temp);
448  }
449 }
450 
451 void imuST_M1::threadRelease()
452 {
453  // start acquisition
454  printf("Stopping acquisition imu-3DM-GX3-25 thread\n");
455 
456  uint8_t buff[7];
457  buff[0] = 0x20;
458  buff[1] = 0x01;
459  buff[2] = 0x53;
460 
461  int nbytes_w = ::write(fd_ser, (void*)buff, 3);
462 
463  memset(buff, 0x00, 7);
464  int nbytes_r = ::read(fd_ser, (void*)buff, 3);
465 
466  printf("Line %d: Received response 0x%0X, 0x%0X, 0x%0X \n", __LINE__, buff[0], buff[1], buff[2]);
467 }
int euler_base
Definition: ST_M1.cpp:192
#define accel_base
virtual bool close()
Definition: ST_M1.cpp:256
virtual bool read(yarp::sig::Vector &out)
Definition: ST_M1.cpp:299
void sample_setting(void)
Definition: ST_M1.cpp:194
virtual bool calibrate(int ch, double v)
Definition: ST_M1.cpp:291
virtual yarp::os::Stamp getLastInputStamp()
Definition: ST_M1.cpp:326
virtual bool getChannels(int *nc)
Definition: ST_M1.cpp:284
virtual bool open(yarp::os::Searchable &config)
Definition: ST_M1.cpp:41
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