Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
15 
16 #include <iostream>
17 #include <thread>
18 
20 
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(nullptr) {}
62  ~DeviceClass()
63  {
64  if (m_streamInterface)
65  {
66  delete m_streamInterface;
67  m_streamInterface = nullptr;
68  }
69  }
70 
71  /*! \brief Open an IO device
72  \param portInfo The info to use for opening the port
73  \return True when successful
74  */
75  bool openPort(const XsPortInfo& portInfo)
76  {
77  if (portInfo.isUsb())
78  m_streamInterface = new UsbInterface();
79  else
80  m_streamInterface = new SerialInterface();
81 
82  if (m_streamInterface->open(portInfo) != XRV_OK) return false;
83  return true;
84  }
85 
86  /*! \brief Close an IO device
87  */
88  void close()
89  {
90  if (m_streamInterface) m_streamInterface->close();
91  }
92 
93  /*! \brief Read available data from the open IO device
94  \details This function will attempt to read all available data from the
95  open device (COM port
96  or USB port).
97  The function will read from the device, but it won't wait for data to
98  become available.
99  \param raw A XsByteArray to where the read data will be stored.
100  \return Whether data has been read from the IO device
101  */
102  XsResultValue readDataToBuffer(XsByteArray& raw)
103  {
104  // always read data and append it to the cache before doing analysis
105  const int maxSz = 8192;
106  XsResultValue res = m_streamInterface->readData(maxSz, raw);
107  if (raw.size()) return XRV_OK;
108 
109  return res;
110  }
111 
112  /*! \brief Read all messages from the buffered read data after adding new
113  data supplied in \a rawIn
114  \details This function will read all present messages in the read
115  buffer. In order for this function
116  to work, you need to call readDataToBuffer() first.
117  \param rawIn The buffered data in which to search for messages
118  \param messages The messages found in the data
119  \return The messages that were read.
120  */
121  XsResultValue processBufferedData(
122  XsByteArray& rawIn, XsMessageArray& messages)
123  {
124  ProtocolHandler protocol;
125 
126  if (rawIn.size()) m_dataBuffer.append(rawIn);
127 
128  int popped = 0;
129  messages.clear();
130 
131  for (;;)
132  {
133  XsByteArray raw(
134  m_dataBuffer.data() + popped, m_dataBuffer.size() - popped);
135  XsMessage message;
136  MessageLocation location = protocol.findMessage(message, raw);
137 
138  if (location.isValid())
139  {
140  // message is valid, remove data from cache
141  popped += location.m_size + location.m_startPos;
142  messages.push_back(message);
143  }
144  else
145  {
146  if (popped) m_dataBuffer.pop_front(popped);
147 
148  if (messages.empty()) return XRV_TIMEOUTNODATA;
149 
150  return XRV_OK;
151  }
152  }
153  }
154 
155  /*! \brief Wait for the requested XsXbusMessageId
156  \param xmid The message id to wait for
157  \param rcv The received message
158  \return Whether the requested message was found
159  */
160  bool waitForMessage(XsXbusMessageId xmid, XsMessage& rcv)
161  {
163  XsMessageArray msgs;
164  bool foundAck = false;
165  do
166  {
167  readDataToBuffer(data);
168  processBufferedData(data, msgs);
169  for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end();
170  ++it)
171  if ((*it).getMessageId() == xmid)
172  {
173  foundAck = true;
174  rcv = *it;
175  }
176  } while (!foundAck);
177  return foundAck;
178  }
179 
180  /*! \brief Write a message to the IO device
181  \param msg The message to write
182  \return Whether the message could be written
183  */
184  bool writeMessage(const XsMessage& msg)
185  {
186  XsByteArray raw;
187  if (ProtocolHandler::composeMessage(raw, msg) < 0) return false;
188 
189  return (m_streamInterface->writeData(raw) == XRV_OK);
190  }
191 
192  /*! \brief Put a device in config mode
193  \return True when the device acknowledged config mode
194  */
195  bool gotoConfig()
196  {
197  XsMessage snd(XMID_GotoConfig, 0), rcv;
198  writeMessage(snd);
199 
200  return waitForMessage(XMID_GotoConfigAck, rcv);
201  }
202 
203  /*! \brief Put a device in measurement mode
204  \return True when the device acknowledged measurement mode
205  */
206  bool gotoMeasurement()
207  {
208  XsMessage snd(XMID_GotoMeasurement, 0), rcv;
209  writeMessage(snd);
210 
211  return waitForMessage(XMID_GotoMeasurementAck, rcv);
212  }
213 
214  /*! \brief Request the product code from a device
215  \return The product code when ok, otherwise an empty XsString
216  */
217  XsString getProductCode()
218  {
219  XsMessage snd(XMID_ReqProductCode, 0), rcv;
220  writeMessage(snd);
221 
222  if (waitForMessage(XMID_ProductCode, rcv))
223  {
224  const char* pc = (const char*)rcv.getDataBuffer(0);
225  std::string result(pc ? pc : "", rcv.getDataSize());
226  std::string::size_type thingy = result.find(" ");
227  if (thingy < 20)
228  result.erase(
229  result.begin() + thingy, result.end()); // lint !e534
230  return XsString(result);
231  }
232  else
233  return XsString();
234  }
235 
236  /*! \brief Request the device id from a device
237  \return The device id (XsDeviceId) when ok, otherwise an empty
238  XsDeviceId
239  */
240  XsDeviceId getDeviceId()
241  {
242  XsMessage snd(XMID_ReqDid, 0), rcv;
243  writeMessage(snd);
244 
245  if (waitForMessage(XMID_DeviceId, rcv))
246  {
247  return rcv.getDataLong();
248  }
249  else
250  return XsDeviceId();
251  }
252 
253  /*! \brief Set the device mode of a device (outputmode and outputsettings)
254  \param outputMode The XsOutputMode to set
255  \param outputSettings The XsOutputSettings to set
256  \return True when successful
257  */
258  bool setDeviceMode(
259  const XsOutputMode& outputMode, const XsOutputSettings& outputSettings)
260  {
262 
263  sndOM.resizeData(2);
264  sndOM.setDataShort((uint16_t)outputMode);
265  writeMessage(sndOM);
266  if (!waitForMessage(XMID_SetOutputModeAck, rcv)) return false;
267 
269  snd.resizeData(4);
270  snd.setDataLong((uint32_t)outputSettings);
271  writeMessage(sndOS);
272  if (!waitForMessage(XMID_SetOutputSettingsAck, rcv)) return false;
273 
274  return true;
275  }
276 
277  /*! \brief Set the output configuration of a device
278  \param config An array XsOutputConfigurationArray) containing the one or
279  multiple XsOutputConfigurations
280  \return True when successful
281  */
282  bool setOutputConfiguration(XsOutputConfigurationArray& config)
283  {
285  if (config.size() == 0)
286  {
287  snd.setDataShort((uint16_t)XDI_None, 0);
288  snd.setDataShort(0, 2);
289  }
290  else
291  {
292  for (XsSize i = 0; i < (XsSize)config.size(); ++i)
293  {
294  snd.setDataShort((uint16_t)config[i].m_dataIdentifier, i * 4);
295  snd.setDataShort(config[i].m_frequency, i * 4 + 2);
296  }
297  }
298  writeMessage(snd);
299 
300  return waitForMessage(XMID_SetOutputConfigurationAck, rcv);
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 _WIN32
316 // WINDOWS:
317 #if defined(_MSC_VER)
318 #pragma comment(lib, "SetupAPI.lib")
319 #pragma comment(lib, "WinUsb.lib")
320 #endif
321 #endif // _WIN32
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(nullptr),
335  m_devid_ptr(nullptr)
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
344  "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
345  "cannot be used.");
346 #endif
347 }
348 
349 /*-------------------------------------------------------------
350  ~CIMUXSens_MT4
351 -------------------------------------------------------------*/
353 {
354 #if MRPT_HAS_xSENS_MT4
355  my_xsens_device.close();
356  delete static_cast<DeviceClass*>(m_dev_ptr);
357  m_dev_ptr = nullptr;
358 
359  delete static_cast<XsDeviceId*>(m_devid_ptr);
360  m_devid_ptr = nullptr;
361 #endif
362 }
363 
364 /*-------------------------------------------------------------
365  doProcess
366 -------------------------------------------------------------*/
368 {
369 #if MRPT_HAS_xSENS_MT4
370  if (m_state == ssError)
371  {
372  std::this_thread::sleep_for(200ms);
373  initialize();
374  }
375 
376  if (m_state == ssError) return;
377 
379  XsMessageArray msgs;
380 
381  my_xsens_device.readDataToBuffer(data);
382  my_xsens_device.processBufferedData(data, msgs);
383  for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
384  {
385  // Retrieve a packet
386  XsDataPacket packet;
387  if ((*it).getMessageId() == XMID_MtData)
388  {
389  LegacyDataPacket lpacket(1, false);
390 
391  lpacket.setMessage((*it));
392  lpacket.setXbusSystem(false, false);
393  lpacket.setDeviceId(my_xsens_devid, 0);
394  lpacket.setDataFormat(
397  XOS_CalibratedMode_All /*XOS_OrientationMode_Quaternion*/,
398  0); // lint !e534
399  XsDataPacket_assignFromXsLegacyDataPacket(&packet, &lpacket, 0);
400  }
401  else if ((*it).getMessageId() == XMID_MtData2)
402  {
403  packet.setMessage((*it));
404  packet.setDeviceId(my_xsens_devid);
405  }
406 
407  // Data properly collected: extract data fields
408  // -------------------------------------------------
409  m_state = ssWorking;
410  CObservationIMU::Ptr obs = mrpt::make_aligned_shared<CObservationIMU>();
411 
412  if (packet.containsOrientation())
413  {
414  XsEuler euler = packet.orientationEuler();
415  obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler.yaw());
416  obs->dataIsPresent[IMU_YAW] = true;
417  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler.pitch());
418  obs->dataIsPresent[IMU_PITCH] = true;
419  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler.roll());
420  obs->dataIsPresent[IMU_ROLL] = true;
421 
422  XsQuaternion quat = packet.orientationQuaternion();
423  obs->rawMeasurements[IMU_ORI_QUAT_X] = quat.x();
424  obs->dataIsPresent[IMU_ORI_QUAT_X] = true;
425  obs->rawMeasurements[IMU_ORI_QUAT_Y] = quat.y();
426  obs->dataIsPresent[IMU_ORI_QUAT_Y] = true;
427  obs->rawMeasurements[IMU_ORI_QUAT_Z] = quat.z();
428  obs->dataIsPresent[IMU_ORI_QUAT_Z] = true;
429  obs->rawMeasurements[IMU_ORI_QUAT_W] = quat.w();
430  obs->dataIsPresent[IMU_ORI_QUAT_W] = true;
431  }
432 
433  if (packet.containsCalibratedAcceleration())
434  {
435  XsVector acc_data = packet.calibratedAcceleration();
436  obs->rawMeasurements[IMU_X_ACC] = acc_data[0];
437  obs->dataIsPresent[IMU_X_ACC] = true;
438  obs->rawMeasurements[IMU_Y_ACC] = acc_data[1];
439  obs->dataIsPresent[IMU_Y_ACC] = true;
440  obs->rawMeasurements[IMU_Z_ACC] = acc_data[2];
441  obs->dataIsPresent[IMU_Z_ACC] = true;
442  }
443 
444  if (packet.containsCalibratedGyroscopeData())
445  {
446  XsVector gyr_data = packet.calibratedGyroscopeData();
447  obs->rawMeasurements[IMU_YAW_VEL] = gyr_data[2];
448  obs->dataIsPresent[IMU_YAW_VEL] = true;
449  obs->rawMeasurements[IMU_PITCH_VEL] = gyr_data[1];
450  obs->dataIsPresent[IMU_PITCH_VEL] = true;
451  obs->rawMeasurements[IMU_ROLL_VEL] = gyr_data[0];
452  obs->dataIsPresent[IMU_ROLL_VEL] = true;
453  }
454 
455  if (packet.containsCalibratedMagneticField())
456  {
457  XsVector mag_data = packet.calibratedMagneticField();
458  obs->rawMeasurements[IMU_MAG_X] = mag_data[0];
459  obs->dataIsPresent[IMU_MAG_X] = true;
460  obs->rawMeasurements[IMU_MAG_Y] = mag_data[1];
461  obs->dataIsPresent[IMU_MAG_Y] = true;
462  obs->rawMeasurements[IMU_MAG_Z] = mag_data[2];
463  obs->dataIsPresent[IMU_MAG_Z] = true;
464  }
465 
466  if (packet.containsVelocity())
467  {
468  XsVector vel_data = packet.velocity();
469  obs->rawMeasurements[IMU_X_VEL] = vel_data[0];
470  obs->dataIsPresent[IMU_X_VEL] = true;
471  obs->rawMeasurements[IMU_Y_VEL] = vel_data[1];
472  obs->dataIsPresent[IMU_Y_VEL] = true;
473  obs->rawMeasurements[IMU_Z_VEL] = vel_data[2];
474  obs->dataIsPresent[IMU_Z_VEL] = true;
475  }
476 
477  if (packet.containsTemperature())
478  {
479  obs->rawMeasurements[IMU_TEMPERATURE] = packet.temperature();
480  obs->dataIsPresent[IMU_TEMPERATURE] = true;
481  }
482 
483  if (packet.containsAltitude())
484  {
485  obs->rawMeasurements[IMU_ALTITUDE] = packet.altitude();
486  obs->dataIsPresent[IMU_ALTITUDE] = true;
487  }
488 
489  // TimeStamp
490  if (packet.containsSampleTime64())
491  {
492  const uint64_t nowUI = packet.sampleTime64();
493 
494  uint64_t AtUI = 0;
495  if (m_timeStartUI == 0)
496  {
497  m_timeStartUI = nowUI;
499  }
500  else
501  AtUI = nowUI - m_timeStartUI;
502 
503  double AtDO =
504  AtUI * 1000.0; // Difference in intervals of 100 nsecs
505  obs->timestamp = m_timeStartTT + AtDO;
506  }
507  else if (packet.containsUtcTime())
508  {
509  XsUtcTime utc = packet.utcTime();
510 
512 
513  parts.day_of_week = 0;
514  parts.daylight_saving = 0;
515  parts.year = utc.m_year;
516  parts.month = utc.m_month;
517  parts.day = utc.m_day;
518  parts.hour = utc.m_hour;
519  parts.minute = utc.m_minute;
520  parts.second = utc.m_second + (utc.m_nano * 1000000000.0);
521 
522  obs->timestamp = mrpt::system::buildTimestampFromParts(parts);
523  }
524  else
525  obs->timestamp = mrpt::system::now();
526 
527  obs->sensorPose = m_sensorPose;
528  obs->sensorLabel = m_sensorLabel;
529 
530  appendObservation(obs);
531 
532  if (packet.containsLatitudeLongitude())
533  {
534  XsVector lla_data = packet.latitudeLongitude();
535 
536  CObservationGPS::Ptr obsGPS =
540  rGPS.latitude_degrees = lla_data[0];
541  rGPS.longitude_degrees = lla_data[1];
542 
543  if (packet.containsStatus() && packet.status() & XSF_GpsValid)
544  rGPS.validity_char = 'A';
545  else
546  rGPS.validity_char = 'V';
547 
548  if (packet.containsUtcTime())
549  {
550  XsUtcTime utc = packet.utcTime();
551  rGPS.UTCTime.hour = utc.m_hour;
552  rGPS.UTCTime.minute = utc.m_minute;
553  rGPS.UTCTime.sec = utc.m_second + (utc.m_nano * 1000000.0);
554  }
555  else
556  {
557  rGPS.UTCTime.hour =
558  ((obs->timestamp / (60 * 60 * ((uint64_t)1000000 / 100))) %
559  24);
560  rGPS.UTCTime.minute =
561  ((obs->timestamp / (60 * ((uint64_t)1000000 / 100))) % 60);
562  rGPS.UTCTime.sec = fmod(obs->timestamp / (1000000.0 / 100), 60);
563  }
564 
565  if (packet.containsVelocity())
566  {
567  XsVector vel_data = packet.velocity();
568 
569  rGPS.speed_knots =
570  sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
571  rGPS.direction_degrees = 0; // Could be worked out from
572  // velocity and magnatic field
573  // perhaps.
574  }
575  else
576  rGPS.speed_knots = rGPS.direction_degrees = 0;
577 
578  obsGPS->setMsg(rGPSs);
579  obsGPS->timestamp = obs->timestamp;
580  obsGPS->originalReceivedTimestamp = obs->timestamp;
581  obsGPS->has_satellite_timestamp = false;
582  obsGPS->sensorPose = m_sensorPose;
583  obsGPS->sensorLabel = m_sensorLabel;
584 
585  appendObservation(obsGPS);
586  }
587  }
588  msgs.clear();
589 #else
591  "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
592  "cannot be used.");
593 #endif
594 }
595 
596 /*-------------------------------------------------------------
597  initialize
598 -------------------------------------------------------------*/
600 {
601 #if MRPT_HAS_xSENS_MT4
603 
604  try
605  {
606  // Try to open a specified device, or scan the bus?
607  XsPortInfoArray portInfoArray;
608 
609  if (m_portname.empty())
610  {
611  if (m_verbose)
612  cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
613  xsEnumerateUsbDevices(portInfoArray);
614 
615  if (portInfoArray.empty())
617  "CIMUXSens_MT4: No 'portname' was specified and no XSens "
618  "device was found after scanning the system!")
619 
620  if (m_verbose)
621  cout << "[CIMUXSens_MT4] Found " << portInfoArray.size()
622  << " devices. Opening the first one.\n";
623  }
624  else
625  {
626  XsPortInfo portInfo(
627  m_portname, XsBaud::numericToRate(m_port_bauds));
628  if (m_verbose)
629  cout << "[CIMUXSens_MT4] Using user-supplied portname '"
630  << m_portname << "' at " << m_port_bauds << " baudrate.\n";
631  portInfoArray.push_back(portInfo);
632  }
633 
634  // Use the first detected device
635  XsPortInfo mtPort = portInfoArray.at(0);
636 
637  // Open the port with the detected device
638  cout << "[CIMUXSens_MT4] Opening port "
639  << mtPort.portName().toStdString() << std::endl;
640 
641  if (!my_xsens_device.openPort(mtPort))
642  throw std::runtime_error("Could not open port. Aborting.");
643 
644  // Put the device in configuration mode
645  if (m_verbose)
646  cout << "[CIMUXSens_MT4] Putting device into configuration "
647  "mode...\n";
648  if (!my_xsens_device.gotoConfig()) // Put the device into configuration
649  // mode before configuring the
650  // device
651  throw std::runtime_error(
652  "Could not put device into configuration mode. Aborting.");
653 
654  // Request the device Id to check the device type
655  mtPort.setDeviceId(my_xsens_device.getDeviceId());
656 
657  my_xsens_devid = mtPort.deviceId();
658 
659  // Check if we have an MTi / MTx / MTmk4 device
660  if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
661  {
662  throw std::runtime_error(
663  "No MTi / MTx / MTmk4 device found. Aborting.");
664  }
665  cout << "[CIMUXSens_MT4] Found a device with id: "
666  << mtPort.deviceId().toString().toStdString()
667  << " @ port: " << mtPort.portName().toStdString()
668  << ", baudrate: " << mtPort.baudrate() << std::endl;
669 
670  // Print information about detected MTi / MTx / MTmk4 device
671  if (m_verbose)
672  cout << "[CIMUXSens_MT4] Device: "
673  << my_xsens_device.getProductCode().toStdString() << " opened."
674  << std::endl;
675 
676  // Configure the device. Note the differences between MTix and MTmk4
677  if (m_verbose)
678  cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
679  if (mtPort.deviceId().isMtix())
680  {
681  XsOutputMode outputMode =
682  XOM_Orientation; // output orientation data
683  XsOutputSettings outputSettings =
685  XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; //
686  // output orientation data as
687  // quaternion
688 
689  // set the device configuration
690  if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
691  throw std::runtime_error(
692  "Could not configure MT device. Aborting.");
693  }
694  else if (mtPort.deviceId().isMtMk4())
695  {
696  XsOutputConfigurationArray configArray;
697  configArray.push_back(
699  configArray.push_back(
701  configArray.push_back(
703  configArray.push_back(
705  configArray.push_back(
707  configArray.push_back(
709  configArray.push_back(
711  configArray.push_back(
713  configArray.push_back(
715 
716  configArray.push_back(
718  configArray.push_back(
720  configArray.push_back(
722  configArray.push_back(
724 
725  if (!my_xsens_device.setOutputConfiguration(configArray))
726  throw std::runtime_error(
727  "Could not configure MTmk4 device. Aborting.");
728  }
729  else
730  {
731  throw std::runtime_error(
732  "Unknown device while configuring. Aborting.");
733  }
734 
735  // Put the device in measurement mode
736  if (m_verbose)
737  cout << "[CIMUXSens_MT4] Putting device into measurement mode..."
738  << std::endl;
739  if (!my_xsens_device.gotoMeasurement())
740  throw std::runtime_error(
741  "Could not put device into measurement mode. Aborting.");
742 
743  m_state = ssWorking;
744  }
745  catch (std::exception&)
746  {
747  m_state = ssError;
748  std::cerr << "Error Could not initialize the device" << std::endl;
749  throw;
750  }
751 
752 #else
754  "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
755  "cannot be used.");
756 #endif
757 }
758 
759 /*-------------------------------------------------------------
760  loadConfig_sensorSpecific
761 -------------------------------------------------------------*/
763  const mrpt::config::CConfigFileBase& configSource,
764  const std::string& iniSection)
765 {
767  configSource.read_float(iniSection, "pose_x", 0, false),
768  configSource.read_float(iniSection, "pose_y", 0, false),
769  configSource.read_float(iniSection, "pose_z", 0, false),
770  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
771  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
772  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
773 
774  m_sampleFreq =
775  configSource.read_int(iniSection, "sampleFreq", m_sampleFreq, false);
776 
777  m_port_bauds =
778  configSource.read_int(iniSection, "baudRate", m_port_bauds, false);
779 
780 #ifdef _WIN32
781  m_portname =
782  configSource.read_string(iniSection, "portname_WIN", m_portname, false);
783 #else
784  m_portname =
785  configSource.read_string(iniSection, "portname_LIN", m_portname, false);
786 #endif
787 }
XsOutputMode
Bit values for legacy output mode.
Definition: xsoutputmode.h:16
Operation was performed successfully.
Definition: xsens_std.h:34
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
mrpt::system::TTimeStamp m_timeStartTT
Definition: CIMUXSens_MT4.h:69
Scalar * iterator
Definition: eigen_plugins.h:26
content_t fields
Message content, accesible by individual fields.
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:44
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
virtual ~CIMUXSens_MT4()
Destructor.
void setMessage(const XsMessage &message)
Set the source message to msg.
mrpt::poses::CPose3D m_sensorPose
Definition: CIMUXSens_MT4.h:71
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
GLint location
Definition: glext.h:4086
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
orientation pitch absolute value (global/navigation frame) (rad)
A stream interface.
mrpt::system::TTimeStamp buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
Definition: datetime.cpp:128
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:24
uint8_t day_of_week
Seconds (0.0000-59.9999)
Definition: datetime.h:45
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
uint16_t m_year
The year (if date is valid)
Definition: xsutctime.h:20
temperature (degrees Celsius)
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
x magnetic field value (local/vehicle frame) (gauss)
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:19
y-axis acceleration (local/vehicle frame) (m/sec2)
STL namespace.
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: &#39;A&#39;=OK or &#39;V&#39;=void.
uint8_t m_minute
The minute (if time is valid)
Definition: xsutctime.h:28
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:65
int m_port_bauds
Baudrate, only for COM ports.
Definition: CIMUXSens_MT4.h:63
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
uint32_t m_nano
Nanosecond part of the time.
Definition: xsutctime.h:18
struct XsMessageArray XsMessageArray
pitch angular velocity (local/vehicle frame) (rad/sec)
int daylight_saving
Day of week (1:Sunday, 7:Saturday)
Definition: datetime.h:46
bool xsEnumerateUsbDevices(XsPortInfoList &ports)
Enumerate Xsens USB devices.
This class allows loading and storing values and vectors of different types from a configuration text...
XsResultValue
Xsens result values.
Definition: xsresultvalue.h:27
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:198
uint8_t m_hour
The hour (if time is valid)
Definition: xsutctime.h:26
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:37
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.
uint8_t day
Month (1-12)
Definition: datetime.h:41
void setDeviceId(XsDeviceId deviceId, int32_t index)
Sets the device ID of the device with the given index to deviceid.
This namespace contains representation of robot actions and observations.
struct XsDeviceId XsDeviceId
Definition: xsdeviceid.h:235
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.
z magnetic field value (local/vehicle frame) (gauss)
GLsizei const GLchar ** string
Definition: glext.h:4101
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.
double second
Minute (0-59)
Definition: datetime.h:44
y magnetic field value (local/vehicle frame) (gauss)
unsigned __int64 uint64_t
Definition: rptypes.h:50
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:43
A structure for storing UTC Time values.
Definition: xsutctime.h:15
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:239
uint8_t month
The year.
Definition: datetime.h:40
Contains data received from a device or read from a file.
Definition: xsdatapacket.h:302
A class for interfacing XSens 4th generation Inertial Measuring Units (IMUs): MTi 10-series...
Definition: CIMUXSens_MT4.h:58
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
uint8_t hour
Day (1-31)
Definition: datetime.h:42
uint8_t m_month
The month (if date is valid)
Definition: xsutctime.h:22
orientation yaw absolute value (global/navigation frame) (rad)
#define my_xsens_device
y-axis velocity (global/navigation frame) (m/sec)
GLuint res
Definition: glext.h:7268
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:47
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
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:30
struct XsOutputConfigurationArray XsOutputConfigurationArray
An IoInterface for dealing specifically with Xsens USB devices.
Definition: usbinterface.h:35
altitude from an altimeter (meters)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019