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  */
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>
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()
57  {
58  if (m_streamInterface)
59  {
60  delete m_streamInterface;
61  m_streamInterface = nullptr;
62  }
63  }
64 
65  /*! \brief Open an IO device
66  \param portInfo The info to use for opening the port
67  \return True when successful
68  */
69  bool openPort(const XsPortInfo& portInfo)
70  {
71  if (portInfo.isUsb())
72  m_streamInterface = new UsbInterface();
73  else
74  m_streamInterface = new SerialInterface();
75 
76  if (m_streamInterface->open(portInfo) != XRV_OK) return false;
77  return true;
78  }
79 
80  /*! \brief Close an IO device
81  */
82  void close()
83  {
84  if (m_streamInterface) m_streamInterface->close();
85  }
86 
87  /*! \brief Read available data from the open IO device
88  \details This function will attempt to read all available data from the
89  open device (COM port
90  or USB port).
91  The function will read from the device, but it won't wait for data to
92  become available.
93  \param raw A XsByteArray to where the read data will be stored.
94  \return Whether data has been read from the IO device
95  */
96  XsResultValue readDataToBuffer(XsByteArray& raw)
97  {
98  // always read data and append it to the cache before doing analysis
99  const int maxSz = 8192;
100  XsResultValue res = m_streamInterface->readData(maxSz, raw);
101  if (raw.size()) return XRV_OK;
102 
103  return res;
104  }
105 
106  /*! \brief Read all messages from the buffered read data after adding new
107  data supplied in \a rawIn
108  \details This function will read all present messages in the read
109  buffer. In order for this function
110  to work, you need to call readDataToBuffer() first.
111  \param rawIn The buffered data in which to search for messages
112  \param messages The messages found in the data
113  \return The messages that were read.
114  */
115  XsResultValue processBufferedData(
116  XsByteArray& rawIn, XsMessageArray& messages)
117  {
118  ProtocolHandler protocol;
119 
120  if (rawIn.size()) m_dataBuffer.append(rawIn);
121 
122  int popped = 0;
123  messages.clear();
124 
125  for (;;)
126  {
127  XsByteArray raw(
128  m_dataBuffer.data() + popped, m_dataBuffer.size() - popped);
129  XsMessage message;
130  MessageLocation location = protocol.findMessage(message, raw);
131 
132  if (location.isValid())
133  {
134  // message is valid, remove data from cache
135  popped += location.m_size + location.m_startPos;
136  messages.push_back(message);
137  }
138  else
139  {
140  if (popped) m_dataBuffer.pop_front(popped);
141 
142  if (messages.empty()) return XRV_TIMEOUTNODATA;
143 
144  return XRV_OK;
145  }
146  }
147  }
148 
149  /*! \brief Wait for the requested XsXbusMessageId
150  \param xmid The message id to wait for
151  \param rcv The received message
152  \return Whether the requested message was found
153  */
154  bool waitForMessage(XsXbusMessageId xmid, XsMessage& rcv)
155  {
157  XsMessageArray msgs;
158  bool foundAck = false;
159  do
160  {
161  readDataToBuffer(data);
162  processBufferedData(data, msgs);
163  for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end();
164  ++it)
165  if ((*it).getMessageId() == xmid)
166  {
167  foundAck = true;
168  rcv = *it;
169  }
170  } while (!foundAck);
171  return foundAck;
172  }
173 
174  /*! \brief Write a message to the IO device
175  \param msg The message to write
176  \return Whether the message could be written
177  */
178  bool writeMessage(const XsMessage& msg)
179  {
180  XsByteArray raw;
181  if (ProtocolHandler::composeMessage(raw, msg) < 0) return false;
182 
183  return (m_streamInterface->writeData(raw) == XRV_OK);
184  }
185 
186  /*! \brief Put a device in config mode
187  \return True when the device acknowledged config mode
188  */
189  bool gotoConfig()
190  {
191  XsMessage snd(XMID_GotoConfig, 0), rcv;
192  writeMessage(snd);
193 
194  return waitForMessage(XMID_GotoConfigAck, rcv);
195  }
196 
197  /*! \brief Put a device in measurement mode
198  \return True when the device acknowledged measurement mode
199  */
200  bool gotoMeasurement()
201  {
202  XsMessage snd(XMID_GotoMeasurement, 0), rcv;
203  writeMessage(snd);
204 
205  return waitForMessage(XMID_GotoMeasurementAck, rcv);
206  }
207 
208  /*! \brief Request the product code from a device
209  \return The product code when ok, otherwise an empty XsString
210  */
211  XsString getProductCode()
212  {
213  XsMessage snd(XMID_ReqProductCode, 0), rcv;
214  writeMessage(snd);
215 
216  if (waitForMessage(XMID_ProductCode, rcv))
217  {
218  const char* pc = (const char*)rcv.getDataBuffer(0);
219  std::string result(pc ? pc : "", rcv.getDataSize());
220  std::string::size_type thingy = result.find(" ");
221  if (thingy < 20)
222  result.erase(
223  result.begin() + thingy, result.end()); // lint !e534
224  return XsString(result);
225  }
226  else
227  return XsString();
228  }
229 
230  /*! \brief Request the device id from a device
231  \return The device id (XsDeviceId) when ok, otherwise an empty
232  XsDeviceId
233  */
234  XsDeviceId getDeviceId()
235  {
236  XsMessage snd(XMID_ReqDid, 0), rcv;
237  writeMessage(snd);
238 
239  if (waitForMessage(XMID_DeviceId, rcv))
240  {
241  return rcv.getDataLong();
242  }
243  else
244  return XsDeviceId();
245  }
246 
247  /*! \brief Set the device mode of a device (outputmode and outputsettings)
248  \param outputMode The XsOutputMode to set
249  \param outputSettings The XsOutputSettings to set
250  \return True when successful
251  */
252  bool setDeviceMode(
253  const XsOutputMode& outputMode, const XsOutputSettings& outputSettings)
254  {
256 
257  sndOM.resizeData(2);
258  sndOM.setDataShort((uint16_t)outputMode);
259  writeMessage(sndOM);
260  if (!waitForMessage(XMID_SetOutputModeAck, rcv)) return false;
261 
263  snd.resizeData(4);
264  snd.setDataLong((uint32_t)outputSettings);
265  writeMessage(sndOS);
266  if (!waitForMessage(XMID_SetOutputSettingsAck, rcv)) return false;
267 
268  return true;
269  }
270 
271  /*! \brief Set the output configuration of a device
272  \param config An array XsOutputConfigurationArray) containing the one or
273  multiple XsOutputConfigurations
274  \return True when successful
275  */
276  bool setOutputConfiguration(XsOutputConfigurationArray& config)
277  {
279  if (config.size() == 0)
280  {
281  snd.setDataShort((uint16_t)XDI_None, 0);
282  snd.setDataShort(0, 2);
283  }
284  else
285  {
286  for (XsSize i = 0; i < (XsSize)config.size(); ++i)
287  {
288  snd.setDataShort((uint16_t)config[i].m_dataIdentifier, i * 4);
289  snd.setDataShort(config[i].m_frequency, i * 4 + 2);
290  }
291  }
292  writeMessage(snd);
293 
294  return waitForMessage(XMID_SetOutputConfigurationAck, rcv);
295  }
296 
297  private:
298  StreamInterface* m_streamInterface{nullptr};
299  XsByteArray m_dataBuffer;
300 };
301 #endif
302 
303 // Adaptors for the "void*" memory blocks:
304 #define my_xsens_device (*static_cast<DeviceClass*>(m_dev_ptr))
305 #define my_xsens_devid (*static_cast<XsDeviceId*>(m_devid_ptr))
306 
307 // Include libraries in linking:
308 #if MRPT_HAS_xSENS_MT4
309 #ifdef _WIN32
310 // WINDOWS:
311 #if defined(_MSC_VER)
312 #pragma comment(lib, "SetupAPI.lib")
313 #pragma comment(lib, "WinUsb.lib")
314 #endif
315 #endif // _WIN32
316 #endif // MRPT_HAS_xSENS_MT4
317 
319 
320 using namespace mrpt::obs;
321 using namespace mrpt::hwdrivers;
322 using namespace std;
323 
324 /*-------------------------------------------------------------
325  CIMUXSens_MT4
326 -------------------------------------------------------------*/
327 CIMUXSens_MT4::CIMUXSens_MT4() : m_portname(), m_timeStartTT(), m_sensorPose()
328 
329 {
330  m_sensorLabel = "XSensMTi_MT4";
331 
332 #if MRPT_HAS_xSENS_MT4
333  m_dev_ptr = new DeviceClass;
334  m_devid_ptr = new XsDeviceId;
335 #else
337  "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
338  "cannot be used.");
339 #endif
340 }
341 
342 /*-------------------------------------------------------------
343  ~CIMUXSens_MT4
344 -------------------------------------------------------------*/
346 {
347 #if MRPT_HAS_xSENS_MT4
348  my_xsens_device.close();
349  delete static_cast<DeviceClass*>(m_dev_ptr);
350  m_dev_ptr = nullptr;
351 
352  delete static_cast<XsDeviceId*>(m_devid_ptr);
353  m_devid_ptr = nullptr;
354 #endif
355 }
356 
357 /*-------------------------------------------------------------
358  doProcess
359 -------------------------------------------------------------*/
361 {
362 #if MRPT_HAS_xSENS_MT4
363  if (m_state == ssError)
364  {
365  std::this_thread::sleep_for(200ms);
366  initialize();
367  }
368 
369  if (m_state == ssError) return;
370 
372  XsMessageArray msgs;
373 
374  my_xsens_device.readDataToBuffer(data);
375  my_xsens_device.processBufferedData(data, msgs);
376  for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
377  {
378  // Retrieve a packet
379  XsDataPacket packet;
380  if ((*it).getMessageId() == XMID_MtData)
381  {
382  LegacyDataPacket lpacket(1, false);
383 
384  lpacket.setMessage((*it));
385  lpacket.setXbusSystem(false, false);
386  lpacket.setDeviceId(my_xsens_devid, 0);
387  lpacket.setDataFormat(
390  XOS_CalibratedMode_All /*XOS_OrientationMode_Quaternion*/,
391  0); // lint !e534
392  XsDataPacket_assignFromXsLegacyDataPacket(&packet, &lpacket, 0);
393  }
394  else if ((*it).getMessageId() == XMID_MtData2)
395  {
396  packet.setMessage((*it));
397  packet.setDeviceId(my_xsens_devid);
398  }
399 
400  // Data properly collected: extract data fields
401  // -------------------------------------------------
402  m_state = ssWorking;
403  CObservationIMU::Ptr obs = std::make_shared<CObservationIMU>();
404 
405  if (packet.containsOrientation())
406  {
407  XsEuler euler = packet.orientationEuler();
408  obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler.yaw());
409  obs->dataIsPresent[IMU_YAW] = true;
410  obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler.pitch());
411  obs->dataIsPresent[IMU_PITCH] = true;
412  obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler.roll());
413  obs->dataIsPresent[IMU_ROLL] = true;
414 
415  XsQuaternion quat = packet.orientationQuaternion();
416  obs->rawMeasurements[IMU_ORI_QUAT_X] = quat.x();
417  obs->dataIsPresent[IMU_ORI_QUAT_X] = true;
418  obs->rawMeasurements[IMU_ORI_QUAT_Y] = quat.y();
419  obs->dataIsPresent[IMU_ORI_QUAT_Y] = true;
420  obs->rawMeasurements[IMU_ORI_QUAT_Z] = quat.z();
421  obs->dataIsPresent[IMU_ORI_QUAT_Z] = true;
422  obs->rawMeasurements[IMU_ORI_QUAT_W] = quat.w();
423  obs->dataIsPresent[IMU_ORI_QUAT_W] = true;
424  }
425 
426  if (packet.containsCalibratedAcceleration())
427  {
428  XsVector acc_data = packet.calibratedAcceleration();
429  obs->rawMeasurements[IMU_X_ACC] = acc_data[0];
430  obs->dataIsPresent[IMU_X_ACC] = true;
431  obs->rawMeasurements[IMU_Y_ACC] = acc_data[1];
432  obs->dataIsPresent[IMU_Y_ACC] = true;
433  obs->rawMeasurements[IMU_Z_ACC] = acc_data[2];
434  obs->dataIsPresent[IMU_Z_ACC] = true;
435  }
436 
437  if (packet.containsCalibratedGyroscopeData())
438  {
439  XsVector gyr_data = packet.calibratedGyroscopeData();
440  obs->rawMeasurements[IMU_YAW_VEL] = gyr_data[2];
441  obs->dataIsPresent[IMU_YAW_VEL] = true;
442  obs->rawMeasurements[IMU_PITCH_VEL] = gyr_data[1];
443  obs->dataIsPresent[IMU_PITCH_VEL] = true;
444  obs->rawMeasurements[IMU_ROLL_VEL] = gyr_data[0];
445  obs->dataIsPresent[IMU_ROLL_VEL] = true;
446  }
447 
448  if (packet.containsCalibratedMagneticField())
449  {
450  XsVector mag_data = packet.calibratedMagneticField();
451  obs->rawMeasurements[IMU_MAG_X] = mag_data[0];
452  obs->dataIsPresent[IMU_MAG_X] = true;
453  obs->rawMeasurements[IMU_MAG_Y] = mag_data[1];
454  obs->dataIsPresent[IMU_MAG_Y] = true;
455  obs->rawMeasurements[IMU_MAG_Z] = mag_data[2];
456  obs->dataIsPresent[IMU_MAG_Z] = true;
457  }
458 
459  if (packet.containsVelocity())
460  {
461  XsVector vel_data = packet.velocity();
462  obs->rawMeasurements[IMU_X_VEL] = vel_data[0];
463  obs->dataIsPresent[IMU_X_VEL] = true;
464  obs->rawMeasurements[IMU_Y_VEL] = vel_data[1];
465  obs->dataIsPresent[IMU_Y_VEL] = true;
466  obs->rawMeasurements[IMU_Z_VEL] = vel_data[2];
467  obs->dataIsPresent[IMU_Z_VEL] = true;
468  }
469 
470  if (packet.containsTemperature())
471  {
472  obs->rawMeasurements[IMU_TEMPERATURE] = packet.temperature();
473  obs->dataIsPresent[IMU_TEMPERATURE] = true;
474  }
475 
476  if (packet.containsAltitude())
477  {
478  obs->rawMeasurements[IMU_ALTITUDE] = packet.altitude();
479  obs->dataIsPresent[IMU_ALTITUDE] = true;
480  }
481 
482  // TimeStamp
483  if (packet.containsSampleTime64())
484  {
485  const uint64_t nowUI = packet.sampleTime64();
486 
487  uint64_t AtUI = 0;
488  if (m_timeStartUI == 0)
489  {
490  m_timeStartUI = nowUI;
492  }
493  else
494  AtUI = nowUI - m_timeStartUI; // ms
495 
496  obs->timestamp = m_timeStartTT + std::chrono::milliseconds(AtUI);
497  }
498  else if (packet.containsUtcTime())
499  {
500  XsUtcTime utc = packet.utcTime();
501 
503 
504  parts.day_of_week = 0;
505  parts.daylight_saving = 0;
506  parts.year = utc.m_year;
507  parts.month = utc.m_month;
508  parts.day = utc.m_day;
509  parts.hour = utc.m_hour;
510  parts.minute = utc.m_minute;
511  parts.second = utc.m_second + (utc.m_nano * 1000000000.0);
512 
513  obs->timestamp = mrpt::system::buildTimestampFromParts(parts);
514  }
515  else
516  obs->timestamp = mrpt::system::now();
517 
518  obs->sensorPose = m_sensorPose;
519  obs->sensorLabel = m_sensorLabel;
520 
521  appendObservation(obs);
522 
523  if (packet.containsLatitudeLongitude())
524  {
525  XsVector lla_data = packet.latitudeLongitude();
526 
527  CObservationGPS::Ptr obsGPS = CObservationGPS::Create();
530  rGPS.latitude_degrees = lla_data[0];
531  rGPS.longitude_degrees = lla_data[1];
532 
533  if (packet.containsStatus() && packet.status() & XSF_GpsValid)
534  rGPS.validity_char = 'A';
535  else
536  rGPS.validity_char = 'V';
537 
538  if (packet.containsUtcTime())
539  {
540  XsUtcTime utc = packet.utcTime();
541  rGPS.UTCTime.hour = utc.m_hour;
542  rGPS.UTCTime.minute = utc.m_minute;
543  rGPS.UTCTime.sec = utc.m_second + (utc.m_nano * 1000000.0);
544  }
545  else
546  {
547  rGPS.UTCTime.hour =
548  ((obs->timestamp.time_since_epoch().count() /
549  (60 * 60 * ((uint64_t)1000000 / 100))) %
550  24);
551  rGPS.UTCTime.minute =
552  ((obs->timestamp.time_since_epoch().count() /
553  (60 * ((uint64_t)1000000 / 100))) %
554  60);
555  rGPS.UTCTime.sec = fmod(
556  obs->timestamp.time_since_epoch().count() /
557  (1000000.0 / 100),
558  60);
559  }
560 
561  if (packet.containsVelocity())
562  {
563  XsVector vel_data = packet.velocity();
564 
565  rGPS.speed_knots =
566  sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
567  rGPS.direction_degrees = 0; // Could be worked out from
568  // velocity and magnatic field
569  // perhaps.
570  }
571  else
572  rGPS.speed_knots = rGPS.direction_degrees = 0;
573 
574  obsGPS->setMsg(rGPSs);
575  obsGPS->timestamp = obs->timestamp;
576  obsGPS->originalReceivedTimestamp = obs->timestamp;
577  obsGPS->has_satellite_timestamp = false;
578  obsGPS->sensorPose = m_sensorPose;
579  obsGPS->sensorLabel = m_sensorLabel;
580 
581  appendObservation(obsGPS);
582  }
583  }
584  msgs.clear();
585 #else
587  "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
588  "cannot be used.");
589 #endif
590 }
591 
592 /*-------------------------------------------------------------
593  initialize
594 -------------------------------------------------------------*/
596 {
597 #if MRPT_HAS_xSENS_MT4
599 
600  try
601  {
602  // Try to open a specified device, or scan the bus?
603  XsPortInfoArray portInfoArray;
604 
605  if (m_portname.empty())
606  {
607  if (m_verbose)
608  cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
609  xsEnumerateUsbDevices(portInfoArray);
610 
611  if (portInfoArray.empty())
613  "CIMUXSens_MT4: No 'portname' was specified and no XSens "
614  "device was found after scanning the system!");
615 
616  if (m_verbose)
617  cout << "[CIMUXSens_MT4] Found " << portInfoArray.size()
618  << " devices. Opening the first one.\n";
619  }
620  else
621  {
622  XsPortInfo portInfo(
623  m_portname, XsBaud::numericToRate(m_port_bauds));
624  if (m_verbose)
625  cout << "[CIMUXSens_MT4] Using user-supplied portname '"
626  << m_portname << "' at " << m_port_bauds << " baudrate.\n";
627  portInfoArray.push_back(portInfo);
628  }
629 
630  // Use the first detected device
631  XsPortInfo mtPort = portInfoArray.at(0);
632 
633  // Open the port with the detected device
634  cout << "[CIMUXSens_MT4] Opening port "
635  << mtPort.portName().toStdString() << std::endl;
636 
637  if (!my_xsens_device.openPort(mtPort))
638  throw std::runtime_error("Could not open port. Aborting.");
639 
640  // Put the device in configuration mode
641  if (m_verbose)
642  cout << "[CIMUXSens_MT4] Putting device into configuration "
643  "mode...\n";
644  if (!my_xsens_device.gotoConfig()) // Put the device into configuration
645  // mode before configuring the
646  // device
647  throw std::runtime_error(
648  "Could not put device into configuration mode. Aborting.");
649 
650  // Request the device Id to check the device type
651  mtPort.setDeviceId(my_xsens_device.getDeviceId());
652 
653  my_xsens_devid = mtPort.deviceId();
654 
655  // Check if we have an MTi / MTx / MTmk4 device
656  if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
657  {
658  throw std::runtime_error(
659  "No MTi / MTx / MTmk4 device found. Aborting.");
660  }
661  cout << "[CIMUXSens_MT4] Found a device with id: "
662  << mtPort.deviceId().toString().toStdString()
663  << " @ port: " << mtPort.portName().toStdString()
664  << ", baudrate: " << mtPort.baudrate() << std::endl;
665 
666  // Print information about detected MTi / MTx / MTmk4 device
667  if (m_verbose)
668  cout << "[CIMUXSens_MT4] Device: "
669  << my_xsens_device.getProductCode().toStdString() << " opened."
670  << std::endl;
671 
672  // Configure the device. Note the differences between MTix and MTmk4
673  if (m_verbose)
674  cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
675  if (mtPort.deviceId().isMtix())
676  {
677  XsOutputMode outputMode =
678  XOM_Orientation; // output orientation data
679  XsOutputSettings outputSettings =
681  XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; //
682  // output orientation data as
683  // quaternion
684 
685  // set the device configuration
686  if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
687  throw std::runtime_error(
688  "Could not configure MT device. Aborting.");
689  }
690  else if (mtPort.deviceId().isMtMk4())
691  {
692  XsOutputConfigurationArray configArray;
693  configArray.push_back(
695  configArray.push_back(
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 
712  configArray.push_back(
714  configArray.push_back(
716  configArray.push_back(
718  configArray.push_back(
720 
721  if (!my_xsens_device.setOutputConfiguration(configArray))
722  throw std::runtime_error(
723  "Could not configure MTmk4 device. Aborting.");
724  }
725  else
726  {
727  throw std::runtime_error(
728  "Unknown device while configuring. Aborting.");
729  }
730 
731  // Put the device in measurement mode
732  if (m_verbose)
733  cout << "[CIMUXSens_MT4] Putting device into measurement mode..."
734  << std::endl;
735  if (!my_xsens_device.gotoMeasurement())
736  throw std::runtime_error(
737  "Could not put device into measurement mode. Aborting.");
738 
739  m_state = ssWorking;
740  }
741  catch (std::exception&)
742  {
743  m_state = ssError;
744  std::cerr << "Error Could not initialize the device" << std::endl;
745  throw;
746  }
747 
748 #else
750  "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
751  "cannot be used.");
752 #endif
753 }
754 
755 /*-------------------------------------------------------------
756  loadConfig_sensorSpecific
757 -------------------------------------------------------------*/
759  const mrpt::config::CConfigFileBase& configSource,
760  const std::string& iniSection)
761 {
763  configSource.read_float(iniSection, "pose_x", 0, false),
764  configSource.read_float(iniSection, "pose_y", 0, false),
765  configSource.read_float(iniSection, "pose_z", 0, false),
766  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
767  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
768  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
769 
770  m_sampleFreq =
771  configSource.read_int(iniSection, "sampleFreq", m_sampleFreq, false);
772 
773  m_port_bauds =
774  configSource.read_int(iniSection, "baudRate", m_port_bauds, false);
775 
776 #ifdef _WIN32
777  m_portname =
778  configSource.read_string(iniSection, "portname_WIN", m_portname, false);
779 #else
780  m_portname =
781  configSource.read_string(iniSection, "portname_LIN", m_portname, false);
782 #endif
783 }
XsOutputMode
Bit values for legacy output mode.
Definition: xsoutputmode.h:16
void initialize() override
Turns on the xSens device and configure it for getting orientation data.
Operation was performed successfully.
Definition: xsens_std.h:35
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 XsDataPacket_assignFromXsLegacyDataPacket(struct XsDataPacket *thisPtr, struct LegacyDataPacket const *pack, int index)
unsigned __int16 uint16_t
Definition: rptypes.h:47
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
void setMessage(const XsMessage &message)
Set the source message to msg.
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)
GLint location
Definition: glext.h:4101
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:74
struct XsByteArray XsByteArray
Definition: xsbytearray.h:26
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:57
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
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: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
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:58
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:28
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:202
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
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:49
MessageLocation findMessage(XsMessage &rcv, const XsByteArray &raw) const override
Find the first message in the raw byte stream.
uint8_t day
Month (1-12)
Definition: datetime.h:53
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:247
Orientation Quaternion Y (global/navigation frame)
~CIMUXSens_MT4() override
Destructor.
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:4116
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:56
y magnetic field value (local/vehicle frame) (gauss)
unsigned __int64 uint64_t
Definition: rptypes.h:53
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:55
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:256
uint8_t month
The year.
Definition: datetime.h:52
Contains data received from a device or read from a file.
Definition: xsdatapacket.h:315
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
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:7385
orientation roll absolute value (global/navigation frame) (rad)
struct XsString XsString
Definition: xsstring.h:35
yaw angular velocity (local/vehicle frame) (rad/sec)
roll angular velocity (local/vehicle frame) (rad/sec)
unsigned __int32 uint32_t
Definition: rptypes.h:50
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019