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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020