iCub-main
XSensMTx.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2006 Radu Bogdan Rusu, Alexis Maldonado
5  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
6  *
7  */
8 
9 #include <yarp/os/LogStream.h>
10 #include <yarp/os/Thread.h>
11 #include <yarp/os/Time.h>
12 #include <yarp/os/Stamp.h>
13 #include <string>
14 #include <mutex>
15 
16 #include "MTComm.h"
17 #include "XSensMTx.h"
18 
19 using namespace yarp::os;
20 using namespace yarp::dev;
21 using namespace yarp::sig;
22 //using ACE_OS::printf;
23 
24 #define M_PI 3.14159265358979323846264338328
25 #define CTRL_RAD2DEG (180.0/M_PI)
26 #define CTRL_DEG2RAD (M_PI/180.0)
27 
28 constexpr size_t rpyStartIdx = 0;
29 constexpr size_t accelStartIdx = 3;
30 constexpr size_t gyroStartIdx = 6;
31 constexpr size_t magnStartIdx = 9;
32 
33 
34 class XSensMTxResources: public Thread
35 {
36 public:
37  explicit XSensMTxResources()
38  {
39  _bStreamStarted=false;
40  _bError=false;
41 
42  _last=0;
43 
44  _last=new double [12];
45  for(int k=0;k<12;k++)
46  _last[k]=0.0;
47  }
48 
50  {
51  if (isRunning())
52  stop();
53 
54  _bStreamStarted=false;
55 
56  if (_last!=0)
57  {
58  delete [] _last;
59  _last=0;
60  }
61  }
62 
64  bool _bError;
65 
67 
68  double *_last;
69  yarp::os::Stamp _lastStamp;
70 
71  std::mutex _semaphore;
72 
73  virtual void run ();
74 };
75 
77 {
78  unsigned char data[MAXMSGLEN];
79  float euler_data[3] = {0};
80  float accel_data[3] = {0};
81  float gyro_data [3] = {0};
82  float magn_data [3] = {0};
83  short datalen;
84  int received;
85 
86  while (!Thread::isStopping ())
87  {
88  // Get data from the MTx device
89  received = mtcomm.readDataMessage (data, datalen);
90  // Parse and get value (EULER ORIENTATION)
91  mtcomm.getValue (VALUE_ORIENT_EULER, euler_data, data, BID_MASTER);
92  // Parse and get calibrated acceleration values
93  mtcomm.getValue (VALUE_CALIB_ACC, accel_data, data, BID_MASTER);
94  // Parse and get calibrated gyro values
95  mtcomm.getValue (VALUE_CALIB_GYR, gyro_data, data, BID_MASTER);
96  // Parse and get calibrated magnetometer values
97  mtcomm.getValue (VALUE_CALIB_MAG, magn_data, data, BID_MASTER);
98 
99  std::lock_guard<std::mutex> lck(_semaphore);
100 
101  //euler_data are expressed in deg
102  _last[0] = euler_data[0]; //roll
103  _last[1] = euler_data[1]; //pitch
104  _last[2] = euler_data[2]; //yaw
105 
106  _last[3] = accel_data[0]; //accel-X
107  _last[4] = accel_data[1]; //accel-Y
108  _last[5] = accel_data[2]; //accel-Z
109 
110  //gyro_data are expressed in rad/s, so they have to be converted in deg/s
111  _last[6] = gyro_data[0]*CTRL_RAD2DEG; //gyro-X
112  _last[7] = gyro_data[1]*CTRL_RAD2DEG; //gyro-Y
113  _last[8] = gyro_data[2]*CTRL_RAD2DEG; //gyro-Z
114 
115  _last[9] = magn_data[0]; //magn-X
116  _last[10] = magn_data[1]; //magn-Y
117  _last[11] = magn_data[2]; //magn-Z
118 
119  if (received == MTRV_OK)
120  _bError=false;
121  else
122  _bError=true;
123 
124  _lastStamp.update();
125  }
126 }
127 
128 inline XSensMTxResources& RES(void *res) { return *(XSensMTxResources *)res; }
129 
134 XSensMTx::XSensMTx() : system_resources{nullptr},
135  nchannels{12},
136  m_sensorName{"sensor_imu_xsens"},
137  m_frameName{"sensor_imu_xsens"}
138 {
139 }
140 
142 {
143  // stop thread first
144  if (system_resources!=0)
145  {
146  delete ((XSensMTxResources *)(system_resources));
147  system_resources=0;
148  }
149 }
150 
151 bool XSensMTx::read(Vector &out)
152 {
153  XSensMTxResources &d= RES(system_resources);
154  bool ret;
155 
156  if (d._bStreamStarted)
157  {
158  std::lock_guard<std::mutex> lck(d._semaphore);
159 
160  // Euler+accel+gyro+magn orientation values
161  for (int i = 0; i < nchannels; i++)
162  out[i]=d._last[i];
163 
164  lastStamp=d._lastStamp;
165  ret=!d._bError;
166  }
167  else
168  ret=false;
169 
170  return ret;
171 }
172 
174 {
175  *nc=nchannels;
176  return true;
177 }
178 
179 bool XSensMTx::calibrate(int ch, double v)
180 {
181  printf("Not implemented yet\n");
182  return false;
183 }
184 
185 bool XSensMTx::start()
186 {
187  XSensMTxResources &d=RES(system_resources);
188 
189  d.start();
190  d._bStreamStarted=true;
191 
192  return true;
193 }
194 
195 bool XSensMTx::stop()
196 {
197  XSensMTxResources &d=RES(system_resources);
198 
199  if (d.isRunning())
200  d.stop();
201 
202  d._bStreamStarted=false;
203 
204  return true;
205 }
206 
207 bool XSensMTx::open(yarp::os::Searchable &config)
208 {
209  XSensMTxParameters par;
210 
211 #ifdef WIN32
212  par.comPort = config.check ("serial", Value(11),
213  "numeric identifier of comport").asInt();
214 #else
215  par.comPortString = config.check("serial",Value("/dev/ttyUSB0"),
216  "device name of comport").asString().c_str();
217 #endif
218  if (config.check("sensor_name") && config.find("sensor_name").isString())
219  {
220  m_sensorName = config.find("sensor_name").asString();
221  }
222  if (config.check("frame_name") && config.find("frame_name").isString())
223  {
224  m_frameName = config.find("frame_name").asString();
225  }
226 
227  return open(par);
228 }
229 
231 {
232  if (system_resources!=0)
233  return false;
234 
235  system_resources=(void *) (new XSensMTxResources);
236 
237  XSensMTxResources &d=RES(system_resources);
238 
239  // Open the MTx device
240 #ifdef WIN32
241  if (d.mtcomm.openPort (par.comPort) != MTRV_OK)
242  {
243  fprintf(stderr, "Failed to open com port %d\n",
244  par.comPort);
245 
246  return false;
247  }
248 #else
249  if (d.mtcomm.openPort (par.comPortString.c_str ()) != MTRV_OK)
250  {
251  fprintf(stderr, "Failed to open com port %s\n",
252  par.comPortString.c_str());
253  return false;
254  }
255 #endif
256 
257  int outputSettings = OUTPUTSETTINGS_ORIENTMODE_EULER;
258 
259  unsigned long tmpOutputMode, tmpOutputSettings;
260  unsigned short tmpDataLength;
261 
262  // Put MTi/MTx in Config State. Here sometimes there are problems if the device was not properly closed.
263  int count = 0;
264  for (count = 0; count <10; count++ )
265  {
267  {
268  printf ("MRCHECK Unable to connect to XSensMtX device, attempt %d.\n", count);
269  yarp::os::Time::delay(0.010);
270  }
271  else
272  break;
273  }
274  if (count >= 10)
275  {
276  printf ("XSensMtX init check: no device connected.\n");
277  return false;
278  }
279  else
280  printf ("XSensMtX init check: device ok.\n");
281 
282  unsigned short numDevices;
283  // Get current settings and check if Xbus Master is connected
284  if (d.mtcomm.getDeviceMode(&numDevices) != MTRV_OK) {
285  if (numDevices == 1)
286  printf ("MTi / MTx has not been detected\nCould not get device mode.\n");
287  else
288  printf ("Not just MTi / MTx connected to Xbus, %d devices found.\nCould not get all device modes.\n", numDevices);
289  return false;
290  }
291 
292  // Check if Xbus Master is connected
293  d.mtcomm.getMode (tmpOutputMode, tmpOutputSettings, tmpDataLength, BID_MASTER);
294  if (tmpOutputMode == OUTPUTMODE_XM)
295  {
296  // If Xbus Master is connected, attached Motion Trackers should not send sample counter
297  printf ("Sorry, this driver only talks to one MTx device.\n");
298  return false;
299  }
300 
301  int outputMode = OUTPUTMODE_CALIB + OUTPUTMODE_ORIENT;
302  // Set output mode and output settings for the MTi/MTx
303  if (d.mtcomm.setDeviceMode(outputMode, outputSettings, BID_MASTER) != MTRV_OK) {
304  printf ("Could not set device mode(s)\n");
305  return false;
306  }
307 
308  // Put MTi/MTx in Measurement State
310 
311  // start thread
312  return XSensMTx::start();
313 }
314 
316 {
317  // stop thread
318  if (system_resources==0)
319  return false; //the device was never opened, or there was an error
320 
321  XSensMTx::stop();
322 
323  XSensMTxResources &d=RES(system_resources);
324 
325  // Close the MTx device
326  if (d.mtcomm.close () != MTRV_OK)
327  {
328  delete ((XSensMTxResources *)(system_resources));
329  system_resources=0;
330  return false;
331  }
332  else
333  {
334  delete ((XSensMTxResources *)(system_resources));
335  system_resources=0;
336  return true;
337  }
338 }
339 
341 {
342  return lastStamp;
343 }
344 
345 
347 {
348  return 1;
349 }
350 
351 
352 yarp::dev::MAS_status XSensMTx::getThreeAxisLinearAccelerometerStatus(size_t sens_index) const
353 {
354  return genericGetStatus(sens_index);
355 }
356 
357 bool XSensMTx::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string& name) const
358 {
359  return genericGetSensorName(sens_index, name);
360 }
361 
362 bool XSensMTx::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string& frameName) const
363 {
364  return genericGetFrameName(sens_index, frameName);
365 }
366 
367 bool XSensMTx::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
368 {
369  return genericGetMeasure(sens_index, out, timestamp, accelStartIdx);
370 }
371 
372 
374 {
375  return 1;
376 }
377 
378 
379 yarp::dev::MAS_status XSensMTx::getThreeAxisGyroscopeStatus(size_t sens_index) const
380 {
381  return genericGetStatus(sens_index);
382 }
383 
384 bool XSensMTx::getThreeAxisGyroscopeName(size_t sens_index, std::string& name) const
385 {
386  return genericGetSensorName(sens_index, name);
387 }
388 
389 bool XSensMTx::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const
390 {
391  return genericGetFrameName(sens_index, frameName);
392 }
393 
394 bool XSensMTx::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
395 {
396  return genericGetMeasure(sens_index, out, timestamp, gyroStartIdx);
397 }
398 
400 {
401  return 1;
402 }
403 
404 yarp::dev::MAS_status XSensMTx::getOrientationSensorStatus(size_t sens_index) const
405 {
406  return genericGetStatus(sens_index);
407 }
408 
409 bool XSensMTx::getOrientationSensorName(size_t sens_index, std::string& name) const
410 {
411  return genericGetSensorName(sens_index, name);
412 }
413 
414 bool XSensMTx::getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const
415 {
416  return genericGetFrameName(sens_index, frameName);
417 }
418 
419 bool XSensMTx::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy, double& timestamp) const
420 {
421  return genericGetMeasure(sens_index, rpy, timestamp, rpyStartIdx);
422 }
423 
425 {
426  return 1;
427 }
428 
429 yarp::dev::MAS_status XSensMTx::getThreeAxisMagnetometerStatus(size_t sens_index) const
430 {
431  return genericGetStatus(sens_index);
432 }
433 
434 bool XSensMTx::getThreeAxisMagnetometerName(size_t sens_index, std::string& name) const
435 {
436  return genericGetSensorName(sens_index, name);
437 }
438 
439 bool XSensMTx::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string& frameName) const
440 {
441  return genericGetFrameName(sens_index, frameName);
442 }
443 
444 bool XSensMTx::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
445 {
446  return genericGetMeasure(sens_index, out, timestamp, magnStartIdx);
447 }
448 yarp::dev::MAS_status XSensMTx::genericGetStatus(size_t sens_index) const
449 {
450  if (sens_index != 0)
451  {
452  yError() << "xsens: sens_index must be equal to 0, since there is only one sensor in consideration";
453  return yarp::dev::MAS_status::MAS_ERROR;
454  }
455 
456  return yarp::dev::MAS_status::MAS_OK;
457 }
458 
459 bool XSensMTx::genericGetSensorName(size_t sens_index, std::string& name) const
460 {
461  if (sens_index != 0)
462  {
463  yError() << "xsens: sens_index must be equal to 0, since there is only one sensor in consideration";
464  return false;
465  }
466 
467  name = m_sensorName;
468  return true;
469 }
470 
471 bool XSensMTx::genericGetFrameName(size_t sens_index, std::string& frameName) const
472 {
473  if (sens_index != 0)
474  {
475  yError() << "xsens: sens_index must be equal to 0, since there is only one sensor in consideration";
476  return false;
477  }
478 
479  frameName = m_frameName;
480  return true;
481 
482 }
483 
484 bool XSensMTx::genericGetMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp, size_t startIdx) const {
485 
486  if (sens_index != 0)
487  {
488  yError() << "xsens: sens_index must be equal to 0, since there is only one sensor in consideration";
489  return false;
490  }
491 
492  auto &d= RES(system_resources);
493  out.resize(3);
494  std::lock_guard<std::mutex> lck(d._semaphore);
495  out[0] = d._last[startIdx];
496  out[1] = d._last[startIdx + 1];
497  out[2] = d._last[startIdx + 2];
498 
499  timestamp = d._lastStamp.getTime();
500  return true;
501 }
bool getChannels(int *nc) override
Definition: XSensMTx.cpp:173
short openPort(const int portNumber, const unsigned long baudrate=PBR_115K2, const unsigned long inqueueSize=4096, const unsigned long outqueueSize=1024)
Definition: MTComm.cpp:160
bool read(yarp::sig::Vector &out) override
Definition: XSensMTx.cpp:151
bool calibrate(int ch, double v) override
Definition: XSensMTx.cpp:179
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which orientation sensor measurements are expressed. ...
Definition: XSensMTx.cpp:414
#define MAXMSGLEN
Definition: MTComm.h:139
out
Definition: sine.m:8
static int stop
Definition: iCub_Sim.cpp:41
size_t getNrOfOrientationSensors() const override
Get the number of orientation sensors in the device.
Definition: XSensMTx.cpp:399
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
Get the status of three axis magnetometer.
Definition: XSensMTx.cpp:429
#define MTRV_OK
Definition: MTComm.h:878
yarp::os::Stamp getLastInputStamp() override
Definition: XSensMTx.cpp:340
double * _last
Definition: XSensMTx.cpp:68
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of three axis linear accelerometer.
Definition: XSensMTx.cpp:357
bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
Get the name of three axis magnetometer.
Definition: XSensMTx.cpp:434
#define BID_MASTER
Definition: MTComm.h:123
short close()
Definition: MTComm.cpp:702
short getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:2187
virtual void run()
Definition: XSensMTx.cpp:76
#define VALUE_CALIB_MAG
Definition: MTComm.h:471
bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get three axis magnetometer measurements.
Definition: XSensMTx.cpp:444
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of orientation sensor.
Definition: XSensMTx.cpp:404
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis gyroscope measurements are expressed.
Definition: XSensMTx.cpp:389
fprintf(fid,'\n')
constexpr size_t rpyStartIdx
Definition: XSensMTx.cpp:28
static int v
Definition: iCub_Sim.cpp:42
short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:2130
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of three axis linear accelerometer.
Definition: XSensMTx.cpp:352
#define OUTPUTMODE_CALIB
Definition: MTComm.h:772
bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis magnetometer measurements are expressed.
Definition: XSensMTx.cpp:439
XSensMTxResources & RES(void *res)
Definition: XSensMTx.cpp:128
#define VALUE_CALIB_GYR
Definition: MTComm.h:470
#define MID_GOTOMEASUREMENT
Definition: MTComm.h:183
size_t getNrOfThreeAxisGyroscopes() const override
Get the number of three axis gyroscopes in the device.
Definition: XSensMTx.cpp:373
bool close() override
Definition: XSensMTx.cpp:315
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of orientation sensor.
Definition: XSensMTx.cpp:409
#define VALUE_ORIENT_EULER
Definition: MTComm.h:473
size_t getNrOfThreeAxisLinearAccelerometers() const override
Get the number of three axis linear accelerometers in the device.
Definition: XSensMTx.cpp:346
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get three axis linear accelerometer measurements.
Definition: XSensMTx.cpp:367
virtual ~XSensMTx()
Definition: XSensMTx.cpp:141
#define MID_GOTOCONFIG
Definition: MTComm.h:306
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis linear accelerometer measurements are expressed...
Definition: XSensMTx.cpp:362
std::string comPortString
Definition: 3dm_gx3.h:41
std::mutex _semaphore
Definition: XSensMTx.cpp:71
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get three axis gyroscope measurements.
Definition: XSensMTx.cpp:394
size_t getNrOfThreeAxisMagnetometers() const override
Get the number of three axis magnetometers in the device.
Definition: XSensMTx.cpp:424
virtual ~XSensMTxResources()
Definition: XSensMTx.cpp:49
short getDeviceMode(unsigned short *numDevices=NULL)
Definition: MTComm.cpp:2021
#define VALUE_CALIB_ACC
Definition: MTComm.h:469
yarp::os::Stamp _lastStamp
Definition: XSensMTx.cpp:69
bool open(yarp::os::Searchable &config) override
Definition: XSensMTx.cpp:207
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
Get orientation sensor measurements.
Definition: XSensMTx.cpp:419
#define OUTPUTSETTINGS_ORIENTMODE_EULER
Definition: MTComm.h:780
constexpr size_t magnStartIdx
Definition: XSensMTx.cpp:31
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of three axis gyroscope.
Definition: XSensMTx.cpp:379
short writeMessage(const unsigned char mid, const unsigned long dataValue=0, const unsigned char dataValueLen=0, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:1012
#define OUTPUTMODE_ORIENT
Definition: MTComm.h:773
#define OUTPUTMODE_XM
Definition: MTComm.h:769
constexpr size_t gyroStartIdx
Definition: XSensMTx.cpp:30
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of three axis gyroscope.
Definition: XSensMTx.cpp:384
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
constexpr size_t accelStartIdx
Definition: XSensMTx.cpp:29