Main MRPT website > C++ reference for MRPT 1.5.6
CIMUXSens_MT4.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 
13 #include <mrpt/system/threads.h>
17 
19 
20 using namespace mrpt::utils;
21 using namespace mrpt::obs;
22 using namespace mrpt::hwdrivers;
23 using namespace std;
24 
25 #if MRPT_HAS_xSENS_MT4
26  /* Copyright (c) Xsens Technologies B.V., 2006-2012. All rights reserved.
27 
28  This source code is provided under the MT SDK Software License Agreement
29  and is intended for use only by Xsens Technologies BV and
30  those that have explicit written permission to use it from
31  Xsens Technologies BV.
32 
33  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
34  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
35  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
36  PARTICULAR PURPOSE.
37  */
38  #include <xsens/xsresultvalue.h>
39  #include <xsens/xsbytearray.h>
40  #include <xsens/xsmessagearray.h>
41  #include <xsens/xsdeviceid.h>
42  #include <xsens/xsportinfo.h>
43  #include <xsens/xsoutputmode.h>
44  #include <xsens/xsoutputsettings.h>
46  #include <xsens/protocolhandler.h>
47  #include <xsens/usbinterface.h>
48  #include <xsens/serialinterface.h>
49  #include <xsens/streaminterface.h>
50  #include <xsens/xsportinfoarray.h>
51  #include <xsens/xsdatapacket.h>
52  #include <xsens/xsstatusflag.h>
53  #include <xsens/xstime.h>
54  #include <xsens/legacydatapacket.h>
55  #include <xsens/int_xsdatapacket.h>
57 
58  class DeviceClass
59  {
60  public:
61  DeviceClass() : m_streamInterface(NULL) { }
62  ~DeviceClass()
63  {
64  if (m_streamInterface) {
65  delete m_streamInterface;
66  m_streamInterface=NULL;
67  }
68  }
69 
70  /*! \brief Open an IO device
71  \param portInfo The info to use for opening the port
72  \return True when successful
73  */
74  bool openPort(const XsPortInfo& portInfo)
75  {
76  if (portInfo.isUsb())
77  m_streamInterface = new UsbInterface();
78  else
79  m_streamInterface = new SerialInterface();
80 
81  if (m_streamInterface->open(portInfo) != XRV_OK)
82  return false;
83  return true;
84  }
85 
86  /*! \brief Close an IO device
87  */
88  void close()
89  {
90  if (m_streamInterface)
91  m_streamInterface->close();
92  }
93 
94  /*! \brief Read available data from the open IO device
95  \details This function will attempt to read all available data from the open device (COM port
96  or USB port).
97  The function will read from the device, but it won't wait for data to become available.
98  \param raw A XsByteArray to where the read data will be stored.
99  \return Whether data has been read from the IO device
100  */
101  XsResultValue readDataToBuffer(XsByteArray& raw)
102  {
103  // always read data and append it to the cache before doing analysis
104  const int maxSz = 8192;
105  XsResultValue res = m_streamInterface->readData(maxSz, raw);
106  if (raw.size())
107  return XRV_OK;
108 
109  return res;
110  }
111 
112  /*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn
113  \details This function will read all present messages in the read buffer. In order for this function
114  to work, you need to call readDataToBuffer() first.
115  \param rawIn The buffered data in which to search for messages
116  \param messages The messages found in the data
117  \return The messages that were read.
118  */
119  XsResultValue processBufferedData(XsByteArray& rawIn, XsMessageArray& messages)
120  {
121  ProtocolHandler protocol;
122 
123  if (rawIn.size())
124  m_dataBuffer.append(rawIn);
125 
126  int popped = 0;
127  messages.clear();
128 
129  for(;;)
130  {
131  XsByteArray raw(m_dataBuffer.data()+popped, m_dataBuffer.size()-popped);
132  XsMessage message;
133  MessageLocation location = protocol.findMessage(message, raw);
134 
135  if (location.isValid())
136  {
137  // message is valid, remove data from cache
138  popped += location.m_size + location.m_startPos;
139  messages.push_back(message);
140  }
141  else
142  {
143  if (popped)
144  m_dataBuffer.pop_front(popped);
145 
146  if (messages.empty())
147  return XRV_TIMEOUTNODATA;
148 
149  return XRV_OK;
150  }
151  }
152  }
153 
154  /*! \brief Wait for the requested XsXbusMessageId
155  \param xmid The message id to wait for
156  \param rcv The received message
157  \return Whether the requested message was found
158  */
159  bool waitForMessage(XsXbusMessageId xmid, XsMessage& rcv)
160  {
162  XsMessageArray msgs;
163  bool foundAck = false;
164  do
165  {
166  readDataToBuffer(data);
167  processBufferedData(data, msgs);
168  for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
169  if ((*it).getMessageId() == xmid)
170  {
171  foundAck = true;
172  rcv = *it;
173  }
174  } while (!foundAck);
175  return foundAck;
176  }
177 
178  /*! \brief Write a message to the IO device
179  \param msg The message to write
180  \return Whether the message could be written
181  */
182  bool writeMessage(const XsMessage& msg)
183  {
184  XsByteArray raw;
185  if (ProtocolHandler::composeMessage(raw, msg) < 0)
186  return false;
187 
188  return (m_streamInterface->writeData(raw) == XRV_OK);
189  }
190 
191  /*! \brief Put a device in config mode
192  \return True when the device acknowledged config mode
193  */
194  bool gotoConfig()
195  {
196  XsMessage snd(XMID_GotoConfig, 0), rcv;
197  writeMessage(snd);
198 
199  return waitForMessage(XMID_GotoConfigAck, rcv);
200  }
201 
202  /*! \brief Put a device in measurement mode
203  \return True when the device acknowledged measurement mode
204  */
205  bool gotoMeasurement()
206  {
207  XsMessage snd(XMID_GotoMeasurement, 0), rcv;
208  writeMessage(snd);
209 
210  return waitForMessage(XMID_GotoMeasurementAck, rcv);
211  }
212 
213  /*! \brief Request the product code from a device
214  \return The product code when ok, otherwise an empty XsString
215  */
216  XsString getProductCode()
217  {
218  XsMessage snd(XMID_ReqProductCode, 0), rcv;
219  writeMessage(snd);
220 
221  if (waitForMessage(XMID_ProductCode,rcv))
222  {
223  const char* pc = (const char*) rcv.getDataBuffer(0);
224  std::string result(pc?pc:"", rcv.getDataSize());
225  std::string::size_type thingy = result.find(" ");
226  if (thingy < 20)
227  result.erase(result.begin() + thingy, result.end()); //lint !e534
228  return XsString(result);
229  }
230  else
231  return XsString();
232  }
233 
234  /*! \brief Request the device id from a device
235  \return The device id (XsDeviceId) when ok, otherwise an empty XsDeviceId
236  */
237  XsDeviceId getDeviceId()
238  {
239  XsMessage snd(XMID_ReqDid, 0), rcv;
240  writeMessage(snd);
241 
242  if (waitForMessage(XMID_DeviceId,rcv))
243  {
244  return rcv.getDataLong();
245  }
246  else
247  return XsDeviceId();
248  }
249 
250  /*! \brief Set the device mode of a device (outputmode and outputsettings)
251  \param outputMode The XsOutputMode to set
252  \param outputSettings The XsOutputSettings to set
253  \return True when successful
254  */
255  bool setDeviceMode(const XsOutputMode& outputMode, const XsOutputSettings& outputSettings)
256  {
258 
259  sndOM.resizeData(2);
260  sndOM.setDataShort((uint16_t) outputMode);
261  writeMessage(sndOM);
262  if (!waitForMessage(XMID_SetOutputModeAck, rcv))
263  return false;
264 
266  snd.resizeData(4);
267  snd.setDataLong((uint32_t)outputSettings);
268  writeMessage(sndOS);
269  if (!waitForMessage(XMID_SetOutputSettingsAck, rcv))
270  return false;
271 
272  return true;
273  }
274 
275  /*! \brief Set the output configuration of a device
276  \param config An array XsOutputConfigurationArray) containing the one or multiple XsOutputConfigurations
277  \return True when successful
278  */
279  bool setOutputConfiguration(XsOutputConfigurationArray& config)
280  {
282  if (config.size() == 0)
283  {
284  snd.setDataShort((uint16_t)XDI_None, 0);
285  snd.setDataShort(0, 2);
286  }
287  else
288  {
289  for (XsSize i = 0; i < (XsSize) config.size(); ++i)
290  {
291  snd.setDataShort((uint16_t)config[i].m_dataIdentifier, i*4);
292  snd.setDataShort(config[i].m_frequency, i*4+2);
293  }
294  }
295  writeMessage(snd);
296 
297  return waitForMessage(XMID_SetOutputConfigurationAck, rcv);
298  }
299 
300 
301 
302 
303  private:
304  StreamInterface *m_streamInterface;
305  XsByteArray m_dataBuffer;
306  };
307 #endif
308 
309 // Adaptors for the "void*" memory blocks:
310 #define my_xsens_device (*static_cast<DeviceClass*>(m_dev_ptr))
311 #define my_xsens_devid (*static_cast<XsDeviceId*>(m_devid_ptr))
312 
313 // Include libraries in linking:
314 #if MRPT_HAS_xSENS_MT4
315  #ifdef MRPT_OS_WINDOWS
316  // WINDOWS:
317  #if defined(_MSC_VER) || defined(__BORLANDC__)
318  #pragma comment (lib,"SetupAPI.lib")
319  #pragma comment (lib,"WinUsb.lib")
320  #endif
321  #endif // MRPT_OS_WINDOWS
322 #endif // MRPT_HAS_xSENS_MT4
323 
324 /*-------------------------------------------------------------
325  CIMUXSens_MT4
326 -------------------------------------------------------------*/
327 CIMUXSens_MT4::CIMUXSens_MT4( ) :
328  m_port_bauds (0),
329  m_portname (),
330  m_sampleFreq (100),
331  m_timeStartUI (0),
332  m_timeStartTT (0),
333  m_sensorPose (),
334  m_dev_ptr (NULL),
335  m_devid_ptr (NULL)
336 {
337  m_sensorLabel = "XSensMTi_MT4";
338 
339 #if MRPT_HAS_xSENS_MT4
340  m_dev_ptr = new DeviceClass;
341  m_devid_ptr = new XsDeviceId;
342 #else
343  THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class cannot be used.");
344 #endif
345 }
346 
347 /*-------------------------------------------------------------
348  ~CIMUXSens_MT4
349 -------------------------------------------------------------*/
351 {
352 #if MRPT_HAS_xSENS_MT4
353  my_xsens_device.close();
354  delete static_cast<DeviceClass*>(m_dev_ptr);
355  m_dev_ptr=NULL;
356 
357  delete static_cast<XsDeviceId*>(m_devid_ptr);
358  m_devid_ptr=NULL;
359 #endif
360 }
361 
362 /*-------------------------------------------------------------
363  doProcess
364 -------------------------------------------------------------*/
366 {
367 #if MRPT_HAS_xSENS_MT4
368  if(m_state == ssError)
369  {
370  mrpt::system::sleep(200);
371  initialize();
372  }
373 
374  if(m_state == ssError)
375  return;
376 
378  XsMessageArray msgs;
379 
380  my_xsens_device.readDataToBuffer(data);
381  my_xsens_device.processBufferedData(data, msgs);
382  for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
383  {
384  // Retrieve a packet
385  XsDataPacket packet;
386  if ((*it).getMessageId() == XMID_MtData)
387  {
388  LegacyDataPacket lpacket(1, false);
389 
390  lpacket.setMessage((*it));
391  lpacket.setXbusSystem(false, false);
392  lpacket.setDeviceId(my_xsens_devid, 0);
393  lpacket.setDataFormat(XOM_Orientation, XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All/*XOS_OrientationMode_Quaternion*/,0); //lint !e534
394  XsDataPacket_assignFromXsLegacyDataPacket(&packet, &lpacket, 0);
395  }
396  else if ((*it).getMessageId() == XMID_MtData2) {
397  packet.setMessage((*it));
398  packet.setDeviceId(my_xsens_devid);
399  }
400 
401  // Data properly collected: extract data fields
402  // -------------------------------------------------
403  m_state = ssWorking;
404  CObservationIMUPtr obs = CObservationIMU::Create();
405 
406  if (packet.containsOrientation())
407  {
408  XsEuler euler = packet.orientationEuler();
409  obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler.yaw()); obs->dataIsPresent[IMU_YAW] = true;
410  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler.pitch()); obs->dataIsPresent[IMU_PITCH] = true;
411  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler.roll()); obs->dataIsPresent[IMU_ROLL] = true;
412 
413  XsQuaternion quat = packet.orientationQuaternion();
414  obs->rawMeasurements[IMU_ORI_QUAT_X] = quat.x(); obs->dataIsPresent[IMU_ORI_QUAT_X] = true;
415  obs->rawMeasurements[IMU_ORI_QUAT_Y] = quat.y(); obs->dataIsPresent[IMU_ORI_QUAT_Y] = true;
416  obs->rawMeasurements[IMU_ORI_QUAT_Z] = quat.z(); obs->dataIsPresent[IMU_ORI_QUAT_Z] = true;
417  obs->rawMeasurements[IMU_ORI_QUAT_W] = quat.w(); obs->dataIsPresent[IMU_ORI_QUAT_W] = true;
418  }
419 
420  if (packet.containsCalibratedAcceleration())
421  {
422  XsVector acc_data = packet.calibratedAcceleration();
423  obs->rawMeasurements[IMU_X_ACC] = acc_data[0]; obs->dataIsPresent[IMU_X_ACC] = true;
424  obs->rawMeasurements[IMU_Y_ACC] = acc_data[1]; obs->dataIsPresent[IMU_Y_ACC] = true;
425  obs->rawMeasurements[IMU_Z_ACC] = acc_data[2]; obs->dataIsPresent[IMU_Z_ACC] = true;
426  }
427 
428  if (packet.containsCalibratedGyroscopeData())
429  {
430  XsVector gyr_data = packet.calibratedGyroscopeData();
431  obs->rawMeasurements[IMU_YAW_VEL] = gyr_data[2]; obs->dataIsPresent[IMU_YAW_VEL] = true;
432  obs->rawMeasurements[IMU_PITCH_VEL] = gyr_data[1]; obs->dataIsPresent[IMU_PITCH_VEL] = true;
433  obs->rawMeasurements[IMU_ROLL_VEL] = gyr_data[0]; obs->dataIsPresent[IMU_ROLL_VEL] = true;
434  }
435 
436  if (packet.containsCalibratedMagneticField())
437  {
438  XsVector mag_data = packet.calibratedMagneticField();
439  obs->rawMeasurements[IMU_MAG_X] = mag_data[0]; obs->dataIsPresent[IMU_MAG_X] = true;
440  obs->rawMeasurements[IMU_MAG_Y] = mag_data[1]; obs->dataIsPresent[IMU_MAG_Y] = true;
441  obs->rawMeasurements[IMU_MAG_Z] = mag_data[2]; obs->dataIsPresent[IMU_MAG_Z] = true;
442  }
443 
444  if (packet.containsVelocity())
445  {
446  XsVector vel_data = packet.velocity();
447  obs->rawMeasurements[IMU_X_VEL] = vel_data[0]; obs->dataIsPresent[IMU_X_VEL] = true;
448  obs->rawMeasurements[IMU_Y_VEL] = vel_data[1]; obs->dataIsPresent[IMU_Y_VEL] = true;
449  obs->rawMeasurements[IMU_Z_VEL] = vel_data[2]; obs->dataIsPresent[IMU_Z_VEL] = true;
450  }
451 
452  if (packet.containsTemperature())
453  {
454  obs->rawMeasurements[IMU_TEMPERATURE] = packet.temperature(); obs->dataIsPresent[IMU_TEMPERATURE] = true;
455  }
456 
457  if (packet.containsAltitude())
458  {
459  obs->rawMeasurements[IMU_ALTITUDE ] = packet.altitude(); obs->dataIsPresent[IMU_ALTITUDE ] = true;
460  }
461 
462  // TimeStamp
463  if (packet.containsSampleTime64())
464  {
465  const uint64_t nowUI = packet.sampleTime64();
466 
467  uint64_t AtUI = 0;
468  if( m_timeStartUI == 0 )
469  {
470  m_timeStartUI = nowUI;
472  }
473  else
474  AtUI = nowUI - m_timeStartUI;
475 
476  double AtDO = AtUI * 1000.0; // Difference in intervals of 100 nsecs
477  obs->timestamp = m_timeStartTT + AtDO;
478  }
479  else if (packet.containsUtcTime())
480  {
481  XsUtcTime utc = packet.utcTime();
482 
484 
485  parts.day_of_week = 0;
486  parts.daylight_saving = 0;
487  parts.year = utc.m_year;
488  parts.month = utc.m_month;
489  parts.day = utc.m_day;
490  parts.hour = utc.m_hour;
491  parts.minute = utc.m_minute;
492  parts.second = utc.m_second + (utc.m_nano * 1000000000.0);
493 
494  obs->timestamp = mrpt::system::buildTimestampFromParts(parts);
495  }
496  else obs->timestamp = mrpt::system::now();
497 
498  obs->sensorPose = m_sensorPose;
499  obs->sensorLabel = m_sensorLabel;
500 
501  appendObservation(obs);
502 
503  if (packet.containsLatitudeLongitude())
504  {
505  XsVector lla_data = packet.latitudeLongitude();
506 
507  CObservationGPSPtr obsGPS = CObservationGPSPtr( new CObservationGPS() );
510  rGPS.latitude_degrees = lla_data[0];
511  rGPS.longitude_degrees = lla_data[1];
512 
513  if (packet.containsStatus() && packet.status() & XSF_GpsValid)
514  rGPS.validity_char = 'A';
515  else
516  rGPS.validity_char = 'V';
517 
518  if (packet.containsUtcTime())
519  {
520  XsUtcTime utc = packet.utcTime();
521  rGPS.UTCTime.hour = utc.m_hour;
522  rGPS.UTCTime.minute = utc.m_minute;
523  rGPS.UTCTime.sec = utc.m_second + (utc.m_nano * 1000000.0);
524  }
525  else
526  {
527  rGPS.UTCTime.hour = ((obs->timestamp / (60 * 60 * ((uint64_t)1000000 / 100))) % 24);
528  rGPS.UTCTime.minute = ((obs->timestamp / (60 * ((uint64_t)1000000 / 100))) % 60);
529  rGPS.UTCTime.sec = fmod(obs->timestamp / (1000000.0 / 100), 60);
530  }
531 
532  if (packet.containsVelocity())
533  {
534  XsVector vel_data = packet.velocity();
535 
536  rGPS.speed_knots = sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
537  rGPS.direction_degrees = 0; //Could be worked out from velocity and magnatic field perhaps.
538  }
539  else rGPS.speed_knots = rGPS.direction_degrees = 0;
540 
541  obsGPS->setMsg(rGPSs);
542  obsGPS->timestamp = obs->timestamp;
543  obsGPS->originalReceivedTimestamp = obs->timestamp;
544  obsGPS->has_satellite_timestamp = false;
545  obsGPS->sensorPose = m_sensorPose;
546  obsGPS->sensorLabel = m_sensorLabel;
547 
548  appendObservation(obsGPS);
549  }
550 
551  std::cout << std::flush;
552  }
553  msgs.clear();
554 
555 #else
556  THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class cannot be used.");
557 #endif
558 }
559 
560 /*-------------------------------------------------------------
561  initialize
562 -------------------------------------------------------------*/
564 {
565 #if MRPT_HAS_xSENS_MT4
567 
568  try
569  {
570  // Try to open a specified device, or scan the bus?
571  XsPortInfoArray portInfoArray;
572 
573  if (m_portname.empty())
574  {
575  if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
576  xsEnumerateUsbDevices(portInfoArray);
577 
578  if (portInfoArray.empty())
579  THROW_EXCEPTION("CIMUXSens_MT4: No 'portname' was specified and no XSens device was found after scanning the system!")
580 
581  if (m_verbose) cout << "[CIMUXSens_MT4] Found " << portInfoArray.size() <<" devices. Opening the first one.\n";
582  }
583  else
584  {
585  XsPortInfo portInfo(m_portname, XsBaud::numericToRate(m_port_bauds));
586  if (m_verbose) cout << "[CIMUXSens_MT4] Using user-supplied portname '"<<m_portname<<"' at "<<m_port_bauds<<" baudrate.\n";
587  portInfoArray.push_back(portInfo);
588  }
589 
590  // Use the first detected device
591  XsPortInfo mtPort = portInfoArray.at(0);
592 
593  // Open the port with the detected device
594  cout << "[CIMUXSens_MT4] Opening port " << mtPort.portName().toStdString() << std::endl;
595 
596  if (!my_xsens_device.openPort(mtPort))
597  throw std::runtime_error("Could not open port. Aborting.");
598 
599  // Put the device in configuration mode
600  if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into configuration mode...\n";
601  if (!my_xsens_device.gotoConfig()) // Put the device into configuration mode before configuring the device
602  throw std::runtime_error("Could not put device into configuration mode. Aborting.");
603 
604  // Request the device Id to check the device type
605  mtPort.setDeviceId(my_xsens_device.getDeviceId());
606 
607  my_xsens_devid = mtPort.deviceId();
608 
609  // Check if we have an MTi / MTx / MTmk4 device
610  if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
611  {
612  throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
613  }
614  cout << "[CIMUXSens_MT4] Found a device with id: " << mtPort.deviceId().toString().toStdString() << " @ port: " << mtPort.portName().toStdString() << ", baudrate: " << mtPort.baudrate() << std::endl;
615 
616  // Print information about detected MTi / MTx / MTmk4 device
617  if (m_verbose) cout << "[CIMUXSens_MT4] Device: " << my_xsens_device.getProductCode().toStdString() << " opened." << std::endl;
618 
619  // Configure the device. Note the differences between MTix and MTmk4
620  if (m_verbose) cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
621  if (mtPort.deviceId().isMtix())
622  {
623  XsOutputMode outputMode = XOM_Orientation; // output orientation data
624  XsOutputSettings outputSettings = XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; // output orientation data as quaternion
625 
626  // set the device configuration
627  if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
628  throw std::runtime_error("Could not configure MT device. Aborting.");
629  }
630  else if (mtPort.deviceId().isMtMk4())
631  {
632  XsOutputConfigurationArray configArray;
633  configArray.push_back( XsOutputConfiguration(XDI_SampleTime64,m_sampleFreq) );
634  configArray.push_back( XsOutputConfiguration(XDI_SampleTimeFine,m_sampleFreq) );
635  configArray.push_back( XsOutputConfiguration(XDI_SampleTimeCoarse,m_sampleFreq) );
636  configArray.push_back( XsOutputConfiguration(XDI_Quaternion,m_sampleFreq) );
637  configArray.push_back( XsOutputConfiguration(XDI_Temperature,m_sampleFreq) );
638  configArray.push_back( XsOutputConfiguration(XDI_Acceleration,m_sampleFreq) );
639  configArray.push_back( XsOutputConfiguration(XDI_RateOfTurn,m_sampleFreq) );
640  configArray.push_back( XsOutputConfiguration(XDI_MagneticField,m_sampleFreq) );
641  configArray.push_back( XsOutputConfiguration(XDI_VelocityXYZ,m_sampleFreq) );
642 
643  configArray.push_back( XsOutputConfiguration(XDI_StatusByte, m_sampleFreq) );
644  configArray.push_back( XsOutputConfiguration(XDI_LatLon, m_sampleFreq) );
645  configArray.push_back( XsOutputConfiguration(XDI_UtcTime, m_sampleFreq) );
646  configArray.push_back( XsOutputConfiguration(XDI_AltitudeEllipsoid, m_sampleFreq) );
647 
648  if (!my_xsens_device.setOutputConfiguration(configArray))
649  throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
650  }
651  else
652  {
653  throw std::runtime_error("Unknown device while configuring. Aborting.");
654  }
655 
656  // Put the device in measurement mode
657  if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;
658  if (!my_xsens_device.gotoMeasurement())
659  throw std::runtime_error("Could not put device into measurement mode. Aborting.");
660 
661  m_state = ssWorking;
662 
663  }
664  catch(std::exception &)
665  {
666  m_state = ssError;
667  std::cerr << "Error Could not initialize the device" << std::endl;
668  throw;
669  }
670 
671 #else
672  THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class cannot be used.");
673 #endif
674 }
675 
676 /*-------------------------------------------------------------
677  loadConfig_sensorSpecific
678 -------------------------------------------------------------*/
680  const mrpt::utils::CConfigFileBase &configSource,
681  const std::string &iniSection )
682 {
684  configSource.read_float( iniSection, "pose_x", 0, false ),
685  configSource.read_float( iniSection, "pose_y", 0, false ),
686  configSource.read_float( iniSection, "pose_z", 0, false ),
687  DEG2RAD( configSource.read_float( iniSection, "pose_yaw", 0, false ) ),
688  DEG2RAD( configSource.read_float( iniSection, "pose_pitch", 0, false ) ),
689  DEG2RAD( configSource.read_float( iniSection, "pose_roll", 0, false ) ) );
690 
691  m_sampleFreq = configSource.read_int(iniSection, "sampleFreq", m_sampleFreq, false );
692 
693  m_port_bauds = configSource.read_int(iniSection, "baudRate", m_port_bauds, false );
694 
695 #ifdef MRPT_OS_WINDOWS
696  m_portname = configSource.read_string(iniSection, "portname_WIN", m_portname, false );
697 #else
698  m_portname = configSource.read_string(iniSection, "portname_LIN", m_portname, false );
699 #endif
700 
701 
702 }
XsOutputMode
Bit values for legacy output mode.
Definition: xsoutputmode.h:16
Operation was performed successfully.
Definition: xsens_std.h:32
mrpt::system::TTimeStamp m_timeStartTT
Definition: CIMUXSens_MT4.h:60
content_t fields
Message content, accesible by individual fields.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
void XsDataPacket_assignFromXsLegacyDataPacket(struct XsDataPacket *thisPtr, struct LegacyDataPacket const *pack, int index)
unsigned __int16 uint16_t
Definition: rptypes.h:46
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz) method must throw an exception w...
virtual ~CIMUXSens_MT4()
Destructor.
void setMessage(const XsMessage &message)
Set the source message to msg.
mrpt::poses::CPose3D m_sensorPose
Definition: CIMUXSens_MT4.h:62
Operation aborted because of no data read.
Definition: xsens_std.h:61
std::string m_sensorLabel
See CGenericSensor.
#define THROW_EXCEPTION(msg)
orientation pitch absolute value (global/navigation frame) (rad)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
A stream interface.
mrpt::system::TTimeStamp BASE_IMPEXP buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
Definition: datetime.cpp:127
Scalar * iterator
Definition: eigen_plugins.h:23
struct XsByteArray XsByteArray
Definition: xsbytearray.h:25
bool setDataFormat(const XsDataFormat &format, int32_t index=0)
Sets the data format of the device with the given index to format.
uint8_t m_day
The day of the month (if date is valid)
Definition: xsutctime.h:19
uint8_t day_of_week
Seconds (0.0000-59.9999)
Definition: datetime.h:43
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
uint16_t m_year
The year (if date is valid)
Definition: xsutctime.h:17
temperature (degrees Celsius)
x magnetic field value (local/vehicle frame) (gauss)
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:17
y-axis acceleration (local/vehicle frame) (m/sec2)
Orientation Quaternion X (global/navigation frame)
struct XsOutputConfiguration XsOutputConfiguration
z-axis acceleration (local/vehicle frame) (m/sec2)
x-axis velocity (global/navigation frame) (m/sec)
int8_t validity_char
This will be: 'A'=OK or 'V'=void.
uint8_t m_minute
The minute (if time is valid)
Definition: xsutctime.h:21
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
std::string m_portname
The USB or COM port name (if blank -> autodetect)
Definition: CIMUXSens_MT4.h:56
int m_port_bauds
Baudrate, only for COM ports.
Definition: CIMUXSens_MT4.h:55
uint32_t m_nano
Nanosecond part of the time.
Definition: xsutctime.h:16
This class allows loading and storing values and vectors of different types from a configuration text...
struct XsMessageArray XsMessageArray
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
pitch angular velocity (local/vehicle frame) (rad/sec)
int daylight_saving
Day of week (1:Sunday, 7:Saturday)
Definition: datetime.h:44
bool xsEnumerateUsbDevices(XsPortInfoList &ports)
Enumerate Xsens USB devices.
XsResultValue
Xsens result values.
Definition: xsresultvalue.h:26
Stores the location of a message in a buffer using a start position and a size.
Structure for storing a single message.
Definition: xsmessage.h:138
int m_startPos
The offset of the first byte of the message or -1 if no message.
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
uint8_t m_hour
The hour (if time is valid)
Definition: xsutctime.h:20
The parts of a date/time (it's like the standard 'tm' but with fractions of seconds).
Definition: datetime.h:35
virtual MessageLocation findMessage(XsMessage &rcv, const XsByteArray &raw) const
Find the first message in the raw byte stream.
void initialize()
Turns on the xSens device and configure it for getting orientation data.
bool isValid() const
Returns whether the stored message information describes a valid message.
uint8_t day
Month (1-12)
Definition: datetime.h:39
void setDeviceId(XsDeviceId deviceId, int32_t index)
Sets the device ID of the device with the given index to deviceid.
struct XsDeviceId XsDeviceId
Definition: xsdeviceid.h:246
Orientation Quaternion Y (global/navigation frame)
Orientation Quaternion Z (global/navigation frame)
static int composeMessage(XsByteArray &raw, const XsMessage &msg)
Compose a message for transmission.
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
#define DEG2RAD
z magnetic field value (local/vehicle frame) (gauss)
Orientation Quaternion W (global/navigation frame)
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define my_xsens_devid
XsXbusMessageId
Xsens Xbus Message Identifiers.
GLuint res
Definition: glew.h:7143
double second
Minute (0-59)
Definition: datetime.h:42
y magnetic field value (local/vehicle frame) (gauss)
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
unsigned __int64 uint64_t
Definition: rptypes.h:52
Message protocol handling class.
Contains an MTData XsMessage and supports functions for extracting contained data.
void setXbusSystem(bool xbus, bool convert=false)
Sets the xbus flag.
XsOutputSettings
Bit values for output settings.
The low-level serial communication class.
int m_size
The size of the message, when less than 0 it indicates the expected message size. ...
uint8_t minute
Hour (0-23)
Definition: datetime.h:41
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A structure for storing UTC Time values.
Definition: xsutctime.h:15
GLint location
Definition: glew.h:1742
double direction_degrees
Measured speed direction (in degrees)
struct XsPortInfoArray XsPortInfoArray
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:254
uint8_t month
The year.
Definition: datetime.h:38
Contains data received from a device or read from a file.
Definition: xsdatapacket.h:180
A class for interfacing XSens 4th generation Inertial Measuring Units (IMUs): MTi 10-series...
Definition: CIMUXSens_MT4.h:51
void appendObservation(const mrpt::utils::CSerializablePtr &obj)
Like appendObservations() but for just one observation.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
uint8_t hour
Day (1-31)
Definition: datetime.h:40
uint8_t m_month
The month (if date is valid)
Definition: xsutctime.h:18
orientation yaw absolute value (global/navigation frame) (rad)
#define my_xsens_device
y-axis velocity (global/navigation frame) (m/sec)
orientation roll absolute value (global/navigation frame) (rad)
struct XsString XsString
Definition: xsstring.h:34
yaw angular velocity (local/vehicle frame) (rad/sec)
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
roll angular velocity (local/vehicle frame) (rad/sec)
unsigned __int32 uint32_t
Definition: rptypes.h:49
x-axis acceleration (local/vehicle frame) (m/sec2)
z-axis velocity (global/navigation frame) (m/sec)
uint8_t m_second
The second (if time is valid)
Definition: xsutctime.h:22
struct XsOutputConfigurationArray XsOutputConfigurationArray
An IoInterface for dealing specifically with Xsens USB devices.
Definition: usbinterface.h:34
Is set when the device has a GPS receiver and the receiver says that there is a GPS position fix...
Definition: xsstatusflag.h:24
altitude from an altimeter (meters)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018