mtdevice.cpp
Go to the documentation of this file.
1 
2 // Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without modification,
6 // are permitted provided that the following conditions are met:
7 //
8 // 1. Redistributions of source code must retain the above copyright notice,
9 // this list of conditions, and the following disclaimer.
10 //
11 // 2. Redistributions in binary form must reproduce the above copyright notice,
12 // this list of conditions, and the following disclaimer in the documentation
13 // and/or other materials provided with the distribution.
14 //
15 // 3. Neither the names of the copyright holders nor the names of their contributors
16 // may be used to endorse or promote products derived from this software without
17 // specific prior written permission.
18 //
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
20 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
22 // THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
24 // OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
26 // TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
27 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS
28 // SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES
29 // OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE
30 // ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
31 //
32 
33 
34 // Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
35 // All rights reserved.
36 //
37 // Redistribution and use in source and binary forms, with or without modification,
38 // are permitted provided that the following conditions are met:
39 //
40 // 1. Redistributions of source code must retain the above copyright notice,
41 // this list of conditions, and the following disclaimer.
42 //
43 // 2. Redistributions in binary form must reproduce the above copyright notice,
44 // this list of conditions, and the following disclaimer in the documentation
45 // and/or other materials provided with the distribution.
46 //
47 // 3. Neither the names of the copyright holders nor the names of their contributors
48 // may be used to endorse or promote products derived from this software without
49 // specific prior written permission.
50 //
51 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
52 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
53 // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
54 // THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
55 // SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
56 // OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
57 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
58 // TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
59 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS
60 // SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES
61 // OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE
62 // ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
63 //
64 
65 #include "mtdevice.h"
66 #include "xsdef.h"
67 #include <xstypes/xssensorranges.h>
69 #include <xstypes/xsvector3.h>
70 #include "xsselftestresult.h"
71 #include <xstypes/xsstatusflag.h>
72 #include <algorithm>
73 
74 // Undef the windows min macro that conflict with e.g. std::numeric_limits<..>::min
75 #ifdef min
76  #undef min
77 #endif
78 
79 using namespace xsens;
80 
84  : XsDeviceEx(id)
85 {
86 }
87 
91  : XsDeviceEx(comm)
92 {
93 }
94 
97 MtDevice::MtDevice(XsDevice* master, const XsDeviceId& childDeviceId)
98  : XsDeviceEx(master, childDeviceId)
99 {
100 }
101 
105 {
106 }
107 
112 bool MtDevice::messageLooksSane(const XsMessage& msg) const
113 {
114  return msg.getBusId() == 1 || XsDevice::messageLooksSane(msg);
115 }
116 
121 {
122  if (!XsDeviceEx::initialize())
123  return false;
124 
125  // we must create the data caches first so they are available even if the rest of the init fails
126  // when reading from file almost all init can fail but we can still read data into the caches
128  {
129  setInitialized(false);
130  return false;
131  }
132 
135 
136  return true;
137 }
138 
142 {
143  const XsMtDeviceConfiguration& info = deviceConfigurationConst().deviceInfo(deviceId());
144  if (info.m_filterProfile != 0)
145  {
147  , info.m_filterProfile >> 8
148  , m_hardwareFilterProfile.kind()
149  , m_hardwareFilterProfile.label()
150  , info.m_filterType
151  , info.m_filterMajor
152  , info.m_filterMinor);
153  }
154 
155  for (auto i = m_hardwareFilterProfiles.begin(); i != m_hardwareFilterProfiles.end(); ++i)
156  {
157  if (i->type() == m_hardwareFilterProfile.type() || i->label() == m_hardwareFilterProfile.label())
158  {
159  m_hardwareFilterProfile.setLabel(i->label());
160  m_hardwareFilterProfile.setKind(i->kind());
161  m_hardwareFilterProfile.setVersion(i->version());
162  break;
163  }
164  }
165 }
166 
170 {
171  return true;
172 }
173 
177 {
178  return XsDevice::updateRateForDataIdentifier(dataType);
179 }
180 
184 {
186  if (!doTransaction(snd, rcv))
187  return 0;
188 
189  return rcv.getDataShort();
190 }
191 
195 {
196  XsMessage snd(XMID_ReqPeriod), rcv;
197  if (!doTransaction(snd, rcv))
198  return 0;
199 
200  return rcv.getDataShort();
201 }
202 
206 {
208  if (!doTransaction(snd, rcv))
209  return 0;
210 
211  return rcv.getDataShort();
212 }
213 
217 {
218  XsMessage snd(XMID_ReqOptionFlags), rcv;
219  if (doTransaction(snd, rcv))
220  return (XsDeviceOptionFlag)rcv.getDataLong();
221  return XDOF_None;
222 }
223 
227 {
229  if (doTransaction(snd, rcv))
230  return static_cast<XsUbloxGnssPlatform>(rcv.getDataShort());
231  return XGP_Portable;
232 }
233 
237 {
239  snd.setBusId(busId());
240  snd.setDataShort((uint16_t)ubloxGnssPlatform);
241  if (!doTransaction(snd))
242  return false;
243  return true;
244 }
245 
249 {
251  snd.setBusId(busId());
252  if (doTransaction(snd, rcv))
253  {
255  gnssReceiverSettings[0] = (int)rcv.getDataShort(0);//receiver type
256  gnssReceiverSettings[1] = (int)rcv.getDataShort(2);//receiver baud rate
257  gnssReceiverSettings[2] = (int)rcv.getDataShort(4);//receiver input rate
258  gnssReceiverSettings[3] = (int)rcv.getDataLong(6);//receiver options
259  return gnssReceiverSettings;
260  }
261  return XsIntArray();
262 }
263 
266 bool MtDevice::setGnssReceiverSettings(const XsIntArray& gnssReceiverSettings)
267 {
269  snd.setBusId(busId());
270 
271  if (gnssReceiverSettings.size() > 0)
272  snd.setDataShort((uint16_t)gnssReceiverSettings[0], 0);//receiver type
273  if (gnssReceiverSettings.size() > 1)
274  snd.setDataShort((uint16_t)gnssReceiverSettings[1], 2);//receiver baud rate
275  if (gnssReceiverSettings.size() > 2)
276  snd.setDataShort((uint16_t)gnssReceiverSettings[2], 4);//receiver input rate
277  if (gnssReceiverSettings.size() > 3)
278  snd.setDataLong((uint32_t)gnssReceiverSettings[3], 6);//receiver options
279 
280  XsMessage ack;
281  if (!doTransaction(snd, ack))
282  return false;
283 
284  return true;
285 }
286 
290 {
292 }
293 
299 {
300  switch (method)
301  {
303  case XRM_DefaultHeading:
305  return true;
306 
307  case XRM_None:
308  return false;
309 
310  default:
311  break;
312  }
313 
315 }
316 
319 {
320  switch (deviceState())
321  {
322  case XDS_Measurement:
323  case XDS_Recording:
325  case XDS_FlushingData:
326  if (method == XRM_StoreAlignmentMatrix)
327  return false;
328 
331  return false;
332 
333  break;
334 
335  case XDS_Config:
336  if (method != XRM_StoreAlignmentMatrix)
337  return false;
338 
340  {
341  if (!storeAlignmentMatrix())
342  return false;
343  // read stored value from emts by reinitializing
344  return reinitialize();
345  }
346  return true;
347 
348  case XDS_Initial:
349  case XDS_Destructing:
350  default:
351  return false;
352  }
353  return true;
354 }
355 
365 {
367  return false;
368 
369  return true;
370 }
371 
375 {
376  XsMessage snd(XMID_ReqHeading), rcv;
377  snd.setBusId(busId());
378 
379  if (!doTransaction(snd, rcv))
380  return 0;
381 
382  return (double)rcv.getDataFloat();
383 }
384 
388 {
390  snd.setBusId(busId());
391  snd.setDataShort((uint16_t)id);
392 
393  return doTransaction(snd);
394 }
395 
399 {
400  XsMessage snd(XMID_ReqLocationId), rcv;
401  snd.setBusId(busId());
402 
403  if (!doTransaction(snd, rcv))
404  return 0;
405 
406  return rcv.getDataShort();
407 }
408 
412 {
413  XsMessage snd(XMID_ReqBaudrate), rcv;
414  snd.setBusId(busId());
415 
416  if (!doTransaction(snd, rcv))
417  return XBR_Invalid;
418 
419  return (XsBaudRate)rcv.getDataByte();
420 }
421 
425 {
427  if (!doTransaction(snd, rcv))
428  return XsVersion();
429  uint16_t hwv = rcv.getDataShort();
430  return XsVersion(hwv >> 8, hwv & 0xff);
431 }
432 
436 {
438 }
439 
444 {
445  XsFilterProfileArray result;
446 
448  snd.setBusId(busId());
449 
450  XsMessage rcv;
451  if (!doTransaction(snd, rcv))
452  return result;
453 
454  const char filterType = deviceConfigurationConst().deviceInfo(deviceId()).m_filterType;
455 
456  XsSize nofScenarios = rcv.getDataSize() / (1 + 1 + XS_LEN_FILTERPROFILELABEL);
457 
458  result.resize(nofScenarios);
459  for (XsSize i = 0; i < nofScenarios; ++i)
460  {
461  uint8_t type = rcv.getDataByte(i * (1 + 1 + XS_LEN_FILTERPROFILELABEL));
462  result[i].setType(type);
463  result[i].setVersion(rcv.getDataByte(1 + i * (1 + 1 + XS_LEN_FILTERPROFILELABEL)));
464  result[i].setLabel((const char*) rcv.getDataBuffer(2 + i * (1 + 1 + XS_LEN_FILTERPROFILELABEL)));
465  result[i].setFilterType(filterType);
466  XsString kind;
467  if (type == XFPK_Base)
468  kind = "base";
469  else if (type == XFPK_Additional)
470  kind = "additional";
471  else if (type == XFPK_Heading)
472  kind = "heading";
473  result[i].setKind(kind.c_str());
474  }
475  return result;
476 }
477 
481 {
482  m_hardwareFilterProfiles.clear();
484  std::sort(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(),
485  [](XsFilterProfile const & left, XsFilterProfile const & right)
486  {
487  if (left.type() == right.type())
488  return strcmp(left.label(), right.label()) < 0;
489  else
490  return left.type() < right.type();
491  }
492  );
493 }
494 
498 {
499  XsMessage snd(XMID_ReqProductCode), rcv;
500  if (!doTransaction(snd, rcv))
501  return XsString();
502 
503  const char* pc = (const char*) rcv.getDataBuffer();
504  assert(pc);
505  std::string result(pc ? pc : " ", 20);
506  std::string::difference_type thingy = (std::string::difference_type) result.find(" ");
507  if (thingy < 20)
508  result.erase(result.begin() + thingy, result.end());
509  return XsString(result);
510 }
511 
515 {
517  return false;
518 
519  clearDataCache();
521  return true;
522 }
523 
527 {
529  return false;
530 
531  return reinitialize();
532 }
533 
537 {
539  snd.setBusId(busId());
540  XsMessage rcv;
541 
542  //tries to get the current filter profile from the device
543  if (!doTransaction(snd, rcv))
544  return m_hardwareFilterProfile;//in case the transaction with the device fails or if the dev is in measurement
545 
546  auto profilesFromDevice = readFilterProfilesFromDevice();
547  std::string fullMessageData((const char*)rcv.getDataBuffer());
548  if (fullMessageData.empty())//for older devices where the profiles are numbers
549  {
550  auto numericFilter = rcv.getDataShort();
551  //looks-up the full profile data among all the profiles available for the device
552  for (auto currentProfile : profilesFromDevice)
553  {
554  if (currentProfile.type() == numericFilter)
555  return currentProfile;
556  }
557  }
558  else//for newer devices where the profiles are strings
559  {
560  //removes the checksum at the end of the message and keeps only the name of the profile
561  // Stop when we see the first space or at message size, whichever is the lowest.
562  fullMessageData = fullMessageData.substr(0, std::min(rcv.getDataSize(), fullMessageData.find(' ')));
563 
564  //looks up the full profile structure from the ones available on the device, by using the label
565  for (auto currentProfile : profilesFromDevice)
566  {
567  if (currentProfile.label() == fullMessageData)
568  return currentProfile;
569  }
570 
571  //some strings have the names of two profiles separated by a '/', so the above look-up will not work
572  //thus two separate profiles need to be found and "combined"
573  auto splitterPosition = fullMessageData.find('/');
574  if (splitterPosition != std::string::npos)
575  {
576  std::string combinedName;
577  auto firstProfileName = fullMessageData.substr(0, splitterPosition);
578  auto secondProfileName = fullMessageData.substr(splitterPosition + 1, fullMessageData.size());//the '/' is not taken into consideration
579  XsFilterProfile firstProfile, secondProfile, combinedProfile;
580  for (auto currentProfile : profilesFromDevice)
581  {
582  if (currentProfile.label() == firstProfileName)
583  firstProfile = currentProfile;
584  else if (currentProfile.label() == secondProfileName)
585  secondProfile = currentProfile;
586  }
587  //sets all the data from the first profile
588  combinedProfile = firstProfile;
589  //and changes the name to a combination from the first and second profile
590  combinedName.append(firstProfile.label()).append("/").append(secondProfile.label());
591  combinedProfile.setLabel(combinedName.c_str());
592  return combinedProfile;
593  }
594  }
595  return m_hardwareFilterProfile;//in case everything above fails
596 }
597 
601 {
602  if (deviceState() != XDS_Config)
603  return false;
604 
605  XsFilterProfileArray::iterator item = std::find_if(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(),
606  [profileType](XsFilterProfile const & p)
607  {
608  return p.type() == profileType;
609  });
610  if (item == m_hardwareFilterProfiles.end())
611  return false;
612 
614  snd.setBusId(busId());
615  snd.setDataShort((uint16_t)profileType);
616 
617  if (!doTransaction(snd))
618  return false;
619 
620  m_hardwareFilterProfile = *item;
621  return true;
622 }
623 
627 {
628  if (deviceState() != XDS_Config)
629  return false;
630 
631  XsStringArray profileList(profile, "/");
632 
633  XsFilterProfileArray::iterator item[2];
634  int i = 0;
635  for (auto currentProfile : profileList)
636  {
637  item[i++] = std::find_if(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(),
638  [currentProfile](XsFilterProfile const & p)
639  {
640  return currentProfile == p.label();
641  });
642  if (i == 2)
643  break;
644  }
645  if (i == 0 || item[0] == m_hardwareFilterProfiles.end())
646  return false;
647 
648  XsMessage snd(XMID_SetFilterProfile, profile.size());
649  snd.setBusId(busId());
650  snd.setDataBuffer((const uint8_t*)profile.c_str(), profile.size());
651 
652  if (!doTransaction(snd))
653  return false;
654 
655  if (item[1] != m_hardwareFilterProfiles.end())
656  {
657  m_hardwareFilterProfile = *item[0];
658  m_hardwareFilterProfile.setLabel(profile.c_str()); // Use info from first item, but label can be 2 profiles
659  }
660  else
661  m_hardwareFilterProfile = *item[0];
662  return true;
663 }
664 
669 {
670  return ::accelerometerRange(productCode(), hardwareVersion().major());
671 }
672 
677 {
678  return ::gyroscopeRange(productCode());
679 }
680 
685 {
687 }
688 
691 bool MtDevice::setNoRotation(uint16_t duration)
692 {
694  snd.setBusId(busId());
695  snd.setDataShort(duration);
696 
697  return doTransaction(snd);
698 }
699 
707 {
708  uint8_t bid = (uint8_t)busId();
709  if (bid == XS_BID_INVALID || bid == XS_BID_BROADCAST || lla.size() != 3)
710  return false;
711 
713  snd.setDataFloat((float) lla[0], 0);
714  snd.setDataFloat((float) lla[1], 4);
715  snd.setDataFloat((float) lla[2], 8);
716  snd.setBusId(bid);
717 
718  return doTransaction(snd);
719 }
720 
725 {
726  XsVector vec;
727  XsMessage snd(XMID_ReqLatLonAlt), rcv;
728  if (doTransaction(snd, rcv))
729  {
730  vec.setSize(3);
731  for (XsSize i = 0; i < 3; i++)
732  vec[i] = rcv.getDataDouble(i * 8);
733  }
734  return vec;
735 }
736 
740 {
741  return ((uint32_t)(((double) ticks) * XS_SYNC_CLOCK_TICKS_TO_US + 0.5));
742 }
743 
747 {
748  return ((uint32_t)(((double) us) * XS_SYNC_CLOCK_US_TO_TICKS + 0.5));
749 }
750 
755 {
756  XsMessage snd(XMID_ReqErrorMode), rcv;
757  if (!doTransaction(snd, rcv))
758  return XEM_Ignore;
759  return static_cast<XsErrorMode>(rcv.getDataShort());
760 }
761 
770 {
772  snd.setBusId(busId());
773  snd.setDataShort(em);
774  return doTransaction(snd);
775 }
776 
785 {
787  if (!doTransaction(snd, rcv))
788  return 0;
789 
790  return rcv.getDataShort();
791 }
792 
799 {
801  snd.setBusId(busId());
802  snd.setDataShort(delay);
803 
804  return doTransaction(snd);
805 }
806 
813 {
814  XsMessage snd(XMID_ReqData);
815  snd.setBusId(busId());
816 
817  return sendRawMessage(snd);
818 }
819 
824 {
825  XsMessage snd(XMID_RunSelfTest, 0);
826  snd.setBusId(busId());
827  XsMessage rcv;
828  if (!doTransaction(snd, rcv, 3000))
829  return XsSelfTestResult();
830 
831  return XsSelfTestResult::create(rcv.getDataShort());
832 }
833 
837 {
838  if (deviceState() == XDS_Config)
839  {
841  snd.setBusId(busId());
842 
843  if (doTransaction(snd))
844  return true;
845  }
846  return false;
847 }
848 
854 int MtDevice::calcFrequency(int baseFrequency, uint16_t skipFactor)
855 {
856  int result = baseFrequency / (skipFactor + 1);
857  return result;
858 }
859 
861 
874 {
875  JLDEBUGG("");
876 
878  return false;
879 
880  return true;
881 }
882 
884 
886 {
887  // essentially an unknown device, assume everything is supported
888  return ~(uint32_t)0;
889 }
890 
896 {
897  XsString hwtype = findHardwareType(code);
898  if (hwtype.empty())
899  return code;
900 
901  ptrdiff_t offset = code.findSubStr(hwtype);
902  while (offset >= 0 && code[(unsigned int)offset] != '-')
903  --offset;
904 
905  if (offset < 0)
906  return code;
907 
908  return code.mid(0, (unsigned int)offset);
909 }
910 
914 {
915  (void) dataType;
916  return 0;
917 }
XDOF_None
@ XDOF_None
When set to 1, disables all option flags.
Definition: xsdeviceoptionflag.h:92
XEM_Ignore
@ XEM_Ignore
Ignore all errors without warning.
Definition: xserrormode.h:76
XMID_ReqProductCode
@ XMID_ReqProductCode
Definition: xsxbusmessageid.h:118
MtDevice::readFilterProfilesFromDevice
XsFilterProfileArray readFilterProfilesFromDevice() const
Request the filter profiles headers from the hardware device and returns a vector with the found prof...
Definition: mtdevice.cpp:443
XsMtDeviceConfiguration::m_filterType
char m_filterType
The filter type.
Definition: xsdeviceconfiguration.h:110
XsString
struct XsString XsString
Definition: xsstring.h:87
MtDevice::errorMode
XsErrorMode errorMode() const
Definition: mtdevice.cpp:754
XsFilterProfile
struct XsFilterProfile XsFilterProfile
Definition: xsfilterprofile.h:300
MtDevice::setErrorMode
bool setErrorMode(XsErrorMode errorMode)
Set the error mode of the device.
Definition: mtdevice.cpp:769
XS_LEN_SETFILTERPROFILE
#define XS_LEN_SETFILTERPROFILE
Definition: xsdef.h:137
XMID_SetLatLonAlt
@ XMID_SetLatLonAlt
Definition: xsxbusmessageid.h:273
XsMtDeviceConfiguration::m_filterProfile
uint16_t m_filterProfile
The currently chosen filter profile.
Definition: xsdeviceconfiguration.h:106
XMID_ReqGnssPlatform
@ XMID_ReqGnssPlatform
Definition: xsxbusmessageid.h:285
XsDevice::deviceId
XsDeviceId const & deviceId() const
Return the device ID of the device.
Definition: xsdevice_def.cpp:742
XS_LEN_LOCATIONID
#define XS_LEN_LOCATIONID
Definition: xsdef.h:128
XMID_ReqPeriod
@ XMID_ReqPeriod
Definition: xsxbusmessageid.h:82
XsVector
A class that represents a vector of real numbers.
Definition: xsvector.h:113
MtDevice::accelerometerRange
double accelerometerRange() const
Definition: mtdevice.cpp:668
msg
msg
XDS_FlushingData
@ XDS_FlushingData
Definition: xsdevicestate.h:84
MtDevice::setLocationId
virtual bool setLocationId(int id)
Set the location ID of the device.
Definition: mtdevice.cpp:387
MtDevice::deviceOptionFlags
XsDeviceOptionFlag deviceOptionFlags() const override
Returns the device option flags.
Definition: mtdevice.cpp:216
MtDevice::~MtDevice
virtual ~MtDevice()
Destroys the MtDevice.
Definition: mtdevice.cpp:104
MtDevice::stripProductCode
static XsString stripProductCode(const XsString &code)
Helper function to strip the hardware type from the product code.
Definition: mtdevice.cpp:895
XsDevice::deviceState
virtual XsDeviceState deviceState() const
Return the state of this device.
Definition: xsdevice_def.cpp:1064
XFPK_Additional
@ XFPK_Additional
Indicates an additional profile.
Definition: xsfilterprofilekind.h:73
MtDevice::m_hardwareFilterProfile
XsFilterProfile m_hardwareFilterProfile
A hardware filter profile.
Definition: mtdevice.h:181
XMID_SetNoRotation
@ XMID_SetNoRotation
Definition: xsxbusmessageid.h:129
XsFilterProfileArray
A list of XsFilterProfile values.
MtDevice::storeAlignmentMatrix
virtual bool storeAlignmentMatrix()
Store the current alignment matrix in the device.
Definition: mtdevice.cpp:364
XRM_None
@ XRM_None
No reset planned.
Definition: xsresetmethod.h:85
XsDevice::initialize
virtual bool XSNOEXPORT initialize()
MtDevice::canDoOrientationResetInFirmware
virtual bool canDoOrientationResetInFirmware(XsResetMethod method)
Checks if this device can do orientation reset in firmware.
Definition: mtdevice.cpp:298
MtDevice::stringSkipFactor
uint16_t stringSkipFactor() const override
Returns the skipfactor for string output.
Definition: mtdevice.cpp:205
XsDeviceEx
An abstract internal struct of a device.
Definition: xsdevice_public.h:78
MtDevice::productCode
XsString productCode() const
Return the product code of the device.
Definition: mtdevice.cpp:497
XsIntArray
A list of XsInt values.
MtDevice::runSelfTest
XsSelfTestResult runSelfTest()
Run a self test.
Definition: mtdevice.cpp:823
xsstatusflag.h
MtDevice::updateRateForDataIdentifier
int updateRateForDataIdentifier(XsDataIdentifier dataType) const override
Returns the currently configured update rate for the supplied dataType.
Definition: mtdevice.cpp:176
XDS_Initial
@ XDS_Initial
Definition: xsdevicestate.h:79
XsVersion
struct XsVersion XsVersion
Definition: xsversion.h:80
MtDevice::setInitialPositionLLA
bool setInitialPositionLLA(const XsVector &lla) override
Set the current sensor position.
Definition: mtdevice.cpp:706
XsDevice::sendRawMessage
virtual bool sendRawMessage(const XsMessage &message)
Send a message directly to the communicator.
Definition: xsdevice_def.cpp:1311
XsOutputConfigurationArray
struct XsOutputConfigurationArray XsOutputConfigurationArray
Definition: xsoutputconfigurationarray.h:82
MtDevice::headingOffset
double headingOffset() const
The heading offset set for this device.
Definition: mtdevice.cpp:374
XsSelfTestResult
Contains the results of a self-test performed by an Xsens device.
Definition: xsselftestresult.h:105
MtDevice::hardwareVersion
XsVersion hardwareVersion() const
Return the hardware version of the device.
Definition: mtdevice.cpp:424
XMID_SetFilterProfile
@ XMID_SetFilterProfile
Set the current filter profile.
Definition: xsxbusmessageid.h:248
XS_SYNC_CLOCK_US_TO_TICKS
#define XS_SYNC_CLOCK_US_TO_TICKS
Definition: xsdef.h:216
XRM_DefaultInclination
@ XRM_DefaultInclination
Revert to default behaviour for inclination, undoes XRM_Inclination.
Definition: xsresetmethod.h:83
XsStringArray
A list of XsString values.
XMID_ReqOptionFlags
@ XMID_ReqOptionFlags
Definition: xsxbusmessageid.h:197
MtDevice::updateFilterProfiles
virtual void updateFilterProfiles()
Updates the scenarios.
Definition: mtdevice.cpp:141
MtDevice::setRs485TransmissionDelay
bool setRs485TransmissionDelay(uint16_t delay)
Set the RS485 acknowledge transmission delay of the device.
Definition: mtdevice.cpp:798
MtDevice::restoreFactoryDefaults
bool restoreFactoryDefaults()
Restore to factory default settings.
Definition: mtdevice.cpp:526
findHardwareType
static const char * findHardwareType(const char *productCode)
Definition: xssensorranges.cpp:74
XMID_ReqData
@ XMID_ReqData
Definition: xsxbusmessageid.h:165
XMID_ReqErrorMode
@ XMID_ReqErrorMode
Definition: xsxbusmessageid.h:480
MtDevice::setOnboardFilterProfile
bool setOnboardFilterProfile(int profileType) override
Sets the filter profile to use for computing orientations on the device.
Definition: mtdevice.cpp:600
MtDevice::gnssReceiverSettings
XsIntArray gnssReceiverSettings() const override
Gets some GNSS receiver settings.
Definition: mtdevice.cpp:248
XsDevice::updateRateForDataIdentifier
virtual int updateRateForDataIdentifier(XsDataIdentifier dataType) const
Returns the currently configured update rate for the supplied dataType.
Definition: xsdevice_def.cpp:601
XMID_ReqStringOutputType
@ XMID_ReqStringOutputType
Definition: xsxbusmessageid.h:348
MtDevice::syncTicksToUs
uint32_t syncTicksToUs(uint32_t ticks) const
Convert mt sync ticks to microseconds.
Definition: mtdevice.cpp:739
XMID_SetErrorMode
@ XMID_SetErrorMode
Definition: xsxbusmessageid.h:482
MtDevice::stringSamplePeriod
uint16_t stringSamplePeriod() const override
Returns the sample period for string output.
Definition: mtdevice.cpp:194
XsDevice::busId
virtual int busId() const
The bus ID for this device.
Definition: xsdevice_def.cpp:860
MtDevice::onboardFilterProfile
XsFilterProfile onboardFilterProfile() const override
Gets the filter profile in use by the device for computing orientations.
Definition: mtdevice.cpp:536
XsOutputConfigurationArray
A list of XsOutputConfiguration values.
MtDevice::initialize
bool initialize() override
Initialize the Mt device using the supplied filter profiles.
Definition: mtdevice.cpp:120
XsMtDeviceConfiguration::m_filterMinor
uint8_t m_filterMinor
The filter minor version.
Definition: xsdeviceconfiguration.h:112
XMID_RunSelfTest
@ XMID_RunSelfTest
Definition: xsxbusmessageid.h:132
XMID_ReqTransmitDelay
@ XMID_ReqTransmitDelay
Definition: xsxbusmessageid.h:485
XMID_SetGnssPlatform
@ XMID_SetGnssPlatform
Definition: xsxbusmessageid.h:287
XsDataIdentifier
XsDataIdentifier
Defines the data identifiers.
Definition: xsdataidentifier.h:84
XsFilterProfile
Contains information about an available filter profile.
Definition: xsfilterprofile.h:92
Communicator
A base struct for a communication interface.
Definition: communicator.h:95
XsBaudRate
enum XsBaudRate XsBaudRate
Communication speed.
Definition: xsbaud.h:81
XsDevice::deviceConfigurationConst
virtual XsDeviceConfiguration const &XSNOEXPORT deviceConfigurationConst() const
uint32_t
unsigned int uint32_t
Definition: pstdint.h:485
MtDevice::availableOnboardFilterProfiles
XsFilterProfileArray availableOnboardFilterProfiles() const override
Return the list of filter profiles available on the device.
Definition: mtdevice.cpp:435
XS_SYNC_CLOCK_TICKS_TO_US
#define XS_SYNC_CLOCK_TICKS_TO_US
Definition: xsdef.h:217
xsdef.h
Macros and types for use in the Xsens communication protocol and Xsens Device API classes.
xssensorranges.h
MtDevice::rs485TransmissionDelay
uint16_t rs485TransmissionDelay() const
Return the RS485 acknowledge transmission delay of the device.
Definition: mtdevice.cpp:784
MtDevice::fetchAvailableHardwareScenarios
virtual void fetchAvailableHardwareScenarios()
Fetches available hardware scenarios.
Definition: mtdevice.cpp:480
XS_BID_BROADCAST
#define XS_BID_BROADCAST
The bus broadcast bus identifier (all devices)
Definition: xsbusid.h:76
XsDevice::restoreFactoryDefaults
virtual bool restoreFactoryDefaults()
Restore the device to its factory default settings.
Definition: xsdevice_def.cpp:1953
XGP_Portable
@ XGP_Portable
Definition: xsubloxgnssplatform.h:74
XsDevice::messageLooksSane
virtual XSNOEXPORT bool messageLooksSane(const XsMessage &msg) const
MtDevice::MtDevice
MtDevice(XsDeviceId const &id)
Constructs a standalone MtDevice with device Id id.
Definition: mtdevice.cpp:83
XMID_ReqBaudrate
@ XMID_ReqBaudrate
Definition: xsxbusmessageid.h:112
xsselftestresult.h
XsMtDeviceConfiguration::m_filterMajor
uint8_t m_filterMajor
The filter major version.
Definition: xsdeviceconfiguration.h:111
XDS_Recording
@ XDS_Recording
Definition: xsdevicestate.h:83
MtDevice::writeDeviceSettingsToFile
void writeDeviceSettingsToFile() override
Write the emts of the device to the open logfile.
Definition: mtdevice.cpp:684
XsSelfTestResult
struct XsSelfTestResult XsSelfTestResult
Definition: xsselftestresult.h:228
XsResetMethod
XsResetMethod
Orientation reset type.
Definition: xsresetmethod.h:74
XsSize
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:74
XDS_WaitingForRecordingStart
@ XDS_WaitingForRecordingStart
Definition: xsdevicestate.h:82
XsVersion
A class to store version information.
Definition: xsversion.h:95
mtdevice.h
XsUbloxGnssPlatform
XsUbloxGnssPlatform
Used to select u-blox GNSS chip platform.
Definition: xsubloxgnssplatform.h:72
MtDevice::setNoRotation
bool setNoRotation(uint16_t duration)
Set the no rotation period to duration.
Definition: mtdevice.cpp:691
MtDevice::messageLooksSane
bool messageLooksSane(const XsMessage &msg) const override
Checks for the sanity of a message.
Definition: mtdevice.cpp:112
MtDevice::getBaseFrequency
virtual int getBaseFrequency(XsDataIdentifier dataType=XDI_None) const
Returns the base update rate (Hz) corresponding to the dataType. Returns 0 if no update rate is avail...
Definition: mtdevice.cpp:913
MtDevice::supportedStatusFlags
uint32_t supportedStatusFlags() const override
Returns a bitmask with all the status flags supported by this device.
Definition: mtdevice.cpp:885
MtDevice::m_hardwareFilterProfiles
XsFilterProfileArray m_hardwareFilterProfiles
A vector of hardware filter profiles.
Definition: mtdevice.h:178
XsMessage
Structure for storing a single message.
Definition: xsmessage.h:202
MtDevice::requestData
bool requestData()
Request data from the motion tracker.
Definition: mtdevice.cpp:812
xsoutputconfigurationarray.h
XsDeviceId
Contains an Xsens device ID and provides operations for determining the type of device.
Definition: xsdeviceid.h:192
XMID_SetTransmitDelay
@ XMID_SetTransmitDelay
Definition: xsxbusmessageid.h:487
XDS_Destructing
@ XDS_Destructing
Definition: xsdevicestate.h:85
XsIntArray
struct XsIntArray XsIntArray
Definition: xsintarray.h:80
XDS_Measurement
@ XDS_Measurement
Definition: xsdevicestate.h:81
XMID_ReqAvailableFilterProfiles
@ XMID_ReqAvailableFilterProfiles
Request the available filter profiles.
Definition: xsxbusmessageid.h:243
XsDevice::readDeviceConfiguration
virtual bool readDeviceConfiguration()
XsDevice::resetLogFileReadPosition
virtual bool resetLogFileReadPosition()
Set the read position of the open log file to the start of the file.
Definition: xsdevice_def.cpp:3198
XMID_ReqHeading
@ XMID_ReqHeading
Definition: xsxbusmessageid.h:314
XsDevice::clearDataCache
virtual void clearDataCache()
MtDevice::scheduleOrientationReset
virtual bool scheduleOrientationReset(XsResetMethod method)
Definition: mtdevice.cpp:318
XRM_DefaultHeading
@ XRM_DefaultHeading
Revert to default behaviour for heading, undoes XRM_Heading.
Definition: xsresetmethod.h:82
XDS_Config
@ XDS_Config
Definition: xsdevicestate.h:80
MtDevice::usToSyncTicks
uint32_t usToSyncTicks(uint32_t us) const
Convert microseconds to mt sync ticks.
Definition: mtdevice.cpp:746
JLDEBUGG
#define JLDEBUGG(msg)
Definition: journaller.h:280
XsMtDeviceConfiguration
Device information for MT devices in an XsDeviceConfiguration.
Definition: xsdeviceconfiguration.h:102
XS_LEN_FILTERPROFILELABEL
#define XS_LEN_FILTERPROFILELABEL
Definition: xsdef.h:133
MtDevice::isMotionTracker
bool isMotionTracker() const override
Definition: mtdevice.cpp:169
MtDevice::setGnssReceiverSettings
bool setGnssReceiverSettings(const XsIntArray &gnssReceiverSettings) override
Sets some GNSS receiver settings.
Definition: mtdevice.cpp:266
XsErrorMode
XsErrorMode
Error modes for use in XsDevice::setErrorMode.
Definition: xserrormode.h:74
XMID_ReqLocationId
@ XMID_ReqLocationId
Definition: xsxbusmessageid.h:319
MtDevice::stringOutputType
uint16_t stringOutputType() const override
Returns the string output type.
Definition: mtdevice.cpp:183
XMID_StoreFilterState
@ XMID_StoreFilterState
Definition: xsxbusmessageid.h:340
XMID_ReqGnssReceiverSettings
@ XMID_ReqGnssReceiverSettings
Definition: xsxbusmessageid.h:401
XsDevice::writeMessageToLogFile
virtual void writeMessageToLogFile(const XsMessage &message)
XMID_ReqOutputSkipFactor
@ XMID_ReqOutputSkipFactor
Definition: xsxbusmessageid.h:472
XFPK_Heading
@ XFPK_Heading
Indicates a heading profile.
Definition: xsfilterprofilekind.h:74
XRM_StoreAlignmentMatrix
@ XRM_StoreAlignmentMatrix
Store the current object alignment matrix to persistent memory.
Definition: xsresetmethod.h:76
XsDevice::m_emtsBlob
XsMessage m_emtsBlob
An EMTS blob from device.
Definition: xsdevice_def.h:761
XMID_ReqLatLonAlt
@ XMID_ReqLatLonAlt
Definition: xsxbusmessageid.h:271
XDI_OrientationGroup
@ XDI_OrientationGroup
Group for orientation related outputs.
Definition: xsdataidentifier.h:117
MtDevice::gyroscopeRange
double gyroscopeRange() const
Definition: mtdevice.cpp:676
xsvector3.h
XsDevice::setInitialized
void setInitialized(bool initialized)
Set the initialized state to initialized.
Definition: xsdevice_def.h:611
XMID_ReqHardwareVersion
@ XMID_ReqHardwareVersion
Definition: xsxbusmessageid.h:121
XsDevice::doTransaction
bool doTransaction(const XsMessage &snd) const
XFPK_Base
@ XFPK_Base
Indicates a base profile.
Definition: xsfilterprofilekind.h:72
XsDevice::scheduleOrientationReset
virtual bool scheduleOrientationReset(XsResetMethod method)
MtDevice::initialPositionLLA
XsVector initialPositionLLA() const override
Definition: mtdevice.cpp:724
XBR_Invalid
XBR_Invalid
Not a valid baud rate.
Definition: xsbaudrate.h:126
MtDevice::ubloxGnssPlatform
XsUbloxGnssPlatform ubloxGnssPlatform() const override
Returns the device GNSS platform for u-blox GNSS receivers.
Definition: mtdevice.cpp:226
XS_BID_INVALID
#define XS_BID_INVALID
An invalid bus identifier.
Definition: xsbusid.h:84
XMID_ReqFilterProfile
@ XMID_ReqFilterProfile
Request the current filter profile.
Definition: xsxbusmessageid.h:246
XsString
A 0-terminated managed string of characters.
XsDevice
Definition: xsdevice_def.h:164
MtDevice::calcFrequency
static int calcFrequency(int baseFrequency, uint16_t skipFactor)
Calculates the frequency.
Definition: mtdevice.cpp:854
MtDevice::locationId
int locationId() const
Get the location ID of the device.
Definition: mtdevice.cpp:398
XMID_SetLocationId
@ XMID_SetLocationId
Definition: xsxbusmessageid.h:321
xsens
Definition: threading.cpp:78
XRM_DefaultAlignment
@ XRM_DefaultAlignment
Revert to default behaviour for heading and inclination, undoes any reset.
Definition: xsresetmethod.h:84
MtDevice::reinitialize
bool reinitialize()
Reinitialize the XsDevice.
Definition: mtdevice.cpp:514
MtDevice::outputConfiguration
XsOutputConfigurationArray outputConfiguration() const override
Returns the currently configured output of the device.
Definition: mtdevice.cpp:289
MtDevice::resetLogFileReadPosition
bool resetLogFileReadPosition() override
Set the read position of the open log file to the start of the file.
Definition: mtdevice.cpp:873
XS_LEN_LATLONALT
#define XS_LEN_LATLONALT
Definition: xsdef.h:140
XsDeviceOptionFlag
XsDeviceOptionFlag
Used to enable or disable some device options.
Definition: xsdeviceoptionflag.h:75
MtDevice::setUbloxGnssPlatform
bool setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform) override
Set the device GNSS platform for u-blox GNSS receivers.
Definition: mtdevice.cpp:236
MtDevice::serialBaudRate
XsBaudRate serialBaudRate() const override
The baud rate configured for cabled connection.
Definition: mtdevice.cpp:411
XMID_SetGnssReceiverSettings
@ XMID_SetGnssReceiverSettings
Definition: xsxbusmessageid.h:403
MtDevice::storeFilterState
bool storeFilterState() override
Store orientation filter state in the device.
Definition: mtdevice.cpp:836


xsens_mti_driver
Author(s):
autogenerated on Sun Sep 3 2023 02:43:20