Main MRPT website > C++ reference for MRPT 1.9.9
cmt3.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*! \file cmt3.cpp
11 
12  For information about objects in this file, see the appropriate header:
13  \ref Cmt3.h
14 
15  \section FileCopyright Copyright Notice
16  Copyright (C) Xsens Technologies B.V., 2006. All rights reserved.
17 
18  This source code is intended for use only by Xsens Technologies BV and
19  those that have explicit written permission to use it from
20  Xsens Technologies BV.
21 
22  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
23  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
24  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
25  PARTICULAR PURPOSE.
26 
27  \section FileChangelog Changelog
28  \par 2006-04-28, v0.0.1
29  \li Job Mulder: Created
30  \par 2006-07-21, v0.1.0
31  \li Job Mulder: Updated file for release 0.1.0
32 */
33 
34 #include "cmt3.h"
35 #include <math.h>
36 #include "xsens_janitors.h"
37 
38 #ifdef _LOG_CMT3
39 #define CMT3LOG CMTLOG
40 #define CMT3EXITLOG \
41  JanitorLogFunc<XsensResultValue, uint32_t> _cmtExitLog( \
42  CMTLOG, "L3: " __FUNCTION__ " returns %u\n", m_lastResult);
43 #else
44 #define CMT3LOG(...)
45 #define CMT3EXITLOG
46 #endif
47 
48 #ifdef _LOG_CMT3_DATA
49 #define CMT3LOGDAT CMTLOG
50 #define CMT3EXITLOGDAT \
51  JanitorLogFunc<XsensResultValue, uint32_t> _cmtExitLog( \
52  CMTLOG, "L3: " __FUNCTION__ " returns %u\n", m_lastResult);
53 #else
54 #define CMT3LOGDAT(...)
55 #define CMT3EXITLOGDAT
56 #endif
57 
58 #define CMT3F_DEVINFO_SIZE (sizeof(CmtDeviceId) + sizeof(CmtDataFormat))
59 
60 //////////////////////////////////////////////////////////////////////////////////////////
61 //////////////////////////////////// Support classes
62 ///////////////////////////////////////
63 //////////////////////////////////////////////////////////////////////////////////////////
64 
65 void CmtDeviceConfiguration::readFromMessage(const void* message)
66 {
67  xsens::Message msg((const uint8_t*)message, 0);
68 
69  m_masterDeviceId = msg.getDataLong(0);
70  m_samplingPeriod = msg.getDataShort(4);
71  m_outputSkipFactor = msg.getDataShort(6);
72  m_syncinMode = msg.getDataShort(8);
73  m_syncinSkipFactor = msg.getDataShort(10);
74  m_syncinOffset = msg.getDataLong(12);
75  memcpy(m_date, msg.getDataBuffer(16), 8);
76  memcpy(m_time, msg.getDataBuffer(24), 8);
77  memcpy(m_reservedForHost, msg.getDataBuffer(32), 32);
78  memcpy(m_reservedForClient, msg.getDataBuffer(64), 32);
79  m_numberOfDevices = msg.getDataShort(96);
80 
81  for (uint16_t i = 0; i < m_numberOfDevices; ++i)
82  {
83  m_deviceInfo[i].m_deviceId = msg.getDataLong(98 + i * 20);
84  m_deviceInfo[i].m_dataLength = msg.getDataShort(102 + i * 20);
85  m_deviceInfo[i].m_outputMode = msg.getDataShort(104 + i * 20);
86  m_deviceInfo[i].m_outputSettings = msg.getDataLong(106 + i * 20);
87  memcpy(m_deviceInfo[i].m_reserved, msg.getDataBuffer(110 + i * 20), 8);
88  }
89 }
90 
91 //////////////////////////////////////////////////////////////////////////////////////////
92 // Compute the period and skip factor.
93 void CmtDeviceMode::getPeriodAndSkipFactor(
94  uint16_t& period, uint16_t& skip) const
95 {
96  if (m_sampleFrequency == 0)
97  {
98  period = 0;
99  skip = 0;
100  return;
101  }
102  if (m_sampleFrequency >= 512)
103  {
104  period = 225;
105  skip = 0;
106  return;
107  }
108 
109  int32_t freq, sf = m_sampleFrequency;
110 
111  // first try the simple ones
112  skip = 0;
113  freq = sf;
114  while (freq < 100)
115  {
116  ++skip;
117  freq += sf;
118  }
119 
120  period = (uint16_t)(115200 / freq);
121 }
122 
123 //////////////////////////////////////////////////////////////////////////////////////////
124 // Return the real sample frequency in Hz
125 double CmtDeviceMode::getRealSampleFrequency(void) const
126 {
127  uint16_t skip, period;
128  getPeriodAndSkipFactor(period, skip);
129  return (115200.0 / ((1.0 + (double)skip) * (double)period));
130 }
131 
132 //////////////////////////////////////////////////////////////////////////////////////////
133 // Compute sample frequency from the period and skip factor.
134 void CmtDeviceMode::setPeriodAndSkipFactor(uint16_t period, uint16_t skip)
135 {
136  m_sampleFrequency = (uint16_t)floor(
137  (115200.0 / ((1.0 + (double)skip) * (double)period)) + 0.5);
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////
141 // Compute sample frequency from the period and skip factor.
142 bool CmtDeviceMode::operator==(const CmtDeviceMode& dev) const
143 {
144  return m_outputMode == dev.m_outputMode &&
145  m_outputSettings == dev.m_outputSettings &&
146  m_sampleFrequency == dev.m_sampleFrequency;
147 }
148 
149 //////////////////////////////////////////////////////////////////////////////////////////
150 // Return the real sample frequency in Hz
151 double CmtDeviceMode2::getRealSampleFrequency(void) const
152 {
153  if (m_skip != 0xFFFF)
154  return (115200.0 / ((1.0 + (double)m_skip) * (double)m_period));
155  else
156  return (115200.0 / ((double)m_period));
157 }
158 
159 //////////////////////////////////////////////////////////////////////////////////////////
160 // Return the sample frequency in Hz
161 uint16_t CmtDeviceMode2::getSampleFrequency(void) const
162 {
163  if (m_skip != 0xFFFF)
164  return (uint16_t)floor(
165  (115200.0 / ((1.0 + (double)m_skip) * (double)m_period)) + 0.5);
166  else
167  return (uint16_t)floor((115200.0 / ((double)m_period)) + 0.5);
168 }
169 
170 void CmtDeviceMode2::setSampleFrequency(uint16_t frequency)
171 {
172  if (frequency == 0)
173  {
174  m_period = 0;
175  m_skip = 0;
176  return;
177  }
178  if (frequency >= 512)
179  {
180  m_period = 225;
181  m_skip = 0;
182  return;
183  }
184 
185  int32_t freq, sf = frequency;
186 
187  // first try the simple ones
188  m_skip = 0;
189  freq = sf;
190  while (freq < 100)
191  {
192  ++m_skip;
193  freq += sf;
194  }
195 
196  m_period = (uint16_t)(115200 / freq);
197 }
198 
199 //////////////////////////////////////////////////////////////////////////////////////////
200 // Compute sample frequency from the period and skip factor.
201 bool CmtDeviceMode2::operator==(const CmtDeviceMode2& dev) const
202 {
203  return m_outputMode == dev.m_outputMode &&
204  m_outputSettings == dev.m_outputSettings &&
205  m_period == dev.m_period && m_skip == dev.m_skip;
206 }
207 
208 //////////////////////////////////////////////////////////////////////////////////////////
209 // Common part of a data request function
210 #define DO_DATA_REQUEST_BID(req, bid) \
211  Message snd(req, 0); \
212  Message rcv; \
213  if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST) \
214  return (m_lastResult = XRV_INVALIDID); \
215  if (!m_readFromFile) \
216  { \
217  snd.setBusId(bid); \
218  m_serial.writeMessage(&snd); \
219  if ((m_lastResult = \
220  m_serial.waitForMessage(&rcv, req + 1, 0, true)) != XRV_OK) \
221  return m_lastResult; \
222  if (m_logging) m_logFile.writeMessage(&rcv); \
223  if (rcv.getMessageId() == CMT_MID_ERROR) \
224  { \
225  m_lastHwErrorDeviceId = m_config.m_masterDeviceId; \
226  if (rcv.getDataSize() >= 2) \
227  { \
228  uint8_t biddy = rcv.getDataByte(1); \
229  getDeviceId(biddy, m_lastHwErrorDeviceId); \
230  } \
231  return m_lastResult = m_lastHwError = \
232  (XsensResultValue)rcv.getDataByte(); \
233  } \
234  } \
235  else \
236  while (1) \
237  { \
238  if ((m_lastResult = m_logFile.readMessage(&rcv, req + 1)) != \
239  XRV_OK) \
240  return m_lastResult; \
241  if (rcv.getBusId() == bid || \
242  (rcv.getBusId() == 1 && bid == CMT_BID_MASTER)) \
243  break; \
244  }
245 
246 #define DO_DATA_REQUEST(req) \
247  uint8_t bid = getBusIdInternal(deviceId); \
248  DO_DATA_REQUEST_BID(req, bid);
249 
250 //////////////////////////////////////////////////////////////////////////////////////////
251 // Common part of a serial data set function
252 #define DO_DATA_SET_BID(req, size, type, data, bid) \
253  Message snd(req, size); \
254  Message rcv; \
255  snd.setData##type(data); \
256  if (bid == CMT_BID_BROADCAST) \
257  { \
258  for (uint8_t i = 0; i < m_config.m_numberOfDevices; ++i) \
259  { \
260  snd.setBusId(i + 1); \
261  m_serial.writeMessage(&snd); \
262  m_lastResult = m_serial.waitForMessage(&rcv, req + 1, 0, true); \
263  if (m_lastResult != XRV_OK) return m_lastResult; \
264  if (m_logging) m_logFile.writeMessage(&rcv); \
265  if (rcv.getMessageId() == CMT_MID_ERROR) \
266  { \
267  m_lastHwErrorDeviceId = m_config.m_masterDeviceId; \
268  if (rcv.getDataSize() >= 2) \
269  { \
270  uint8_t biddy = rcv.getDataByte(1); \
271  getDeviceId(biddy, m_lastHwErrorDeviceId); \
272  } \
273  return m_lastResult = m_lastHwError = \
274  (XsensResultValue)rcv.getDataByte(); \
275  } \
276  } \
277  } \
278  else \
279  { \
280  snd.setBusId(bid); \
281  m_serial.writeMessage(&snd); \
282  m_lastResult = m_serial.waitForMessage(&rcv, req + 1, 0, true); \
283  if (m_lastResult != XRV_OK) return m_lastResult; \
284  if (m_logging) m_logFile.writeMessage(&rcv); \
285  if (rcv.getMessageId() == CMT_MID_ERROR) \
286  { \
287  m_lastHwErrorDeviceId = m_config.m_masterDeviceId; \
288  if (rcv.getDataSize() >= 2) \
289  { \
290  uint8_t biddy = rcv.getDataByte(1); \
291  getDeviceId(biddy, m_lastHwErrorDeviceId); \
292  } \
293  return m_lastResult = m_lastHwError = \
294  (XsensResultValue)rcv.getDataByte(); \
295  } \
296  }
297 
298 #define DO_DATA_SET(req, size, type, data) \
299  uint8_t bid = getBusIdInternal(deviceId); \
300  if (bid == CMT_BID_INVALID) return (m_lastResult = XRV_INVALIDID); \
301  DO_DATA_SET_BID(req, size, type, data, bid);
302 
303 #define HANDLE_ERR_RESULT \
304  if (rcv.getMessageId() == CMT_MID_ERROR) \
305  { \
306  m_lastHwErrorDeviceId = m_config.m_masterDeviceId; \
307  if (rcv.getDataSize() >= 2) \
308  { \
309  uint8_t biddy = rcv.getDataByte(1); \
310  getDeviceId(biddy, m_lastHwErrorDeviceId); \
311  } \
312  return m_lastResult = m_lastHwError = \
313  (XsensResultValue)rcv.getDataByte(0); \
314  }
315 
316 //////////////////////////////////////////////////////////////////////////////////////////
317 ///////////////////////////////////////// Cmt3
318 ////////////////////////////////////////////
319 //////////////////////////////////////////////////////////////////////////////////////////
320 
321 namespace xsens
322 {
323 //////////////////////////////////////////////////////////////////////////////////////////
324 // Default constructor, initializes all members to their default values.
325 Cmt3::Cmt3()
326 {
327  m_lastResult = XRV_OK;
328  m_rtcInitialized = false;
329  m_useRtc = true;
330  m_measuring = false;
331  m_timeoutConf = CMT3_DEFAULT_TIMEOUT_CONF;
332  m_timeoutMeas = CMT3_DEFAULT_TIMEOUT_MEAS;
333  m_gotoConfigTries = CMT_GOTO_CONFIG_TRIES;
334  m_readFromFile = false;
335  // m_sampleFrequency = 100.0;
336  m_period = CMT_DEFAULT_PERIOD;
337  m_skip = CMT_DEFAULT_SKIP;
338  memset(m_eMtsData, 0, sizeof(m_eMtsData));
339  memset(&m_config, 0, sizeof(m_config));
340  m_logging = false;
341 
342  clearHwError();
343 }
344 
345 //////////////////////////////////////////////////////////////////////////////////////////
346 // Destructor, de-initializes, frees memory allocated for buffers, etc.
347 Cmt3::~Cmt3()
348 {
349  m_serial.close();
350  m_logFile.close();
351  for (uint32_t i = 0; i < CMT_MAX_DEVICES_PER_PORT; ++i)
352  {
353  CHKFREENUL(m_eMtsData[i]);
354  }
355 }
356 
357 //////////////////////////////////////////////////////////////////////////////////////////
358 // Close the serial communication port.
359 XsensResultValue Cmt3::closePort(bool gotoConfigFirst)
360 {
361  CMT3LOG("L3: closePort\n");
362  CMT3EXITLOG;
363 
364  if (m_measuring && gotoConfigFirst) gotoConfig();
365  CMT3LOG("L3: Closing L2 port\n");
366  m_serial.close();
367  m_measuring = false;
368  if (m_logFile.isOpen())
369  {
370  m_readFromFile = true;
371  m_logging = false;
372  }
373  return m_lastResult = XRV_OK;
374 }
375 
376 //////////////////////////////////////////////////////////////////////////////////////////
377 void Cmt3::fillRtc(Packet* pack)
378 {
379  if (!m_rtcInitialized)
380  {
381  m_rtcStart = pack->m_toa;
382  m_rtcLastSc = pack->getSampleCounter();
383  m_rtcCount = m_rtcLastSc;
384  CmtDeviceMode2 mode;
385  mode.m_period = m_period;
386  mode.m_skip = m_skip;
387  m_rtcMsPerSample = 1000.0 / mode.getRealSampleFrequency();
388  pack->m_rtc = m_rtcStart;
389  m_rtcInitialized = true;
390  }
391  else
392  {
393  CmtMtTimeStamp sc = pack->getSampleCounter();
394  CmtMtTimeStamp scdiff = sc - m_rtcLastSc;
395  m_rtcLastSc = sc;
396  m_rtcCount += scdiff;
397  pack->m_rtc =
398  m_rtcStart +
399  (TimeStamp)floor(m_rtcMsPerSample * (double)m_rtcCount + 0.5);
400  }
401 }
402 
403 #if 0
404 obsolete:
405 //////////////////////////////////////////////////////////////////////////////////////////
406 // Get the state (enabled/disabled) of the AMD algorithm
407 XsensResultValue Cmt3::getAmdState(bool& state, const CmtDeviceId deviceId)
408 {
409  CMT3LOG("L3: getAmdState %08x\n",deviceId);
410  CMT3EXITLOG;
411 
412  DO_DATA_REQUEST(CMT_MID_REQAMD);
413  state = (rcv.getDataShort() != 0);
414  return m_lastResult = XRV_OK;
415 }
416 #endif
417 
418 //////////////////////////////////////////////////////////////////////////////////////////
419 // Get the XM batery level
420 XsensResultValue Cmt3::getBatteryLevel(uint8_t& level)
421 {
422  CMT3LOG("L3: getBatteryLevel\n");
423  CMT3EXITLOG;
424 
425  DO_DATA_REQUEST_BID(CMT_MID_REQBATLEVEL, CMT_BID_MASTER);
426  level = rcv.getDataByte();
427  return m_lastResult = XRV_OK;
428 }
429 
430 //////////////////////////////////////////////////////////////////////////////////////////
431 // Get the baudrate that is currently being used by the port
432 XsensResultValue Cmt3::getBaudrate(uint32_t& baudrate)
433 {
434  CMT3LOG("L3: getBaudrate\n");
435  CMT3EXITLOG;
436 
437  if (!m_serial.isOpen()) return m_lastResult = XRV_NOPORTOPEN;
438  baudrate = m_baudrate;
439  return m_lastResult = XRV_OK;
440 }
441 
442 //////////////////////////////////////////////////////////////////////////////////////////
443 // Get the state of the bluetooth communication, on (true) or off (false)
444 XsensResultValue Cmt3::getBluetoothState(bool& enabled)
445 {
446  CMT3LOG("L3: getBluetoothState\n");
447  CMT3EXITLOG;
448 
449  DO_DATA_REQUEST_BID(CMT_MID_REQBTDISABLE, CMT_BID_MASTER);
450  enabled =
451  (rcv.getDataByte() == 0); // invert the result as it is a disable field
452  return m_lastResult = XRV_OK;
453 }
454 
455 //////////////////////////////////////////////////////////////////////////////////////////
456 // Retrieve the BusId of a device.
457 XsensResultValue Cmt3::getBusId(
458  uint8_t& busId, const CmtDeviceId deviceId) const
459 {
460  CMT3LOG("L3: getBusId %08x\n", deviceId);
461  CMT3EXITLOG;
462 
463  if (deviceId == CMT_DID_MASTER || deviceId == m_config.m_masterDeviceId)
464  {
465  busId = CMT_BID_MASTER;
466  return m_lastResult = XRV_OK;
467  }
468 
469  for (uint16_t i = 0; i <= m_config.m_numberOfDevices; ++i)
470  if (m_config.m_deviceInfo[i].m_deviceId == deviceId)
471  {
472  busId = i + 1;
473  return m_lastResult = XRV_OK;
474  }
475  return m_lastResult = XRV_NOTFOUND;
476 }
477 
478 //////////////////////////////////////////////////////////////////////////////////////////
479 uint8_t Cmt3::getBusIdInternal(const CmtDeviceId devId) const
480 {
481  if (devId == CMT_DID_MASTER) return CMT_BID_MASTER;
482 
483  if (devId == CMT_DID_BROADCAST) return CMT_BID_BROADCAST;
484 
485  if (m_config.m_masterDeviceId == devId) return CMT_BID_MASTER;
486 
487  for (uint16_t i = 0; i <= m_config.m_numberOfDevices; ++i)
488  {
489  if (m_config.m_deviceInfo[i].m_deviceId == devId)
490  return (uint8_t)(i + 1);
491  }
492  return CMT_BID_INVALID;
493 }
494 
495 //////////////////////////////////////////////////////////////////////////////////////////
496 // Retrieve the XM bus power state.
497 XsensResultValue Cmt3::getBusPowerState(bool& enabled)
498 {
499  CMT3LOG("L3: getBusPowerState\n");
500  CMT3EXITLOG;
501 
502  DO_DATA_REQUEST_BID(CMT_MID_BUSPWR, CMT_BID_MASTER);
503  enabled =
504  (rcv.getDataShort() !=
505  0); // invert the result as it is a disable field
506  return m_lastResult = XRV_OK;
507 }
508 
509 //////////////////////////////////////////////////////////////////////////////////////////
510 Cmt2f* Cmt3::getCmt2f(void) { return &m_logFile; }
511 //////////////////////////////////////////////////////////////////////////////////////////
512 Cmt2s* Cmt3::getCmt2s(void) { return &m_serial; }
513 //////////////////////////////////////////////////////////////////////////////////////////
514 // Retrieve the complete device configuration of a device.
515 XsensResultValue Cmt3::getConfiguration(CmtDeviceConfiguration& configuration)
516 {
517  CMT3LOG("L3: getConfiguration\n");
518  CMT3EXITLOG;
519 
520  if (!(m_serial.isOpen() || m_logFile.isOpen()))
521  return m_lastResult = XRV_INVALIDOPERATION;
522 
523  memcpy(&configuration, &m_config, sizeof(CmtDeviceConfiguration));
524 
525  if (m_logging)
526  {
527  // fake the message receipt
528  Message msg(
529  CMT_MID_CONFIGURATION,
530  98 + CMT_CONF_BLOCKLEN * m_config.m_numberOfDevices);
531  msg.setBusId(CMT_BID_MASTER);
532 
533  msg.setDataLong(m_config.m_masterDeviceId, 0);
534  msg.setDataShort(m_config.m_samplingPeriod, 4);
535  msg.setDataShort(m_config.m_outputSkipFactor, 6);
536  msg.setDataShort(m_config.m_syncinMode, 8);
537  msg.setDataShort(m_config.m_syncinSkipFactor, 10);
538  msg.setDataLong(m_config.m_syncinOffset, 12);
539  memcpy(msg.getDataBuffer(16), m_config.m_date, 8);
540  memcpy(msg.getDataBuffer(24), m_config.m_time, 8);
541  memcpy(msg.getDataBuffer(32), m_config.m_reservedForHost, 32);
542  memcpy(msg.getDataBuffer(64), m_config.m_reservedForClient, 32);
543  msg.setDataShort(m_config.m_numberOfDevices, 96);
544 
545  for (uint16_t i = 0; i < m_config.m_numberOfDevices; ++i)
546  {
547  msg.setDataLong(m_config.m_deviceInfo[i].m_deviceId, 98 + i * 20);
548  msg.setDataShort(
549  m_config.m_deviceInfo[i].m_dataLength, 102 + i * 20);
550  msg.setDataShort(
551  m_config.m_deviceInfo[i].m_outputMode, 104 + i * 20);
552  msg.setDataLong(
553  m_config.m_deviceInfo[i].m_outputSettings, 106 + i * 20);
554  memcpy(
555  msg.getDataBuffer(110 + i * 20),
556  m_config.m_deviceInfo[i].m_reserved, 8);
557  }
558 
559  msg.recomputeChecksum();
560 
561  writeMessageToLogFile(msg);
562  }
563  return m_lastResult = XRV_OK;
564 }
565 
566 //////////////////////////////////////////////////////////////////////////////////////////
567 // Retrieve the number of bytes that are in a data message as sent by the
568 // device.
569 XsensResultValue Cmt3::getDataLength(
570  uint32_t& length, const CmtDeviceId deviceId)
571 {
572  CMT3LOG("L3: getDataLength %08x\n", deviceId);
573  CMT3EXITLOG;
574 
575  DO_DATA_REQUEST(CMT_MID_REQDATALENGTH);
576  length = rcv.getDataShort(); // the sensor returns this value as a int16_t
577  return m_lastResult = XRV_OK;
578 }
579 
580 //////////////////////////////////////////////////////////////////////////////////////////
581 // Return the number of connected devices. Returns 0 if not connected.
582 uint32_t Cmt3::getDeviceCount(void) const
583 {
584  CMT3LOG("L3: getDeviceCount\n");
585  if (m_serial.isOpen() || m_logFile.isOpen())
586  {
587  if (isXm())
588  return m_config.m_numberOfDevices + 1;
589  else
590  return m_config.m_numberOfDevices;
591  }
592  return 0;
593 }
594 
595 //////////////////////////////////////////////////////////////////////////////////////////
596 // Retrieve the CmtDeviceId of the device at the given BusId
597 XsensResultValue Cmt3::getDeviceId(
598  const uint8_t busId, CmtDeviceId& deviceId) const
599 {
600  CMT3LOG("L3: getDeviceId %u\n", (uint32_t)busId);
601  CMT3EXITLOG;
602 
603  if (busId == CMT_BID_MASTER || busId == 0)
604  deviceId = m_config.m_masterDeviceId;
605  else
606  {
607  if (busId > m_config.m_numberOfDevices)
608  return m_lastResult = XRV_INVALIDID;
609  deviceId = m_config.m_deviceInfo[busId - 1].m_deviceId;
610  }
611  return m_lastResult = XRV_OK;
612 }
613 
614 //////////////////////////////////////////////////////////////////////////////////////////
615 // Retrieve the complete device output mode of a device.
616 XsensResultValue Cmt3::getDeviceMode(
617  CmtDeviceMode& mode, const CmtDeviceId deviceId)
618 {
619  CMT3LOG("L3: getDeviceMode %08x\n", deviceId);
620  CMT3EXITLOG;
621 
622  CmtDeviceMode2 mode2;
623  XsensResultValue rv;
624  rv = getDeviceMode2(mode2, deviceId);
625  if (rv == XRV_OK)
626  {
627  mode.m_outputMode = mode2.m_outputMode;
628  mode.m_outputSettings = mode2.m_outputSettings;
629  mode.m_sampleFrequency = mode2.getSampleFrequency();
630  }
631  return rv;
632 }
633 
634 //////////////////////////////////////////////////////////////////////////////////////////
635 // Retrieve the complete device output mode of a device.
636 XsensResultValue Cmt3::getDeviceMode2(
637  CmtDeviceMode2& mode, const CmtDeviceId deviceId)
638 {
639  CMT3LOG("L3: getDeviceMode2 %08x\n", deviceId);
640  CMT3EXITLOG;
641 
642  uint8_t bid = getBusIdInternal(deviceId);
643  if (bid == CMT_BID_INVALID) return m_lastResult = XRV_INVALIDID;
644  if (bid == 0) return m_lastResult = XRV_INVALIDID;
645  if (bid == CMT_BID_MASTER) bid = 1;
646 
647  mode.m_period = m_period;
648  mode.m_skip = m_skip;
649  // mode.m_sampleFrequency = (uint16_t) floor(m_sampleFrequency+0.5);
650  mode.m_outputMode = m_config.m_deviceInfo[bid - 1].m_outputMode;
651  mode.m_outputSettings = m_config.m_deviceInfo[bid - 1].m_outputSettings;
652 
653  return m_lastResult = XRV_OK;
654 }
655 
656 //////////////////////////////////////////////////////////////////////////////////////////
657 // Retrieve the eMts data of the specified sensor(s)
658 XsensResultValue Cmt3::getEMtsData(void* buffer, const CmtDeviceId deviceId)
659 {
660  CMT3LOG("L3: getEMtsData %p %08x\n", buffer, deviceId);
661  CMT3EXITLOG;
662 
663  uint8_t bid = getBusIdInternal(deviceId);
664  if (bid == CMT_BID_INVALID) return m_lastResult = XRV_INVALIDID;
665  if (!m_readFromFile && !m_serial.isOpen())
666  return m_lastResult = XRV_NOPORTOPEN;
667  if (m_readFromFile && !m_logFile.isOpen())
668  return m_lastResult = XRV_NOFILEOPEN;
669  if (buffer == nullptr) return m_lastResult = XRV_NULLPTR;
670 
671  if (bid == CMT_BID_BROADCAST)
672  {
673  memset(buffer, 0, m_config.m_numberOfDevices * CMT_EMTS_SIZE);
674  uint8_t* buf = (uint8_t*)buffer;
675  // loop over devices and request emts data for all of them
676  for (uint32_t dev = 0; dev < m_config.m_numberOfDevices; ++dev)
677  if (getEMtsData(
678  buf + dev * CMT_EMTS_SIZE,
679  m_config.m_deviceInfo[dev].m_deviceId) != XRV_OK)
680  return m_lastResult;
681  // CMT3LOG("L3: getEMtsData (%08x) returns
682  // %d\n",deviceId,(int32_t)m_lastResult);
683  return m_lastResult;
684  }
685 
686  uint8_t dataIndex;
687 
688  if (isXm())
689  {
690  if (bid == CMT_BID_MASTER)
691  {
692  // CMT3LOG("L3: getEMtsData (%08x) returns
693  // XRV_INVALIDID\n",deviceId);
694  return m_lastResult = XRV_INVALIDID;
695  }
696  dataIndex = bid - 1;
697  }
698  else
699  dataIndex = 0; // when we have a single MT, we should use bank 0
700 
701  if (m_eMtsData[dataIndex] == nullptr)
702  {
703  m_eMtsData[dataIndex] = malloc(CMT_EMTS_SIZE);
704 
705 // Xbus requires small data packets
706 #if 0
707  if (isXm())
708  {
709  Message msg(CMT_MID_REQEMTS,3);
710  Message rcv;
711  uint8_t* tme = (uint8_t*) m_eMtsData[dataIndex];
712  memset(tme,0,CMT_EMTS_SIZE); // clear eMTS data
713  msg.setBusId(bid);
714  msg.setDataByte(0,0); // read bank 0
715  uint16_t emtPos = 0;
716  for (uint8_t page=0;page < 4;++page) // read all 4 pages, one at a time
717  {
718  msg.setDataByte(page,1);
719  for (uint8_t part=0;part < 4;++part, ++emtPos)
720  {
721  if (!m_readFromFile)
722  {
723  msg.setDataByte(part,2);
724  m_serial.writeMessage(&msg);
725  m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_EMTSDATA,0,false);
726  }
727  else
728  m_lastResult = m_logFile.readMessage(&rcv,CMT_MID_EMTSDATA);
729 
730  if (m_lastResult != XRV_OK)
731  {
732  FREENUL(m_eMtsData[dataIndex]);
733  //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
734  return m_lastResult;
735  }
736  if (!m_readFromFile && m_logging)
737  m_logFile.writeMessage(&rcv);
738  if (rcv.getMessageId() == CMT_MID_ERROR)
739  {
740  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
741  if (rcv.getDataSize() >= 2)
742  {
743  uint8_t biddy = rcv.getDataByte(1);
744  getDeviceId(biddy,m_lastHwErrorDeviceId);
745  }
746  m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte(0);
747  FREENUL(m_eMtsData[dataIndex]);
748  //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
749  return m_lastResult;
750  }
751 
752  memcpy(tme+(emtPos*66),rcv.getDataBuffer(),66);
753  }
754  }
755  } // if (isXm())
756  else // single MT, ask in one packet
757  {
758 #endif
759  Message msg(CMT_MID_REQEMTS, 2);
760  Message rcv;
761  memset(m_eMtsData[dataIndex], 0, CMT_EMTS_SIZE); // clear eMTS data
762  msg.setBusId(bid);
763  msg.setDataByte(0, 0); // read bank 0
764  msg.setDataByte(255, 1); // read all pages
765 
766  if (!m_readFromFile)
767  {
768  m_serial.writeMessage(&msg);
769  m_lastResult =
770  m_serial.waitForMessage(&rcv, CMT_MID_EMTSDATA, 0, false);
771  }
772  else
773  m_lastResult = m_logFile.readMessage(&rcv, CMT_MID_EMTSDATA);
774  if (m_lastResult != XRV_OK)
775  {
776  FREENUL(m_eMtsData[dataIndex]);
777  // CMT3LOG("L3: getEMtsData (%08x) returns
778  // %d\n",deviceId,(int32_t)m_lastResult);
779  return m_lastResult;
780  }
781  if (!m_readFromFile && m_logging) m_logFile.writeMessage(&rcv);
782  if (rcv.getMessageId() == CMT_MID_ERROR)
783  {
784  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
785  if (rcv.getDataSize() >= 2)
786  {
787  uint8_t biddy = rcv.getDataByte(1);
788  getDeviceId(biddy, m_lastHwErrorDeviceId);
789  }
790  m_lastResult = m_lastHwError = (XsensResultValue)rcv.getDataByte(0);
791  FREENUL(m_eMtsData[dataIndex]);
792  // CMT3LOG("L3: getEMtsData (%08x) returns
793  // %d\n",deviceId,(int32_t)m_lastResult);
794  return m_lastResult;
795  }
796 
797  memcpy(m_eMtsData[dataIndex], rcv.getDataBuffer(), CMT_EMTS_SIZE);
798 #if 0
799  }
800 #endif
801  }
802  else if (m_logging)
803  {
804  // fake the message receipt
805  Message msg(CMT_MID_EMTSDATA, CMT_EMTS_SIZE);
806  msg.setBusId(bid);
807  msg.setDataBuffer(
808  (const uint8_t*)m_eMtsData[dataIndex], 0, CMT_EMTS_SIZE);
809  writeMessageToLogFile(msg);
810  }
811 
812  memcpy(buffer, m_eMtsData[dataIndex], CMT_EMTS_SIZE);
813  // CMT3LOG("L3: getEMtsData (%08x) returns XRV_OK\n",deviceId);
814  return m_lastResult = XRV_OK;
815 }
816 
817 //////////////////////////////////////////////////////////////////////////////////////////
818 // Retrieve the error mode
819 XsensResultValue Cmt3::getErrorMode(uint16_t& mode, const CmtDeviceId deviceId)
820 {
821  CMT3LOG("L3: getErrorMode %08x\n", deviceId);
822  CMT3EXITLOG;
823 
824  if (isXm())
825  {
826  DO_DATA_REQUEST_BID(CMT_MID_REQXMERRORMODE, CMT_BID_MASTER);
827  mode = rcv.getDataShort();
828  }
829  else
830  {
831  DO_DATA_REQUEST(CMT_MID_REQERRORMODE);
832  mode = rcv.getDataShort();
833  }
834  return m_lastResult = XRV_OK;
835 }
836 
837 #if 0
838 obsolete:
839 //////////////////////////////////////////////////////////////////////////////////////////
840 // Retrieve the filter settings of a device.
841 XsensResultValue Cmt3::getFilterSettings(CmtFilterSettings& settings, const CmtDeviceId deviceId)
842 {
843  Message snd;
844  Message rcv;
845  uint8_t bid = getBusIdInternal(deviceId);
846  if (bid == CMT_BID_INVALID)
847  return (m_lastResult = XRV_INVALIDID);
848 
849  bool xm = isXm();
850 
851  if (xm && deviceId == m_config.m_masterDeviceId)
852  return m_lastResult = XRV_INVALIDOPERATION;
853 
854  if (!xm)
855  bid = CMT_BID_MASTER;
856 
857  // first setting: gain
858  snd.setBusId(bid);
859  snd.setDataByte(0);
860  snd.setMessageId(CMT_MID_REQFILTERSETTINGS);
861  m_serial.writeMessage(&snd);
862  m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQFILTERSETTINGSACK,0,true);
863  if (m_lastResult != XRV_OK)
864  return m_lastResult;
865  if (m_logging)
866  m_logFile.writeMessage(&rcv);
867  HANDLE_ERR_RESULT;
868  settings.m_gain = rcv.getDataFloat(1);
869 
870  // second setting: weighting
871  snd.setBusId(bid);
872  snd.setDataByte(1);
873  snd.setMessageId(CMT_MID_REQFILTERSETTINGS);
874  m_serial.writeMessage(&snd);
875  m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQFILTERSETTINGSACK,0,true);
876  if (m_lastResult != XRV_OK)
877  return m_lastResult;
878  if (m_logging)
879  m_logFile.writeMessage(&rcv);
880  HANDLE_ERR_RESULT;
881  settings.m_weighting = rcv.getDataFloat(1);
882 
883  return m_lastResult = XRV_OK;
884 }
885 #endif
886 
887 //////////////////////////////////////////////////////////////////////////////////////////
888 // Retrieve the firmware revision of a device.
889 XsensResultValue Cmt3::getFirmwareRevision(
890  CmtVersion& revision, const CmtDeviceId deviceId)
891 {
892  CMT3LOG("L3: getFirmwareRevision %08x\n", deviceId);
893  CMT3EXITLOG;
894 
895  DO_DATA_REQUEST(CMT_MID_REQFWREV);
896 
897  revision.m_major = rcv.getDataByte(0);
898  revision.m_minor = rcv.getDataByte(1);
899  revision.m_revision = rcv.getDataByte(2);
900 
901  return m_lastResult = XRV_OK;
902 }
903 
904 //////////////////////////////////////////////////////////////////////////////////////////
905 // Retrieve the heading offset of a device. The range is -pi to +pi.
906 XsensResultValue Cmt3::getHeading(double& heading, const CmtDeviceId deviceId)
907 {
908  CMT3LOG("L3: getHeading %08x\n", deviceId);
909  CMT3EXITLOG;
910 
911  DO_DATA_REQUEST(CMT_MID_REQHEADING);
912  heading = rcv.getDataFloat();
913  return m_lastResult = XRV_OK;
914 }
915 
916 //////////////////////////////////////////////////////////////////////////////////////////
917 // Retrieve the location ID of a device. The buffer should be at least 20 bytes.
918 XsensResultValue Cmt3::getLocationId(
919  uint16_t& locationId, const CmtDeviceId deviceId)
920 {
921  CMT3LOG("L3: getLocationId %08x\n", deviceId);
922  CMT3EXITLOG;
923 
924  DO_DATA_REQUEST(CMT_MID_REQLOCATIONID);
925  locationId = rcv.getDataShort();
926  return m_lastResult = XRV_OK;
927 }
928 
929 //////////////////////////////////////////////////////////////////////////////////////////
930 // Retrieve the current log file read position
931 XsensResultValue Cmt3::getLogFileReadPosition(CmtFilePos& pos)
932 {
933  CMT3LOG("L3: getLogFileReadPosition\n");
934  CMT3EXITLOG;
935 
936  if (m_logFile.isOpen())
937  {
938  pos = m_logFile.getReadPosition();
939  return m_lastResult = XRV_OK;
940  }
941  pos = 0;
942  return m_lastResult = XRV_NOFILEOPEN;
943 }
944 
945 //////////////////////////////////////////////////////////////////////////////////////////
946 // Retrieve the size of the log file
947 XsensResultValue Cmt3::getLogFileSize(CmtFilePos& size)
948 {
949  CMT3LOG("L3: getLogFileSize\n");
950  CMT3EXITLOG;
951 
952  if (m_logFile.isOpen())
953  {
954  size = m_logFile.getFileSize();
955  return m_lastResult = XRV_OK;
956  }
957  size = 0;
958  return m_lastResult = XRV_NOFILEOPEN;
959 }
960 
961 //////////////////////////////////////////////////////////////////////////////////////////
962 // Retrieve the name of the open log file or an empty string if no logfile is
963 // open
964 XsensResultValue Cmt3::getLogFileName(char* filename)
965 {
966  CMT3LOG("L3: getLogFileName (char)\n");
967  CMT3EXITLOG;
968 
969  if (m_logFile.isOpen()) return m_lastResult = m_logFile.getName(filename);
970  filename[0] = '\0';
971  return m_lastResult = XRV_NOFILEOPEN;
972 }
973 
974 //////////////////////////////////////////////////////////////////////////////////////////
975 // Retrieve the name of the open log file or an empty string if no logfile is
976 // open
977 XsensResultValue Cmt3::getLogFileName(wchar_t* filename)
978 {
979  CMT3LOG("L3: getLogFileName (wchar_t)\n");
980  CMT3EXITLOG;
981 
982  if (m_logFile.isOpen()) return m_lastResult = m_logFile.getName(filename);
983  filename[0] = L'\0';
984  return m_lastResult = XRV_NOFILEOPEN;
985 }
986 
987 //////////////////////////////////////////////////////////////////////////////////////////
988 // Return the magnetic declination. The range is -pi to +pi.
989 XsensResultValue Cmt3::getMagneticDeclination(
990  double& declination, const CmtDeviceId deviceId)
991 {
992  CMT3LOG("L3: getMagneticDeclination %08x\n", deviceId);
993  CMT3EXITLOG;
994 
995  DO_DATA_REQUEST(CMT_MID_REQMAGNETICDECLINATION);
996  declination = rcv.getDataFloat();
997  return m_lastResult = XRV_OK;
998 }
999 
1000 //////////////////////////////////////////////////////////////////////////////////////////
1001 // Return the device Id of the first device (master)
1002 CmtDeviceId Cmt3::getMasterId(void)
1003 {
1004  CMT3LOG("L3: getMasterId\n");
1005 
1006  if (m_serial.isOpen() || m_logFile.isOpen())
1007  return m_config.m_masterDeviceId;
1008  return 0;
1009 }
1010 
1011 //////////////////////////////////////////////////////////////////////////////////////////
1012 // Return the nr of connected MTs (excludes XMs)
1013 uint16_t Cmt3::getMtCount(void) const
1014 {
1015  CMT3LOGDAT("L3: getMtCount\n");
1016 
1017  if (m_serial.isOpen() || m_logFile.isOpen())
1018  return m_config.m_numberOfDevices;
1019  return 0;
1020 }
1021 
1022 //////////////////////////////////////////////////////////////////////////////////////////
1023 // Retrieve the CmtDeviceId of the MT device with the given index
1024 XsensResultValue Cmt3::getMtDeviceId(
1025  const uint8_t index, CmtDeviceId& deviceId) const
1026 {
1027  CMT3LOG("L3: getMtDeviceId %u\n", (uint32_t)index);
1028  CMT3EXITLOG;
1029 
1030  if (index >= m_config.m_numberOfDevices)
1031  return m_lastResult = XRV_INVALIDPARAM;
1032  deviceId = m_config.m_deviceInfo[index].m_deviceId;
1033  return m_lastResult = XRV_OK;
1034 }
1035 
1036 //////////////////////////////////////////////////////////////////////////////////////////
1037 // Retrieve the port that the object is connected to.
1038 XsensResultValue Cmt3::getPortNr(uint8_t& port) const
1039 {
1040  CMT3LOG("L3: getPortNr\n");
1041  CMT3EXITLOG;
1042 
1043  return m_lastResult = m_serial.getPortNr(port);
1044 }
1045 
1046 //////////////////////////////////////////////////////////////////////////////////////////
1047 // Retrieve the product code of a device. The buffer should be at least 21
1048 // bytes.
1049 XsensResultValue Cmt3::getProductCode(
1050  char* productCode, const CmtDeviceId deviceId)
1051 {
1052  CMT3LOG("L3: getProductCode %p %08x\n", productCode, deviceId);
1053  CMT3EXITLOG;
1054 
1055  DO_DATA_REQUEST(CMT_MID_REQPRODUCTCODE);
1056 
1057  uint16_t len = rcv.getDataSize();
1058  memcpy(productCode, rcv.getDataBuffer(), len);
1059  productCode[len] = '\0';
1060 
1061  return m_lastResult = XRV_OK;
1062 }
1063 
1064 //////////////////////////////////////////////////////////////////////////////////////////
1065 // Retrieve the sample frequency of the devices on the bus.
1066 uint16_t Cmt3::getSampleFrequency(void)
1067 {
1068  CMT3LOG("L3: getSampleFrequency\n");
1069  CmtDeviceMode2 mode;
1070  mode.m_period = m_period;
1071  mode.m_skip = m_skip;
1072 
1073  return mode.getSampleFrequency();
1074 }
1075 
1076 //////////////////////////////////////////////////////////////////////////////////////////
1077 // Get the baudrate that is reported for the serial connection
1078 XsensResultValue Cmt3::getSerialBaudrate(uint32_t& baudrate)
1079 {
1080  CMT3LOG("L3: getSerialBaudrate\n");
1081  CMT3EXITLOG;
1082 
1083  DO_DATA_REQUEST_BID(CMT_MID_REQBAUDRATE, CMT_BID_MASTER);
1084  switch (rcv.getDataByte())
1085  {
1086  case CMT_BAUDCODE_9K6:
1087  baudrate = CMT_BAUD_RATE_9600;
1088  break;
1089  // case CMT_BAUDCODE_14K4:
1090  // baudrate = CMT_BAUD_RATE_14K4;
1091  // break;
1092  case CMT_BAUDCODE_19K2:
1093  baudrate = CMT_BAUD_RATE_19K2;
1094  break;
1095  // case CMT_BAUDCODE_28K8:
1096  // baudrate = CMT_BAUD_RATE_28K8;
1097  // break;
1098  case CMT_BAUDCODE_38K4:
1099  baudrate = CMT_BAUD_RATE_38K4;
1100  break;
1101  case CMT_BAUDCODE_57K6:
1102  baudrate = CMT_BAUD_RATE_57K6;
1103  break;
1104  // case CMT_BAUDCODE_76K8:
1105  // baudrate = CMT_BAUD_RATE_76K8;
1106  // break;
1107  case CMT_BAUDCODE_115K2:
1108  baudrate = CMT_BAUD_RATE_115K2;
1109  break;
1110  case CMT_BAUDCODE_230K4:
1111  baudrate = CMT_BAUD_RATE_230K4;
1112  break;
1113  case CMT_BAUDCODE_460K8:
1114  baudrate = CMT_BAUD_RATE_460K8;
1115  break;
1116  case CMT_BAUDCODE_921K6:
1117  baudrate = CMT_BAUD_RATE_921K6;
1118  break;
1119  default:
1120  return m_lastResult = XRV_BAUDRATEINVALID;
1121  }
1122  return m_lastResult = XRV_OK;
1123 }
1124 
1125 //////////////////////////////////////////////////////////////////////////////////////////
1126 // Retrieve the inbound synchronization settings of a device.
1127 XsensResultValue Cmt3::getSyncInSettings(CmtSyncInSettings& settings)
1128 {
1129  CMT3LOG("L3: getSyncInSettings\n");
1130  CMT3EXITLOG;
1131 
1132  Message snd(CMT_MID_REQSYNCINSETTINGS, 1);
1133  Message rcv;
1134 
1135  snd.setBusId(CMT_BID_MASTER);
1136 
1137  snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
1138  m_serial.writeMessage(&snd);
1139  m_lastResult =
1140  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
1141  if (m_lastResult != XRV_OK) return m_lastResult;
1142  if (m_logging) m_logFile.writeMessage(&rcv);
1143  HANDLE_ERR_RESULT;
1144  settings.m_mode = rcv.getDataShort(1);
1145 
1146  snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
1147  m_serial.writeMessage(&snd);
1148  m_lastResult =
1149  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
1150  if (m_lastResult != XRV_OK) return m_lastResult;
1151  if (m_logging) m_logFile.writeMessage(&rcv);
1152  HANDLE_ERR_RESULT;
1153  settings.m_skipFactor = rcv.getDataShort(1);
1154 
1155  snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
1156  m_serial.writeMessage(&snd);
1157  m_lastResult =
1158  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
1159  if (m_lastResult != XRV_OK) return m_lastResult;
1160  if (m_logging) m_logFile.writeMessage(&rcv);
1161  HANDLE_ERR_RESULT;
1162  settings.m_offset = rcv.getDataLong(1);
1163 
1164  // convert the offset to ns
1165  settings.m_offset = (uint32_t)(
1166  (((double)settings.m_offset) * CMT_SYNC_CLOCK_TICKS_TO_NS) + 0.5);
1167 
1168  return m_lastResult = XRV_OK;
1169 }
1170 
1171 //////////////////////////////////////////////////////////////////////////////////////////
1172 // Retrieve the inbound synchronization mode of a device.
1173 XsensResultValue Cmt3::getSyncInMode(uint16_t& mode)
1174 {
1175  CMT3LOG("L3: getSyncInMode\n");
1176  CMT3EXITLOG;
1177 
1178  Message snd(CMT_MID_REQSYNCINSETTINGS, 1);
1179  Message rcv;
1180 
1181  snd.setBusId(CMT_BID_MASTER);
1182 
1183  snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
1184  m_serial.writeMessage(&snd);
1185  m_lastResult =
1186  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
1187  if (m_lastResult != XRV_OK) return m_lastResult;
1188  if (m_logging) m_logFile.writeMessage(&rcv);
1189  HANDLE_ERR_RESULT;
1190  mode = rcv.getDataShort(1);
1191 
1192  return m_lastResult = XRV_OK;
1193 }
1194 
1195 //////////////////////////////////////////////////////////////////////////////////////////
1196 // Retrieve the inbound synchronization skip factor of a device.
1197 XsensResultValue Cmt3::getSyncInSkipFactor(uint16_t& skipFactor)
1198 {
1199  CMT3LOG("L3: getSyncInSkipFactor\n");
1200  CMT3EXITLOG;
1201 
1202  Message snd(CMT_MID_REQSYNCINSETTINGS, 1);
1203  Message rcv;
1204 
1205  snd.setBusId(CMT_BID_MASTER);
1206 
1207  snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
1208  m_serial.writeMessage(&snd);
1209  m_lastResult =
1210  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
1211  if (m_lastResult != XRV_OK) return m_lastResult;
1212  if (m_logging) m_logFile.writeMessage(&rcv);
1213  HANDLE_ERR_RESULT;
1214  skipFactor = rcv.getDataShort(1);
1215 
1216  return m_lastResult = XRV_OK;
1217 }
1218 
1219 //////////////////////////////////////////////////////////////////////////////////////////
1220 // Retrieve the inbound synchronization offset of a device.
1221 XsensResultValue Cmt3::getSyncInOffset(uint32_t& offset)
1222 {
1223  CMT3LOG("L3: getSyncInOffset\n");
1224  CMT3EXITLOG;
1225 
1226  Message snd(CMT_MID_REQSYNCINSETTINGS, 1);
1227  Message rcv;
1228 
1229  snd.setBusId(CMT_BID_MASTER);
1230 
1231  snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
1232  m_serial.writeMessage(&snd);
1233  m_lastResult =
1234  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
1235  if (m_lastResult != XRV_OK) return m_lastResult;
1236  if (m_logging) m_logFile.writeMessage(&rcv);
1237  HANDLE_ERR_RESULT;
1238  offset = rcv.getDataLong(1);
1239 
1240  // convert the offset to ns
1241  offset = (uint32_t)((((double)offset) * CMT_SYNC_CLOCK_TICKS_TO_NS) + 0.5);
1242 
1243  return m_lastResult = XRV_OK;
1244 }
1245 
1246 //////////////////////////////////////////////////////////////////////////////////////////
1247 // Get the synchronization mode of the XM
1248 XsensResultValue Cmt3::getSyncMode(uint8_t& mode)
1249 {
1250  CMT3LOG("L3: getSyncMode\n");
1251  CMT3EXITLOG;
1252 
1253  DO_DATA_REQUEST_BID(CMT_MID_REQSYNCMODE, CMT_BID_MASTER);
1254  mode = rcv.getDataByte();
1255  return m_lastResult = XRV_OK;
1256 }
1257 
1258 //////////////////////////////////////////////////////////////////////////////////////////
1259 // Retrieve the outbound synchronization settings of a device.
1260 XsensResultValue Cmt3::getSyncOutSettings(CmtSyncOutSettings& settings)
1261 {
1262  CMT3LOG("L3: getSyncOutSettings\n");
1263  CMT3EXITLOG;
1264 
1265  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 1);
1266  Message rcv;
1267 
1268  snd.setBusId(CMT_BID_MASTER);
1269 
1270  snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
1271  m_serial.writeMessage(&snd);
1272  m_lastResult =
1273  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1274  if (m_lastResult != XRV_OK) return m_lastResult;
1275  if (m_logging) m_logFile.writeMessage(&rcv);
1276  HANDLE_ERR_RESULT;
1277  settings.m_mode = rcv.getDataShort(1);
1278 
1279  snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
1280  m_serial.writeMessage(&snd);
1281  m_lastResult =
1282  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1283  if (m_lastResult != XRV_OK) return m_lastResult;
1284  if (m_logging) m_logFile.writeMessage(&rcv);
1285  HANDLE_ERR_RESULT;
1286  settings.m_skipFactor = rcv.getDataShort(1);
1287 
1288  snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
1289  m_serial.writeMessage(&snd);
1290  m_lastResult =
1291  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1292  if (m_lastResult != XRV_OK) return m_lastResult;
1293  if (m_logging) m_logFile.writeMessage(&rcv);
1294  HANDLE_ERR_RESULT;
1295  settings.m_offset = rcv.getDataLong(1);
1296 
1297  snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
1298  m_serial.writeMessage(&snd);
1299  m_lastResult =
1300  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1301  if (m_lastResult != XRV_OK) return m_lastResult;
1302  if (m_logging) m_logFile.writeMessage(&rcv);
1303  HANDLE_ERR_RESULT;
1304  settings.m_pulseWidth = rcv.getDataLong(1);
1305 
1306  // convert the offset and pulse width to ns
1307  settings.m_offset = (uint32_t)(
1308  (((double)settings.m_offset) * CMT_SYNC_CLOCK_TICKS_TO_NS) + 0.5);
1309  settings.m_pulseWidth = (uint32_t)(
1310  (((double)settings.m_pulseWidth) * CMT_SYNC_CLOCK_TICKS_TO_NS) + 0.5);
1311 
1312  return m_lastResult = XRV_OK;
1313 }
1314 
1315 //////////////////////////////////////////////////////////////////////////////////////////
1316 // Retrieve the outbound synchronization mode of a device.
1317 XsensResultValue Cmt3::getSyncOutMode(uint16_t& mode)
1318 {
1319  CMT3LOG("L3: getSyncOutMode\n");
1320  CMT3EXITLOG;
1321 
1322  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 1);
1323  Message rcv;
1324 
1325  snd.setBusId(CMT_BID_MASTER);
1326 
1327  snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
1328  m_serial.writeMessage(&snd);
1329  m_lastResult =
1330  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1331  if (m_lastResult != XRV_OK) return m_lastResult;
1332  if (m_logging) m_logFile.writeMessage(&rcv);
1333  HANDLE_ERR_RESULT;
1334  mode = rcv.getDataShort(1);
1335 
1336  return m_lastResult = XRV_OK;
1337 }
1338 
1339 //////////////////////////////////////////////////////////////////////////////////////////
1340 // Retrieve the outbound synchronization pulse width of a device.
1341 XsensResultValue Cmt3::getSyncOutPulseWidth(uint32_t& pulseWidth)
1342 {
1343  CMT3LOG("L3: getSyncOutPulseWidth\n");
1344  CMT3EXITLOG;
1345 
1346  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 1);
1347  Message rcv;
1348 
1349  snd.setBusId(CMT_BID_MASTER);
1350 
1351  snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
1352  m_serial.writeMessage(&snd);
1353  m_lastResult =
1354  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1355  if (m_lastResult != XRV_OK) return m_lastResult;
1356  if (m_logging) m_logFile.writeMessage(&rcv);
1357  HANDLE_ERR_RESULT;
1358  pulseWidth = rcv.getDataLong(1);
1359 
1360  // convert the pulse width to ns
1361  pulseWidth =
1362  (uint32_t)((((double)pulseWidth) * CMT_SYNC_CLOCK_TICKS_TO_NS) + 0.5);
1363 
1364  return m_lastResult = XRV_OK;
1365 }
1366 
1367 //////////////////////////////////////////////////////////////////////////////////////////
1368 // Retrieve the outbound synchronization skip factor of a device.
1369 XsensResultValue Cmt3::getSyncOutSkipFactor(uint16_t& skipFactor)
1370 {
1371  CMT3LOG("L3: getSyncOutSkipFactor\n");
1372  CMT3EXITLOG;
1373 
1374  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 1);
1375  Message rcv;
1376 
1377  snd.setBusId(CMT_BID_MASTER);
1378 
1379  snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
1380  m_serial.writeMessage(&snd);
1381  m_lastResult =
1382  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1383  if (m_lastResult != XRV_OK) return m_lastResult;
1384  if (m_logging) m_logFile.writeMessage(&rcv);
1385  HANDLE_ERR_RESULT;
1386  skipFactor = rcv.getDataShort(1);
1387 
1388  return m_lastResult = XRV_OK;
1389 }
1390 
1391 //////////////////////////////////////////////////////////////////////////////////////////
1392 // Retrieve the outbound synchronization offset of a device.
1393 XsensResultValue Cmt3::getSyncOutOffset(uint32_t& offset)
1394 {
1395  CMT3LOG("L3: getSyncOutOffset\n");
1396  CMT3EXITLOG;
1397 
1398  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 1);
1399  Message rcv;
1400 
1401  snd.setBusId(CMT_BID_MASTER);
1402 
1403  snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
1404  m_serial.writeMessage(&snd);
1405  m_lastResult =
1406  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
1407  if (m_lastResult != XRV_OK) return m_lastResult;
1408  if (m_logging) m_logFile.writeMessage(&rcv);
1409  HANDLE_ERR_RESULT;
1410  offset = rcv.getDataLong(1);
1411 
1412  // convert the offset to ns
1413  offset = (uint32_t)((((double)offset) * CMT_SYNC_CLOCK_TICKS_TO_NS) + 0.5);
1414 
1415  return m_lastResult = XRV_OK;
1416 }
1417 
1418 //////////////////////////////////////////////////////////////////////////////////////////
1419 uint32_t Cmt3::getTimeoutConfig(void) const { return m_timeoutConf; }
1420 //////////////////////////////////////////////////////////////////////////////////////////
1421 uint32_t Cmt3::getTimeoutMeasurement(void) const { return m_timeoutMeas; }
1422 //////////////////////////////////////////////////////////////////////////////////////////
1423 // Retrieve the UTC time of the last received sample
1424 XsensResultValue Cmt3::getUtcTime(CmtUtcTime& utc, const CmtDeviceId deviceId)
1425 {
1426  CMT3LOG("L3: getUtcTime %08x\n", deviceId);
1427  CMT3EXITLOG;
1428 
1429  DO_DATA_REQUEST(CMT_MID_REQUTCTIME);
1430 
1431  utc.m_nano = rcv.getDataLong(0);
1432  utc.m_year = rcv.getDataShort(4);
1433  utc.m_month = rcv.getDataByte(6);
1434  utc.m_day = rcv.getDataByte(7);
1435  utc.m_hour = rcv.getDataByte(8);
1436  utc.m_minute = rcv.getDataByte(9);
1437  utc.m_second = rcv.getDataByte(10);
1438  utc.m_valid = rcv.getDataByte(11);
1439 
1440  // if (utc.m_valid == 0)
1441  // return m_lastResult = XRV_INVALID_TIME;
1442 
1443  return m_lastResult = XRV_OK;
1444 }
1445 
1446 //////////////////////////////////////////////////////////////////////////////////////////
1447 // Get the dual-mode output settings of the XM
1448 XsensResultValue Cmt3::getXmOutputMode(uint8_t& mode)
1449 {
1450  CMT3LOG("L3: getXmOutputMode\n");
1451  CMT3EXITLOG;
1452 
1453  DO_DATA_REQUEST_BID(CMT_MID_REQOPMODE, CMT_BID_MASTER);
1454  mode = rcv.getDataByte();
1455  return m_lastResult = XRV_OK;
1456 }
1457 
1458 //////////////////////////////////////////////////////////////////////////////////////////
1459 // Place all connected sensors into Configuration Mode.
1460 XsensResultValue Cmt3::gotoConfig(void)
1461 {
1462  CMT3LOG(
1463  "L3: gotoConfig port %u\n", (uint32_t)m_serial.getCmt1s()->getPortNr());
1464  CMT3EXITLOG;
1465 
1466  Message snd(CMT_MID_GOTOCONFIG);
1467  Message rcv;
1468  int32_t tries = 0;
1469 
1470  srand((unsigned int)timeStampNow());
1471 
1472  m_serial.setTimeout(CMT3_CONFIG_TIMEOUT);
1473  snd.setBusId(CMT_BID_MASTER);
1474  while (tries++ < m_gotoConfigTries)
1475  {
1476  m_serial.getCmt1s()->flushData(); // special case for goto config. we
1477  // want the buffer to be empty
1478  CMT3LOG("L3: Attempt to goto config %d\n", tries);
1479  m_serial.writeMessage(&snd);
1480  m_lastResult =
1481  m_serial.waitForMessage(&rcv, CMT_MID_GOTOCONFIGACK, 0, false);
1482  if (m_lastResult == XRV_TIMEOUTNODATA) break;
1483  if (m_lastResult == XRV_OK)
1484  {
1485  if (m_logging) m_logFile.writeMessage(&rcv);
1486  if (rcv.getMessageId() == CMT_MID_ERROR)
1487  {
1488  m_lastHwErrorDeviceId = CMT_DID_MASTER;
1489  if (rcv.getDataSize() >= 2)
1490  {
1491  uint8_t biddy = rcv.getDataByte(1);
1492  getDeviceId(biddy, m_lastHwErrorDeviceId);
1493  }
1494  m_lastResult = m_lastHwError =
1495  (XsensResultValue)rcv.getDataByte(0);
1496  CMT3LOG(
1497  "L3: Goto config failed, error received %d: %s\n",
1498  (int32_t)m_lastResult, xsensResultText(m_lastResult));
1499  m_serial.setTimeout(m_timeoutConf);
1500  return m_lastResult;
1501  }
1502  CMT3LOG("L3: Goto config succeeded\n");
1503  m_measuring = false;
1504  m_serial.setTimeout(m_timeoutConf);
1505  return m_lastResult = XRV_OK;
1506  }
1507  m_lastResult = m_serial.getLastResult();
1508  msleep(((long)rand() * 10) / RAND_MAX);
1509  }
1510  m_serial.setTimeout(m_timeoutConf);
1511  m_measuring = (m_lastResult != XRV_OK);
1512  CMT3LOG(
1513  "L3: Goto config returns %d: %s\n", (int32_t)m_lastResult,
1514  xsensResultText(m_lastResult));
1515  return m_lastResult;
1516 }
1517 
1518 //////////////////////////////////////////////////////////////////////////////////////////
1519 // Place all connected sensors into Measurement Mode.
1520 XsensResultValue Cmt3::gotoMeasurement(void)
1521 {
1522  CMT3LOG(
1523  "L3: gotoMeasurement port %u\n",
1524  (uint32_t)m_serial.getCmt1s()->getPortNr());
1525  CMT3EXITLOG;
1526 
1527  Message snd(CMT_MID_GOTOMEASUREMENT);
1528  Message rcv;
1529 
1530  snd.setBusId(CMT_BID_MASTER);
1531 
1532  m_serial.writeMessage(&snd);
1533  m_lastResult =
1534  m_serial.waitForMessage(&rcv, CMT_MID_GOTOMEASUREMENTACK, 0, false);
1535  if (m_lastResult != XRV_OK) return m_lastResult;
1536  if (m_logging) m_logFile.writeMessage(&rcv);
1537  HANDLE_ERR_RESULT;
1538 
1539  m_rtcInitialized = false;
1540  m_measuring = true;
1541  m_serial.setTimeout(m_timeoutMeas);
1542  return (m_lastResult = XRV_OK);
1543 }
1544 
1545 XsensResultValue Cmt3::initBus(void)
1546 {
1547  CMT3LOG(
1548  "L3: initBus port %u\n", (uint32_t)m_serial.getCmt1s()->getPortNr());
1549  CMT3EXITLOG;
1550 
1551  DO_DATA_REQUEST_BID(CMT_MID_INITBUS, CMT_BID_MASTER);
1552  return m_lastResult =
1553  XRV_OK; // m_serial.waitForMessage(&rcv,CMT_MID_INITBUSRESULTS);
1554 }
1555 
1556 //////////////////////////////////////////////////////////////////////////////////////////
1557 // Return whether the main device is an Xbus Master or not.
1558 bool Cmt3::isXm(void) const
1559 {
1560  return (
1561  (m_config.m_masterDeviceId & CMT_DID_TYPEH_MASK) == CMT_DID_TYPEH_XM);
1562 }
1563 
1564 //////////////////////////////////////////////////////////////////////////////////////////
1565 // Open a communication channel to the given serial port name.
1566 XsensResultValue Cmt3::openPort(const char* portName, const uint32_t baudRate)
1567 {
1568  CMT3LOG(
1569  "L3: openPort Opening port %s @baud %d, timeoutC=%u, timeoutM=%u\n",
1570  portName, baudRate, m_timeoutConf, m_timeoutMeas);
1571  CMT3EXITLOG;
1572 
1573  if (m_logFile.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
1574 
1575  m_serial.setTimeout(m_timeoutConf);
1576  if ((m_lastResult = m_serial.open(portName, baudRate)) != XRV_OK)
1577  return m_lastResult;
1578 
1579  CMT3LOG("L3: openPort: Low level port opened, gotoConfig\n");
1580 
1581  m_baudrate = baudRate;
1582  m_rtcInitialized = false;
1583  m_measuring = true; // required for faster operation of refreshCache
1584  m_logging = false;
1585 
1586  // place the device in config mode
1587  if (gotoConfig() != XRV_OK)
1588  {
1589  CMT3LOG(
1590  "L3: openPort: gotoConfig failed: [%d]%s\n", m_lastResult,
1591  xsensResultText(m_lastResult));
1592  m_serial.close();
1593  return XRV_CONFIGCHECKFAIL;
1594  }
1595 
1596  CMT3LOG("L3: openPort: gotoConfig succeeded, requesting initBus\n");
1597  Message snd, rcv;
1598 
1599  if (initBus() != XRV_OK)
1600  {
1601  CMT3LOG(
1602  "L3: openPort: initBus failed: [%d]%s\n", m_lastResult,
1603  xsensResultText(m_lastResult));
1604  m_serial.close();
1605  return XRV_CONFIGCHECKFAIL;
1606  }
1607 
1608  CMT3LOG("L3: openPort: initBus succeeded, cleaning up the cache\n");
1609  if (refreshCache() != XRV_OK)
1610  {
1611  m_serial.close();
1612  CMT3LOG(
1613  "L3: openPort: refreshCache failed: [%d]%s\n", m_lastResult,
1614  xsensResultText(m_lastResult));
1615  return XRV_CONFIGCHECKFAIL;
1616  }
1617 
1618  CMT3LOG("L3: openPort: returning OK\n");
1619  return m_lastResult = XRV_OK;
1620 }
1621 
1622 #ifdef _WIN32
1623 //////////////////////////////////////////////////////////////////////////////////////////
1624 // Open a communication channel to the given COM port number.
1625 XsensResultValue Cmt3::openPort(
1626  const uint32_t portNumber, const uint32_t baudRate)
1627 {
1628  // open the port
1629  CMT3LOG(
1630  "L3: openPort Opening port %d @baud %d, timeoutC=%u, timeoutM=%u\n",
1631  (int32_t)portNumber, baudRate, m_timeoutConf, m_timeoutMeas);
1632  CMT3EXITLOG;
1633 
1634  if (m_logFile.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
1635 
1636  m_serial.setTimeout(m_timeoutConf); // then update L2 (and L1)
1637  if ((m_lastResult = m_serial.open(portNumber, baudRate)) != XRV_OK)
1638  return m_lastResult;
1639 
1640  CMT3LOG("L3: openPort: Low level port opened, gotoConfig\n");
1641 
1642  m_baudrate = baudRate;
1643  m_rtcInitialized = false;
1644  m_measuring = true; // required for faster operation of refreshCache
1645  m_logging = false;
1646 
1647  // place the device in config mode
1648  if (gotoConfig() != XRV_OK)
1649  {
1650  CMT3LOG(
1651  "L3: openPort: gotoConfig failed: [%d]%s\n", m_lastResult,
1652  xsensResultText(m_lastResult));
1653  m_serial.close();
1654  return XRV_CONFIGCHECKFAIL;
1655  }
1656 
1657  CMT3LOG("L3: openPort: gotoConfig succeeded, requesting initBus\n");
1658  Message snd, rcv;
1659 
1660  if (initBus() != XRV_OK)
1661  {
1662  CMT3LOG(
1663  "L3: openPort: initBus failed: [%d]%s\n", m_lastResult,
1664  xsensResultText(m_lastResult));
1665  m_serial.close();
1666  return XRV_CONFIGCHECKFAIL;
1667  }
1668 
1669  CMT3LOG("L3: openPort: initBus succeeded, cleaning up the cache\n");
1670  if (refreshCache() != XRV_OK)
1671  {
1672  m_serial.close();
1673  CMT3LOG(
1674  "L3: openPort: refreshCache failed: [%d]%s\n", m_lastResult,
1675  xsensResultText(m_lastResult));
1676  return XRV_CONFIGCHECKFAIL;
1677  }
1678 
1679  CMT3LOG("L3: openPort: returning OK\n");
1680  return m_lastResult = XRV_OK;
1681 }
1682 #endif
1683 
1684 //////////////////////////////////////////////////////////////////////////////////////////
1685 // Get the MessageId of the next logged message
1686 XsensResultValue Cmt3::peekLogMessageId(uint8_t& messageId)
1687 {
1688  CMT3LOG("L3: peekLogMessageId started\n");
1689  CMT3EXITLOG;
1690 
1691  if (!m_readFromFile) return m_lastResult = XRV_INVALIDOPERATION;
1692 
1693  Message msg;
1694 
1695  CmtFilePos pos = m_logFile.getReadPosition();
1696  m_lastResult = m_logFile.readMessage(&msg);
1697  m_logFile.setReadPosition(pos);
1698 
1699  if (m_lastResult != XRV_OK)
1700  {
1701  CMT3LOG("L3: peekLogMessageId, no messages to be read\n");
1702  return m_lastResult;
1703  }
1704  messageId = msg.getMessageId();
1705  CMT3LOG(
1706  "L3: peekLogMessageId found msg with ID %02x\n", (int32_t)messageId);
1707  return m_lastResult = XRV_OK;
1708 }
1709 
1710 //////////////////////////////////////////////////////////////////////////////////////////
1711 // Retrieve a data message.
1712 XsensResultValue Cmt3::readDataPacket(Packet* pack, bool acceptOther)
1713 {
1714  CMT3LOGDAT("L3: readDataPacket %p %u\n", pack, acceptOther ? 1 : 0);
1715  CMT3EXITLOGDAT;
1716 
1717  if (!m_readFromFile)
1718  {
1719  while (1)
1720  {
1721  m_lastResult = m_serial.readMessage(&pack->m_msg);
1722  if (m_lastResult != XRV_OK)
1723  {
1724  CMT3LOGDAT("L3: readDataPacket, no data messages to be read\n");
1725  return m_lastResult;
1726  }
1727  if (m_logging) m_logFile.writeMessage(&pack->m_msg);
1728  if (pack->m_msg.getMessageId() == CMT_MID_MTDATA)
1729  {
1730  pack->setXbus(
1731  m_config.m_masterDeviceId !=
1732  m_config.m_deviceInfo[0].m_deviceId,
1733  false);
1734  pack->m_itemCount = m_config.m_numberOfDevices;
1735  for (uint16_t i = 0; i < m_config.m_numberOfDevices; ++i)
1736  pack->setDataFormat(
1737  m_config.m_deviceInfo[i].m_outputMode,
1738  m_config.m_deviceInfo[i].m_outputSettings, i);
1739  pack->m_toa = timeStampNow();
1740  if (m_useRtc) fillRtc(pack);
1741  CMT3LOGDAT("L3: readDataPacket, data message read\n");
1742  return m_lastResult = XRV_OK;
1743  }
1744  else if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
1745  {
1746  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
1747  if (pack->m_msg.getDataSize() >= 2)
1748  {
1749  uint8_t biddy = pack->m_msg.getDataByte(1);
1750  getDeviceId(biddy, m_lastHwErrorDeviceId);
1751  }
1752  return m_lastResult = m_lastHwError =
1753  (XsensResultValue)pack->m_msg.getDataByte(0);
1754  }
1755  CMT3LOGDAT(
1756  "L3: readDataPacket, non-data message read: %2x\n",
1757  (int32_t)pack->m_msg.getMessageId());
1758  if (acceptOther)
1759  {
1760  CMT3LOGDAT("L3: accepting other message\n");
1761  return m_lastResult = XRV_OTHER;
1762  }
1763  }
1764  }
1765  else
1766  {
1767  while (1)
1768  {
1769  m_lastResult = m_logFile.readMessage(&pack->m_msg);
1770  if (m_lastResult != XRV_OK)
1771  {
1772  CMT3LOGDAT("L3: readDataPacket, no data messages to be read\n");
1773  return m_lastResult;
1774  }
1775  if (pack->m_msg.getMessageId() == CMT_MID_MTDATA)
1776  {
1777  pack->setXbus(
1778  m_config.m_masterDeviceId !=
1779  m_config.m_deviceInfo[0].m_deviceId,
1780  false);
1781  pack->m_itemCount = m_config.m_numberOfDevices;
1782  for (uint16_t i = 0; i < m_config.m_numberOfDevices; ++i)
1783  pack->setDataFormat(
1784  m_config.m_deviceInfo[i].m_outputMode,
1785  m_config.m_deviceInfo[i].m_outputSettings, i);
1786  pack->m_toa = timeStampNow();
1787  if (m_useRtc) fillRtc(pack);
1788  CMT3LOGDAT("L3: readDataPacket, data message read\n");
1789  return m_lastResult = XRV_OK;
1790  }
1791  else if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
1792  {
1793  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
1794  if (pack->m_msg.getDataSize() >= 2)
1795  {
1796  uint8_t biddy = pack->m_msg.getDataByte(1);
1797  getDeviceId(biddy, m_lastHwErrorDeviceId);
1798  }
1799  return m_lastResult = m_lastHwError =
1800  (XsensResultValue)pack->m_msg.getDataByte(0);
1801  }
1802  CMT3LOGDAT(
1803  "L3: readDataPacket, non-data message read: %2x\n",
1804  (int32_t)pack->m_msg.getMessageId());
1805  if (acceptOther)
1806  {
1807  CMT3LOGDAT("L3: accepting other message\n");
1808  return m_lastResult = XRV_OTHER;
1809  }
1810  }
1811  }
1812 }
1813 
1814 //////////////////////////////////////////////////////////////////////////////////////////
1815 // Request a data message and wait for it to arrive.
1816 XsensResultValue Cmt3::requestData(Packet* pack)
1817 {
1818  CMT3LOGDAT("L3: requestData %p\n", pack);
1819  CMT3EXITLOGDAT;
1820 
1821  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
1822  Message snd(CMT_MID_REQDATA);
1823  m_serial.writeMessage(&snd);
1824  return readDataPacket(pack);
1825 }
1826 
1827 //////////////////////////////////////////////////////////////////////////////////////////
1828 // Reset all connected sensors.
1829 XsensResultValue Cmt3::reset(void)
1830 {
1831  CMT3LOG("L3: reset\n");
1832  CMT3EXITLOG;
1833 
1834  Message snd(CMT_MID_RESET, 0);
1835  Message rcv;
1836  snd.setBusId(CMT_BID_MASTER);
1837  m_serial.writeMessage(&snd);
1838  m_lastResult = m_serial.waitForMessage(&rcv, CMT_MID_RESETACK, 0, false);
1839  if (m_lastResult != XRV_OK) return m_lastResult;
1840  if (m_logging) m_logFile.writeMessage(&rcv);
1841  m_measuring = true;
1842  refreshCache();
1843  return m_lastResult = XRV_OK;
1844 }
1845 
1846 //////////////////////////////////////////////////////////////////////////////////////////
1847 //! Perform an orientation reset on a device.
1848 XsensResultValue Cmt3::resetOrientation(
1849  const CmtResetMethod method, const CmtDeviceId deviceId)
1850 {
1851  CMT3LOG("L3: resetOrientation %u %08x\n", (uint32_t)method, deviceId);
1852  CMT3EXITLOG;
1853 
1854  DO_DATA_SET(
1855  CMT_MID_RESETORIENTATION, CMT_LEN_RESETORIENTATION, Short,
1856  (uint16_t)method);
1857  return m_lastResult = XRV_OK;
1858 }
1859 
1860 //////////////////////////////////////////////////////////////////////////////////////////
1861 //! Restore the factory defaults of a device.
1862 XsensResultValue Cmt3::restoreFactoryDefaults(const CmtDeviceId deviceId)
1863 {
1864  CMT3LOG("L3: restoreFactoryDefaults %08x\n", deviceId);
1865  CMT3EXITLOG;
1866 
1867  DO_DATA_REQUEST(CMT_MID_RESTOREFACTORYDEF);
1868  return m_lastResult = XRV_OK;
1869 }
1870 
1871 #if 0
1872 obsolete:
1873 //////////////////////////////////////////////////////////////////////////////////////////
1874 // Set the state (enabled/disabled) of the AMD algorithm
1875 XsensResultValue Cmt3::setAmdState (const bool state, const CmtDeviceId deviceId)
1876 {
1877  CMT3LOG("L3: setAmdState %u %08x\n",state?1:0,deviceId);
1878  CMT3EXITLOG;
1879 
1880  uint16_t dat = ((state)?1:0);
1881  DO_DATA_SET(CMT_MID_REQAMD,CMT_LEN_AMD,Short,dat);
1882  return m_lastResult = XRV_OK;
1883 }
1884 #endif
1885 
1886 //////////////////////////////////////////////////////////////////////////////////////////
1887 // Set the baudrate and reconnect at the new baudrate if successful.
1888 XsensResultValue Cmt3::setBaudrate(const uint32_t baudrate, bool reconnect)
1889 {
1890  CMT3LOG("L3: setBaudrate %u %u\n", baudrate, reconnect ? 1 : 0);
1891  CMT3EXITLOG;
1892 
1893  uint8_t tmp;
1894  switch (baudrate)
1895  {
1896  case CMT_BAUD_RATE_9600:
1897  tmp = CMT_BAUDCODE_9K6;
1898  break;
1899  // case CMT_BAUD_RATE_14K4:
1900  // tmp = CMT_BAUDCODE_14K4;
1901  // break;
1902  case CMT_BAUD_RATE_19K2:
1903  tmp = CMT_BAUDCODE_19K2;
1904  break;
1905  // case CMT_BAUD_RATE_28K8:
1906  // tmp = CMT_BAUDCODE_28K8;
1907  // break;
1908  case CMT_BAUD_RATE_38K4:
1909  tmp = CMT_BAUDCODE_38K4;
1910  break;
1911  case CMT_BAUD_RATE_57K6:
1912  tmp = CMT_BAUDCODE_57K6;
1913  break;
1914  // case CMT_BAUD_RATE_76K8:
1915  // tmp = CMT_BAUDCODE_76K8;
1916  // break;
1917  case CMT_BAUD_RATE_115K2:
1918  tmp = CMT_BAUDCODE_115K2;
1919  break;
1920  case CMT_BAUD_RATE_230K4:
1921  tmp = CMT_BAUDCODE_230K4;
1922  break;
1923  case CMT_BAUD_RATE_460K8:
1924  tmp = CMT_BAUDCODE_460K8;
1925  break;
1926  case CMT_BAUD_RATE_921K6:
1927  tmp = CMT_BAUDCODE_921K6;
1928  break;
1929  default:
1930  return m_lastResult = XRV_BAUDRATEINVALID;
1931  }
1932 
1933  if (baudrate != m_baudrate)
1934  {
1935  DO_DATA_SET_BID(
1936  CMT_MID_REQBAUDRATE, CMT_LEN_BAUDRATE, Byte, tmp, CMT_BID_MASTER);
1937 
1938  if (reconnect)
1939  {
1940  CMT3LOG("L3: sending reset for reconnect\n");
1941  // Reset devices on this port and reopen port @ new baudrate
1942  Message sndReset(CMT_MID_RESET, 0);
1943  Message rcvReset;
1944  sndReset.setBusId(CMT_BID_MASTER);
1945  m_serial.writeMessage(&sndReset);
1946  m_lastResult =
1947  m_serial.waitForMessage(&rcvReset, CMT_MID_RESETACK, 0, false);
1948  if (m_lastResult != XRV_OK) return m_lastResult;
1949  if (m_logging) m_logFile.writeMessage(&rcvReset);
1950  bool wasMeasuring = m_measuring;
1951  m_measuring = true;
1952 
1953  CMT3LOG("L3: reopening port at new baud rate\n");
1954 #ifdef _WIN32
1955  int32_t port;
1956  m_serial.getPortNr(port);
1957  closePort(false);
1958  m_lastResult = openPort(port, baudrate);
1959 #else
1960  char portname[32];
1961  m_serial.getPortName(portname);
1962  closePort(false);
1963  m_lastResult = openPort(portname, baudrate);
1964 #endif
1965  if (m_lastResult != XRV_OK) return m_lastResult;
1966  if (wasMeasuring) gotoMeasurement();
1967  return m_lastResult;
1968  }
1969  }
1970 
1971  return m_lastResult = XRV_OK;
1972 }
1973 
1974 //////////////////////////////////////////////////////////////////////////////////////////
1975 // Set the state of the bluetooth communication to on (true) or off (false)
1976 XsensResultValue Cmt3::setBluetoothState(const bool enabled)
1977 {
1978  CMT3LOG("L3: setBluetoothState %u\n", enabled ? 1 : 0);
1979  CMT3EXITLOG;
1980 
1981  uint8_t dat = (enabled ? 0 : 1);
1982  DO_DATA_SET_BID(
1983  CMT_MID_REQBTDISABLE, CMT_LEN_BTDISABLE, Byte, dat, CMT_BID_MASTER);
1984  return m_lastResult = XRV_OK;
1985 }
1986 
1987 //////////////////////////////////////////////////////////////////////////////////////////
1988 // Switch the XM bus power on or off.
1989 XsensResultValue Cmt3::setBusPowerState(const bool enabled)
1990 {
1991  CMT3LOG("L3: setBusPowerState %u\n", enabled ? 1 : 0);
1992  CMT3EXITLOG;
1993 
1994  uint16_t tmp = enabled ? 1 : 0;
1995  DO_DATA_SET_BID(CMT_MID_BUSPWR, CMT_LEN_BUSPWR, Short, tmp, CMT_BID_MASTER);
1996  return m_lastResult = XRV_OK;
1997 }
1998 
1999 //////////////////////////////////////////////////////////////////////////////////////////
2000 // Set the complete device output mode of a device.
2001 XsensResultValue Cmt3::setDeviceMode(
2002  const CmtDeviceMode& mode, bool force, const CmtDeviceId deviceId)
2003 {
2004  CMT3LOG(
2005  "L3: setDeviceMode %04x %08x %u %u %08x\n", (uint32_t)mode.m_outputMode,
2006  (uint32_t)mode.m_outputSettings, (uint32_t)mode.m_sampleFrequency,
2007  force ? 1 : 0, deviceId);
2008  CMT3EXITLOG;
2009 
2010  CmtDeviceMode2 mode2;
2011  mode2.m_outputMode = mode.m_outputMode;
2012  mode2.m_outputSettings = mode.m_outputSettings;
2013  mode2.setSampleFrequency(mode.m_sampleFrequency);
2014 
2015  return setDeviceMode2(mode2, force, deviceId);
2016 }
2017 
2018 //////////////////////////////////////////////////////////////////////////////////////////
2019 // Set the complete device output mode of a device.
2020 XsensResultValue Cmt3::setDeviceMode2(
2021  const CmtDeviceMode2& mode, bool force, const CmtDeviceId deviceId)
2022 {
2023  CMT3LOG(
2024  "L3: setDeviceMode2 %04x %08x %u %u %u %08x\n",
2025  (uint32_t)mode.m_outputMode, (uint32_t)mode.m_outputSettings,
2026  (uint32_t)mode.m_period, (uint32_t)mode.m_skip, force ? 1 : 0,
2027  deviceId);
2028  CMT3EXITLOG;
2029 
2030  Message snd;
2031  Message rcv;
2032  uint8_t bid = getBusIdInternal(deviceId);
2033  if (bid == CMT_BID_INVALID) return (m_lastResult = XRV_INVALIDID);
2034 
2035  // uint16_t period, skip;
2036  // mode.getPeriodAndSkipFactor(period, skip);
2037  uint16_t xperiod;
2038  // bool changed = false;
2039 
2040  bool xm = isXm();
2041  if (bid == CMT_BID_BROADCAST)
2042  {
2043  if (xm)
2044  {
2045  // set the device modes of all connected devices first
2046  for (uint16_t i = 0; i < m_config.m_numberOfDevices; ++i)
2047  if ((m_lastResult = setDeviceMode2(
2048  mode, force, m_config.m_deviceInfo[i].m_deviceId)) !=
2049  XRV_OK)
2050  return m_lastResult;
2051  }
2052  bid = CMT_BID_MASTER;
2053  }
2054 
2055  // set sample frequency if this is an Xbus Master or the primary device
2056  if (deviceId == CMT_DID_BROADCAST || deviceId == m_config.m_masterDeviceId)
2057  {
2058  m_period = mode.m_period;
2059  m_skip = mode.m_skip;
2060  // m_sampleFrequency = (double) mode.getSampleFrequency();
2061  snd.setBusId(CMT_BID_MASTER);
2062  if (xm)
2063  xperiod = mode.m_period * (mode.m_skip + 1);
2064  else
2065  xperiod = mode.m_period;
2066 
2067  if (force || m_config.m_samplingPeriod != xperiod)
2068  {
2069  CMT3LOG(
2070  "L3: setDeviceMode setting device %08x period to %u\n",
2071  deviceId, (uint32_t)xperiod);
2072  // changed = true;
2073  snd.setDataShort(xperiod);
2074  snd.setMessageId(CMT_MID_REQPERIOD);
2075  m_serial.writeMessage(&snd);
2076  m_lastResult =
2077  m_serial.waitForMessage(&rcv, CMT_MID_REQPERIODACK, 0, true);
2078  if (m_lastResult != XRV_OK) return m_lastResult;
2079  if (m_logging) m_logFile.writeMessage(&rcv);
2080  HANDLE_ERR_RESULT;
2081  m_config.m_samplingPeriod = xperiod; // update device info
2082  }
2083 
2084  if (!xm && (force || m_config.m_outputSkipFactor != mode.m_skip))
2085  {
2086  CMT3LOG(
2087  "L3: setDeviceMode setting MT %08x skip factor to %u\n",
2088  deviceId, (uint32_t)mode.m_skip);
2089  // changed = true;
2090  snd.setDataShort(mode.m_skip);
2091  snd.setMessageId(CMT_MID_REQOUTPUTSKIPFACTOR);
2092  m_serial.writeMessage(&snd);
2093  m_lastResult = m_serial.waitForMessage(
2094  &rcv, CMT_MID_REQOUTPUTSKIPFACTORACK, 0, true);
2095  if (m_lastResult != XRV_OK) return m_lastResult;
2096  if (m_logging) m_logFile.writeMessage(&rcv);
2097  HANDLE_ERR_RESULT;
2098  m_config.m_outputSkipFactor = mode.m_skip; // update device info
2099  }
2100  }
2101 
2102  // set the output mode and settings if this device is not an Xbus Master
2103  if ((deviceId & CMT_DID_TYPEH_MASK) != CMT_DID_TYPEH_XM)
2104  {
2105  if (bid == CMT_BID_BROADCAST || bid == CMT_BID_MASTER)
2106  {
2107  snd.setBusId(CMT_BID_MASTER);
2108  bid = 1;
2109  }
2110  else
2111  snd.setBusId(bid);
2112 
2113  if (force ||
2114  m_config.m_deviceInfo[bid - 1].m_outputMode !=
2115  (uint16_t)mode.m_outputMode)
2116  {
2117  CMT3LOG(
2118  "L3: setDeviceMode setting MT %08x output mode to %04X\n",
2119  deviceId, (uint32_t)mode.m_outputMode);
2120  // changed = true;
2121  snd.resizeData(2);
2122  snd.setMessageId(CMT_MID_REQOUTPUTMODE);
2123  snd.setDataShort((uint16_t)mode.m_outputMode);
2124  m_serial.writeMessage(&snd);
2125  m_lastResult = m_serial.waitForMessage(
2126  &rcv, CMT_MID_REQOUTPUTMODEACK, 0, true);
2127  if (m_lastResult != XRV_OK) return m_lastResult;
2128  if (m_logging) m_logFile.writeMessage(&rcv);
2129  HANDLE_ERR_RESULT;
2130  m_config.m_deviceInfo[bid - 1].m_outputMode =
2131  (uint16_t)mode.m_outputMode; // update device info
2132  }
2133 
2134  uint32_t settings = (uint32_t)mode.m_outputSettings;
2135  if (xm) settings &= ~(uint32_t)CMT_OUTPUTSETTINGS_TIMESTAMP_MASK;
2136 
2137  if (force ||
2138  m_config.m_deviceInfo[bid - 1].m_outputSettings != settings)
2139  {
2140  CMT3LOG(
2141  "L3: setDeviceMode setting MT %08x output settings to %08X\n",
2142  deviceId, (uint32_t)settings);
2143  // changed = true;
2144  snd.setMessageId(CMT_MID_REQOUTPUTSETTINGS);
2145  snd.setDataLong(settings);
2146  m_serial.writeMessage(&snd);
2147  m_lastResult = m_serial.waitForMessage(
2148  &rcv, CMT_MID_REQOUTPUTSETTINGSACK, 0, true);
2149  if (m_lastResult != XRV_OK) return m_lastResult;
2150  if (m_logging) m_logFile.writeMessage(&rcv);
2151  HANDLE_ERR_RESULT;
2152  m_config.m_deviceInfo[bid - 1].m_outputSettings =
2153  settings; // update device info
2154  }
2155  }
2156 
2157  return m_lastResult = XRV_OK;
2158 }
2159 
2160 //////////////////////////////////////////////////////////////////////////////////////////
2161 // Retrieve the error mode
2162 XsensResultValue Cmt3::setErrorMode(const uint16_t mode)
2163 {
2164  CMT3LOG("L3: setErrorMode %u\n", (uint32_t)mode);
2165  CMT3EXITLOG;
2166 
2167  uint8_t mid;
2168  if (isXm())
2169  mid = CMT_MID_REQXMERRORMODE;
2170  else
2171  mid = CMT_MID_REQERRORMODE;
2172 
2173  DO_DATA_SET_BID(mid, CMT_LEN_ERRORMODE, Short, mode, CMT_BID_MASTER);
2174  return m_lastResult = XRV_OK;
2175 }
2176 
2177 //////////////////////////////////////////////////////////////////////////////////////////
2178 // Set the number of times the gotoConfig function will attempt gotoConfig
2179 // before failing
2180 XsensResultValue Cmt3::setGotoConfigTries(const uint16_t tries)
2181 {
2182  CMT3LOG("L3: setGotoConfigTries %u\n", (uint32_t)tries);
2183  CMT3EXITLOG;
2184 
2185  m_gotoConfigTries = tries;
2186  return m_lastResult = XRV_OK;
2187 }
2188 
2189 //////////////////////////////////////////////////////////////////////////////////////////
2190 // Set the heading offset of a device. The valid range is -pi to +pi.
2191 XsensResultValue Cmt3::setHeading(
2192  const double heading, const CmtDeviceId deviceId)
2193 {
2194  CMT3LOG("L3: setHeading %f %08x\n", heading, deviceId);
2195  CMT3EXITLOG;
2196 
2197  DO_DATA_SET(CMT_MID_REQHEADING, CMT_LEN_HEADING, Float, (float)heading);
2198  return m_lastResult = XRV_OK;
2199 }
2200 
2201 //////////////////////////////////////////////////////////////////////////////////////////
2202 // Set the location ID of a device. The buffer should be no more than 20 bytes.
2203 XsensResultValue Cmt3::setLocationId(
2204  uint16_t locationId, const CmtDeviceId deviceId)
2205 {
2206  CMT3LOG("L3: setLocationId %u %08x\n", (uint32_t)locationId, deviceId);
2207  CMT3EXITLOG;
2208 
2209  DO_DATA_SET(CMT_MID_REQLOCATIONID, CMT_LEN_LOCATIONID, Short, locationId);
2210  return m_lastResult = XRV_OK;
2211 }
2212 
2213 //////////////////////////////////////////////////////////////////////////////////////////
2214 // Set the magnetic declination offset of a device. The valid range is -pi to
2215 // +pi.
2216 XsensResultValue Cmt3::setMagneticDeclination(
2217  const double declination, const CmtDeviceId deviceId)
2218 {
2219  CMT3LOG("L3: setMagneticDeclination %f %08x\n", declination, deviceId);
2220  CMT3EXITLOG;
2221 
2222  DO_DATA_SET(
2223  CMT_MID_SETMAGNETICDECLINATION, CMT_LEN_MAGNETICDECLINATION, Float,
2224  (float)declination);
2225  return m_lastResult = XRV_OK;
2226 }
2227 
2228 //////////////////////////////////////////////////////////////////////////////////////////
2229 // Switch the XM off
2230 XsensResultValue Cmt3::setXmPowerOff(void)
2231 {
2232  CMT3LOG("L3: setXmPowerOff\n");
2233  CMT3EXITLOG;
2234 
2235  if (!isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2236  Message snd(CMT_MID_XMPWROFF, 0);
2237  snd.setBusId(CMT_BID_MASTER);
2238  return m_lastResult = m_serial.writeMessage(&snd);
2239 }
2240 
2241 //////////////////////////////////////////////////////////////////////////////////////////
2242 // Set the inbound synchronization settings of a device.
2243 XsensResultValue Cmt3::setSyncInSettings(const CmtSyncInSettings& settings)
2244 {
2245  CMT3LOG(
2246  "L3: setSyncInSettings %u %u %u\n", (uint32_t)settings.m_mode,
2247  settings.m_offset, (uint32_t)settings.m_skipFactor);
2248  CMT3EXITLOG;
2249 
2250  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2251 
2252  Message snd(CMT_MID_REQSYNCINSETTINGS, 3);
2253  Message rcv;
2254 
2255  snd.setBusId(CMT_BID_MASTER);
2256 
2257  snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
2258  snd.setDataShort(settings.m_mode, 1);
2259  m_serial.writeMessage(&snd);
2260  m_lastResult =
2261  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
2262  if (m_lastResult != XRV_OK) return m_lastResult;
2263  if (m_logging) m_logFile.writeMessage(&rcv);
2264  HANDLE_ERR_RESULT;
2265 
2266  snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
2267  snd.setDataShort(settings.m_skipFactor, 1);
2268  m_serial.writeMessage(&snd);
2269  m_lastResult =
2270  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
2271  if (m_lastResult != XRV_OK) return m_lastResult;
2272  if (m_logging) m_logFile.writeMessage(&rcv);
2273  HANDLE_ERR_RESULT;
2274 
2275  snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
2276  snd.setDataLong(
2277  (uint32_t)(
2278  ((double)settings.m_offset) * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),
2279  1);
2280  m_serial.writeMessage(&snd);
2281  m_lastResult =
2282  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
2283  if (m_lastResult == XRV_OK)
2284  {
2285  if (m_logging) m_logFile.writeMessage(&rcv);
2286  HANDLE_ERR_RESULT;
2287  }
2288 
2289  return m_lastResult;
2290 }
2291 
2292 //////////////////////////////////////////////////////////////////////////////////////////
2293 // Set the inbound synchronization mode of a device.
2294 XsensResultValue Cmt3::setSyncInMode(const uint16_t mode)
2295 {
2296  CMT3LOG("L3: setSyncInMode %u\n", (uint32_t)mode);
2297  CMT3EXITLOG;
2298 
2299  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2300 
2301  Message snd(CMT_MID_REQSYNCINSETTINGS, 3);
2302  Message rcv;
2303 
2304  snd.setBusId(CMT_BID_MASTER);
2305 
2306  snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
2307  snd.setDataShort(mode, 1);
2308  m_serial.writeMessage(&snd);
2309  m_lastResult =
2310  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
2311  if (m_lastResult != XRV_OK) return m_lastResult;
2312  if (m_logging) m_logFile.writeMessage(&rcv);
2313  HANDLE_ERR_RESULT;
2314 
2315  return m_lastResult;
2316 }
2317 
2318 //////////////////////////////////////////////////////////////////////////////////////////
2319 // Set the inbound synchronization skip factor of a device.
2320 XsensResultValue Cmt3::setSyncInSkipFactor(const uint16_t skipFactor)
2321 {
2322  CMT3LOG("L3: setSyncInSettings %u\n", skipFactor);
2323  CMT3EXITLOG;
2324 
2325  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2326 
2327  Message snd(CMT_MID_REQSYNCINSETTINGS, 3);
2328  Message rcv;
2329 
2330  snd.setBusId(CMT_BID_MASTER);
2331 
2332  snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
2333  snd.setDataShort(skipFactor, 1);
2334  m_serial.writeMessage(&snd);
2335  m_lastResult =
2336  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
2337  if (m_lastResult != XRV_OK) return m_lastResult;
2338  if (m_logging) m_logFile.writeMessage(&rcv);
2339  HANDLE_ERR_RESULT;
2340 
2341  return m_lastResult;
2342 }
2343 
2344 //////////////////////////////////////////////////////////////////////////////////////////
2345 // Set the inbound synchronization offset of a device.
2346 XsensResultValue Cmt3::setSyncInOffset(const uint32_t offset)
2347 {
2348  CMT3LOG("L3: setSyncInSettings %u\n", offset);
2349  CMT3EXITLOG;
2350 
2351  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2352 
2353  Message snd(CMT_MID_REQSYNCINSETTINGS, 3);
2354  Message rcv;
2355 
2356  snd.setBusId(CMT_BID_MASTER);
2357 
2358  snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
2359  snd.setDataLong(
2360  (uint32_t)(((double)offset) * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5), 1);
2361  m_serial.writeMessage(&snd);
2362  m_lastResult =
2363  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCINSETTINGSACK, 0, true);
2364  if (m_lastResult == XRV_OK)
2365  {
2366  if (m_logging) m_logFile.writeMessage(&rcv);
2367  HANDLE_ERR_RESULT;
2368  }
2369 
2370  return m_lastResult;
2371 }
2372 
2373 //////////////////////////////////////////////////////////////////////////////////////////
2374 // Set the synchronization mode of the XM
2375 XsensResultValue Cmt3::setSyncMode(const uint8_t mode)
2376 {
2377  CMT3LOG("L3: setSyncMode %u\n", (uint32_t)mode);
2378  CMT3EXITLOG;
2379 
2380  if (!isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2381 
2382  DO_DATA_SET_BID(
2383  CMT_MID_REQSYNCMODE, CMT_LEN_SYNCMODE, Byte, mode, CMT_BID_MASTER);
2384  return m_lastResult = XRV_OK;
2385 }
2386 
2387 //////////////////////////////////////////////////////////////////////////////////////////
2388 // Set the outbound synchronization settings of a device.
2389 XsensResultValue Cmt3::setSyncOutSettings(const CmtSyncOutSettings& settings)
2390 {
2391  CMT3LOG(
2392  "L3: setSyncOutSettings %u %u %u %u\n", (uint32_t)settings.m_mode,
2393  settings.m_offset, (uint32_t)settings.m_pulseWidth,
2394  (uint32_t)settings.m_skipFactor);
2395  CMT3EXITLOG;
2396 
2397  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2398 
2399  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 3);
2400  Message rcv;
2401 
2402  snd.setBusId(CMT_BID_MASTER);
2403 
2404  snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
2405  snd.setDataShort(settings.m_mode, 1);
2406  m_serial.writeMessage(&snd);
2407  m_lastResult =
2408  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2409  if (m_lastResult != XRV_OK) return m_lastResult;
2410  if (m_logging) m_logFile.writeMessage(&rcv);
2411  HANDLE_ERR_RESULT;
2412 
2413  snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
2414  snd.setDataShort(settings.m_skipFactor, 1);
2415  m_serial.writeMessage(&snd);
2416  m_lastResult =
2417  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2418  if (m_lastResult != XRV_OK) return m_lastResult;
2419  if (m_logging) m_logFile.writeMessage(&rcv);
2420  HANDLE_ERR_RESULT;
2421 
2422  snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
2423  snd.setDataLong(
2424  (uint32_t)(
2425  ((double)settings.m_offset) * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),
2426  1);
2427  m_serial.writeMessage(&snd);
2428  m_lastResult =
2429  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2430  if (m_lastResult != XRV_OK) return m_lastResult;
2431  if (m_logging) m_logFile.writeMessage(&rcv);
2432  HANDLE_ERR_RESULT;
2433 
2434  snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
2435  snd.setDataLong(
2436  (uint32_t)(
2437  ((double)settings.m_pulseWidth) * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),
2438  1);
2439  m_serial.writeMessage(&snd);
2440  m_lastResult =
2441  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2442  if (m_lastResult != XRV_OK) return m_lastResult;
2443  if (m_logging) m_logFile.writeMessage(&rcv);
2444  HANDLE_ERR_RESULT;
2445 
2446  return m_lastResult = XRV_OK;
2447 }
2448 
2449 //////////////////////////////////////////////////////////////////////////////////////////
2450 // Set the outbound synchronization mode of a device.
2451 XsensResultValue Cmt3::setSyncOutMode(const uint16_t mode)
2452 {
2453  CMT3LOG("L3: setSyncOutSettings %u\n", (uint32_t)mode);
2454  CMT3EXITLOG;
2455 
2456  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2457 
2458  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 3);
2459  Message rcv;
2460 
2461  snd.setBusId(CMT_BID_MASTER);
2462 
2463  snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
2464  snd.setDataShort(mode, 1);
2465  m_serial.writeMessage(&snd);
2466  m_lastResult =
2467  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2468  if (m_lastResult != XRV_OK) return m_lastResult;
2469  if (m_logging) m_logFile.writeMessage(&rcv);
2470  HANDLE_ERR_RESULT;
2471 
2472  return m_lastResult = XRV_OK;
2473 }
2474 
2475 //////////////////////////////////////////////////////////////////////////////////////////
2476 // Set the outbound synchronization pulse width of a device.
2477 XsensResultValue Cmt3::setSyncOutPulseWidth(const uint32_t pulseWidth)
2478 {
2479  CMT3LOG("L3: setSyncOutSettings %u\n", pulseWidth);
2480  CMT3EXITLOG;
2481 
2482  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2483 
2484  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 3);
2485  Message rcv;
2486 
2487  snd.setBusId(CMT_BID_MASTER);
2488 
2489  snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
2490  snd.setDataLong(
2491  (uint32_t)(((double)pulseWidth) * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5), 1);
2492  m_serial.writeMessage(&snd);
2493  m_lastResult =
2494  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2495  if (m_lastResult != XRV_OK) return m_lastResult;
2496  if (m_logging) m_logFile.writeMessage(&rcv);
2497  HANDLE_ERR_RESULT;
2498 
2499  return m_lastResult = XRV_OK;
2500 }
2501 
2502 //////////////////////////////////////////////////////////////////////////////////////////
2503 // Set the outbound synchronization skip factor of a device.
2504 XsensResultValue Cmt3::setSyncOutSkipFactor(const uint16_t skipFactor)
2505 {
2506  CMT3LOG("L3: setSyncOutSettings %u\n", skipFactor);
2507  CMT3EXITLOG;
2508 
2509  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2510 
2511  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 3);
2512  Message rcv;
2513 
2514  snd.setBusId(CMT_BID_MASTER);
2515 
2516  snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
2517  snd.setDataShort(skipFactor, 1);
2518  m_serial.writeMessage(&snd);
2519  m_lastResult =
2520  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2521  if (m_lastResult != XRV_OK) return m_lastResult;
2522  if (m_logging) m_logFile.writeMessage(&rcv);
2523  HANDLE_ERR_RESULT;
2524 
2525  return m_lastResult = XRV_OK;
2526 }
2527 
2528 //////////////////////////////////////////////////////////////////////////////////////////
2529 // Set the outbound synchronization offset of a device.
2530 XsensResultValue Cmt3::setSyncOutOffset(const uint32_t offset)
2531 {
2532  CMT3LOG("L3: setSyncOutSettings %u\n", offset);
2533  CMT3EXITLOG;
2534 
2535  if (isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2536 
2537  Message snd(CMT_MID_REQSYNCOUTSETTINGS, 3);
2538  Message rcv;
2539 
2540  snd.setBusId(CMT_BID_MASTER);
2541 
2542  snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
2543  snd.setDataLong(
2544  (uint32_t)(((double)offset) * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5), 1);
2545  m_serial.writeMessage(&snd);
2546  m_lastResult =
2547  m_serial.waitForMessage(&rcv, CMT_MID_REQSYNCOUTSETTINGSACK, 0, true);
2548  if (m_lastResult != XRV_OK) return m_lastResult;
2549  if (m_logging) m_logFile.writeMessage(&rcv);
2550  HANDLE_ERR_RESULT;
2551 
2552  return m_lastResult = XRV_OK;
2553 }
2554 
2555 //////////////////////////////////////////////////////////////////////////////////////////
2556 // Set the configuration mode timeout value in ms.
2557 XsensResultValue Cmt3::setTimeoutConfig(const uint32_t timeout)
2558 {
2559  CMT3LOG("L3: setTimeoutConfig %u\n", timeout);
2560  CMT3EXITLOG;
2561 
2562  m_timeoutConf = timeout;
2563  if (!m_measuring) return m_lastResult = m_serial.setTimeout(m_timeoutConf);
2564  return m_lastResult = XRV_OK;
2565 }
2566 
2567 //////////////////////////////////////////////////////////////////////////////////////////
2568 // Set the measurement mode timeout value in ms.
2569 XsensResultValue Cmt3::setTimeoutMeasurement(const uint32_t timeout)
2570 {
2571  CMT3LOG("L3: setTimeoutMeasurement %u\n", timeout);
2572  CMT3EXITLOG;
2573 
2574  m_timeoutMeas = timeout;
2575  if (m_measuring) return m_lastResult = m_serial.setTimeout(m_timeoutMeas);
2576  return m_lastResult = XRV_OK;
2577 }
2578 
2579 //////////////////////////////////////////////////////////////////////////////////////////
2580 // Set the dual-mode output settings of the XM
2581 XsensResultValue Cmt3::setXmOutputMode(const uint8_t mode)
2582 {
2583  CMT3LOG("L3: setXmOutputMode %u\n", (uint32_t)mode);
2584  CMT3EXITLOG;
2585 
2586  if (!isXm()) return m_lastResult = XRV_INVALIDOPERATION;
2587 
2588  DO_DATA_SET_BID(
2589  CMT_MID_REQOPMODE, CMT_LEN_OPMODE, Byte, mode, CMT_BID_MASTER);
2590  return m_lastResult = XRV_OK;
2591 }
2592 
2593 //////////////////////////////////////////////////////////////////////////////////////////
2594 XsensResultValue Cmt3::refreshCache(const bool file)
2595 {
2596  CMT3LOG("L3: refreshCache %d\n", file ? 1 : 0);
2597  CMT3EXITLOG;
2598 
2599  if (m_serial.isOpen() && (!file || !m_logFile.isOpen()))
2600  {
2601  // clear eMts cache
2602  for (uint32_t i = 0; i < CMT_MAX_DEVICES_PER_PORT; ++i)
2603  {
2604  CHKFREENUL(m_eMtsData[i]);
2605  }
2606 
2607  // port open, go to configuration mode
2608  if (m_measuring && gotoConfig() != XRV_OK)
2609  return m_lastResult; // m_lastResult is already set by gotoConfig()
2610 
2611  // now in configuration mode, read device information
2612  CMT3LOG(
2613  "L3: refreshCache Device in configuration mode, reading device "
2614  "information\n");
2615 
2616  Message snd;
2617  Message rcv;
2618 
2619  // all information is in the Configuration message
2620  snd.setMessageId(CMT_MID_REQCONFIGURATION);
2621  m_serial.writeMessage(&snd);
2622  if ((m_lastResult = m_serial.waitForMessage(
2623  &rcv, CMT_MID_CONFIGURATION, 0, false)) != XRV_OK)
2624  return m_lastResult;
2625  if (m_logging) m_logFile.writeMessage(&rcv);
2626  m_config.readFromMessage(rcv.getMessageStart());
2627 
2628  m_period = m_config.m_samplingPeriod;
2629  m_skip = m_config.m_outputSkipFactor;
2630  // CmtDeviceMode2 mode;
2631  // mode.setPeriodAndSkipFactor(m_config.m_samplingPeriod,m_config.m_outputSkipFactor);
2632  // mode.m_period = m_config.m_samplingPeriod;
2633  // mode.m_skip = m_config.m_outputSkipFactor;
2634  // m_sampleFrequency = mode.getRealSampleFrequency();
2635 
2636  return m_lastResult = XRV_OK;
2637  }
2638  else if (m_logFile.isOpen() && (file || !m_serial.isOpen()))
2639  {
2640  // clear eMts cache
2641  for (uint32_t i = 0; i < CMT_MAX_DEVICES_PER_PORT; ++i)
2642  {
2643  CHKFREENUL(m_eMtsData[i]);
2644  }
2645 
2646  // now in configuration mode, read device information
2647  CMT3LOG(
2648  "L3: refreshCache Reading device configuration information from "
2649  "file\n");
2650 
2651  Message rcv;
2652 
2653  if ((m_lastResult =
2654  m_logFile.readMessage(&rcv, CMT_MID_CONFIGURATION)) != XRV_OK)
2655  return m_lastResult;
2656  m_config.readFromMessage(rcv.getMessageStart());
2657 
2658  m_period = m_config.m_samplingPeriod;
2659  m_skip = m_config.m_outputSkipFactor;
2660  // CmtDeviceMode2 mode;
2661  // mode.setPeriodAndSkipFactor(m_config.m_samplingPeriod,m_config.m_outputSkipFactor);
2662  // mode.m_period = m_config.m_samplingPeriod;
2663  // mode.m_skip = m_config.m_outputSkipFactor;
2664  // m_sampleFrequency = mode.getRealSampleFrequency();
2665 
2666  return m_lastResult = XRV_OK;
2667  }
2668  else
2669  return m_lastResult = XRV_NOFILEORPORTOPEN;
2670 }
2671 
2672 //////////////////////////////////////////////////////////////////////////////////////////
2673 // Wait for a data message to arrive.
2674 XsensResultValue Cmt3::waitForDataMessage(Packet* pack)
2675 {
2676  CMT3LOGDAT("L3: waitForDataMessage %p\n", pack);
2677  CMT3EXITLOGDAT;
2678 
2679  m_lastResult = XRV_TIMEOUTNODATA;
2680 
2681  uint32_t toEnd =
2682  (getTimeOfDay() + (uint32_t)m_timeoutMeas) % (XSENS_MS_PER_DAY);
2683  while (toEnd >= getTimeOfDay())
2684  {
2685  m_lastResult =
2686  m_serial.waitForMessage(&pack->m_msg, CMT_MID_MTDATA, 0, true);
2687  if (m_lastResult == XRV_OK)
2688  {
2689  if (m_logging) m_logFile.writeMessage(&pack->m_msg);
2690  if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
2691  {
2692  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
2693  if (pack->m_msg.getDataSize() >= 2)
2694  {
2695  uint8_t biddy = pack->m_msg.getDataByte(1);
2696  getDeviceId(biddy, m_lastHwErrorDeviceId);
2697  }
2698  return m_lastResult = m_lastHwError =
2699  (XsensResultValue)pack->m_msg.getDataByte(0);
2700  }
2701 
2702  pack->m_itemCount = m_config.m_numberOfDevices;
2703  for (uint16_t i = 0; i < m_config.m_numberOfDevices; ++i)
2704  pack->setDataFormat(
2705  m_config.m_deviceInfo[i].m_outputMode,
2706  m_config.m_deviceInfo[i].m_outputSettings, i);
2707  pack->m_toa = timeStampNow();
2708  if (m_useRtc) fillRtc(pack);
2709 
2710  return m_lastResult = XRV_OK;
2711  }
2712  }
2713  return m_lastResult; // = XRV_TIMEOUT;
2714 }
2715 
2716 //////////////////////////////////////////////////////////////////////////////////////////
2717 XsensResultValue Cmt3::createLogFile(const char* filename, bool startLogging)
2718 {
2719  CMT3LOG(
2720  "L3: createLogFile \"%s\" %u\n", filename ? filename : "",
2721  startLogging ? 1 : 0);
2722  CMT3EXITLOG;
2723 
2724  if (!m_serial.isOpen()) return m_lastResult = XRV_NOPORTOPEN;
2725  if (m_logFile.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
2726  m_lastResult = m_logFile.create(filename);
2727  if (m_lastResult == XRV_OK)
2728  {
2729  m_logging = true;
2730  CmtDeviceConfiguration config;
2731  if (getConfiguration(config) == XRV_OK)
2732  {
2733  void* buffer =
2734  malloc(CMT_EMTS_SIZE * (m_config.m_numberOfDevices + 1));
2735  getEMtsData(buffer, CMT_DID_BROADCAST);
2736  free(buffer);
2737  m_logging = startLogging;
2738  }
2739  }
2740 
2741  if (m_lastResult != XRV_OK)
2742  {
2743  m_logFile.closeAndDelete();
2744  m_logging = false;
2745  }
2746  return m_lastResult;
2747 }
2748 
2749 //////////////////////////////////////////////////////////////////////////////////////////
2750 XsensResultValue Cmt3::createLogFile(const wchar_t* filename, bool startLogging)
2751 {
2752  CMT3LOG(
2753  "L3: createLogFile \"%S\" %u\n", filename ? filename : L"",
2754  startLogging ? 1 : 0);
2755  CMT3EXITLOG;
2756 
2757  if (!m_serial.isOpen()) return m_lastResult = XRV_NOPORTOPEN;
2758  if (m_logFile.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
2759  m_lastResult = m_logFile.create(filename);
2760  if (m_lastResult == XRV_OK)
2761  {
2762  m_logging = true;
2763  CmtDeviceConfiguration config;
2764  if (getConfiguration(config) == XRV_OK)
2765  {
2766  void* buffer =
2767  malloc(CMT_EMTS_SIZE * (m_config.m_numberOfDevices + 1));
2768  getEMtsData(buffer, CMT_DID_BROADCAST);
2769  free(buffer);
2770  m_logging = startLogging;
2771  }
2772  }
2773 
2774  if (m_lastResult != XRV_OK)
2775  {
2776  m_logFile.closeAndDelete();
2777  m_logging = false;
2778  }
2779  return m_lastResult;
2780 }
2781 
2782 //////////////////////////////////////////////////////////////////////////////////////////
2783 XsensResultValue Cmt3::closeLogFile(bool del)
2784 {
2785  CMT3LOG("L3: closeLogFile %u\n", del ? 1 : 0);
2786  CMT3EXITLOG;
2787 
2788  m_logging = false;
2789  if (!m_logFile.isOpen()) return m_lastResult = XRV_NOFILEOPEN;
2790  if (del)
2791  return m_lastResult = m_logFile.closeAndDelete();
2792  else
2793  return m_lastResult = m_logFile.close();
2794 }
2795 
2796 //////////////////////////////////////////////////////////////////////////////////////////
2797 bool Cmt3::isLogFileOpen(const char* filename) const
2798 {
2799  CMT3LOG("L3: isLogFileOpen \"%s\"\n", filename ? filename : "");
2800 
2801  if (m_logFile.isOpen())
2802  {
2803  if (filename != nullptr && filename[0] != 0)
2804  {
2805  char fn[CMT_MAX_FILENAME_LENGTH];
2806  m_logFile.getName(fn);
2807  if (_strnicmp(filename, fn, CMT_MAX_FILENAME_LENGTH) != 0)
2808  return false;
2809  }
2810  return true;
2811  }
2812  return false;
2813 }
2814 
2815 //////////////////////////////////////////////////////////////////////////////////////////
2816 bool Cmt3::isLogFileOpen(const wchar_t* filename) const
2817 {
2818  CMT3LOG("L3: isLogFileOpen \"%S\"\n", filename ? filename : L"");
2819 
2820  if (m_logFile.isOpen())
2821  {
2822  if (filename != nullptr && filename[0] != L'\0')
2823  {
2824  wchar_t fn[CMT_MAX_FILENAME_LENGTH];
2825  m_logFile.getName(fn);
2826  if (_wcsnicmp(filename, fn, CMT_MAX_FILENAME_LENGTH) != 0)
2827  return false;
2828  }
2829  return true;
2830  }
2831  return false;
2832 }
2833 
2834 //////////////////////////////////////////////////////////////////////////////////////////
2835 XsensResultValue Cmt3::openLogFile(const char* filename)
2836 {
2837  CMT3LOG("L3: openLogFile \"%s\"\n", filename ? filename : "");
2838  CMT3EXITLOG;
2839 
2840  m_logging = false;
2841  if (m_serial.isOpen()) return m_lastResult = XRV_INVALIDOPERATION;
2842  if (m_logFile.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
2843  m_lastResult = m_logFile.open(filename, true);
2844  if (m_lastResult == XRV_OK)
2845  {
2846  if (refreshCache() == XRV_OK)
2847  m_readFromFile = true;
2848  else
2849  {
2850  m_logFile.close();
2851  m_readFromFile = false;
2852  }
2853  }
2854  return m_lastResult;
2855 }
2856 
2857 //////////////////////////////////////////////////////////////////////////////////////////
2858 XsensResultValue Cmt3::openLogFile(const wchar_t* filename)
2859 {
2860  CMT3LOG("L3: openLogFile \"%S\"\n", filename ? filename : L"");
2861  CMT3EXITLOG;
2862 
2863  m_logging = false;
2864  if (m_serial.isOpen()) return m_lastResult = XRV_INVALIDOPERATION;
2865  if (m_logFile.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
2866  m_lastResult = m_logFile.open(filename, true);
2867  if (m_lastResult == XRV_OK)
2868  {
2869  if (refreshCache() == XRV_OK)
2870  m_readFromFile = true;
2871  else
2872  {
2873  m_logFile.close();
2874  m_readFromFile = false;
2875  }
2876  }
2877  return m_lastResult;
2878 }
2879 
2880 //////////////////////////////////////////////////////////////////////////////////////////
2881 XsensResultValue Cmt3::setDataSource(bool readFromFile)
2882 {
2883  CMT3LOG("L3: setDataSource %s\n", readFromFile ? "file" : "port");
2884  CMT3EXITLOG;
2885 
2886  if (readFromFile)
2887  {
2888  m_logging = false;
2889  if (m_logFile.isOpen())
2890  {
2891  m_readFromFile = true;
2892  return m_lastResult = XRV_OK;
2893  }
2894  m_readFromFile = false;
2895  return m_lastResult = XRV_INVALIDOPERATION;
2896  }
2897  if (m_serial.isOpen())
2898  {
2899  m_readFromFile = false;
2900  return m_lastResult = XRV_OK;
2901  }
2902  if (m_logFile.isOpen())
2903  {
2904  m_readFromFile = true;
2905  return m_lastResult = XRV_INVALIDOPERATION;
2906  }
2907  m_readFromFile = false;
2908  return m_lastResult = XRV_NOFILEORPORTOPEN;
2909 }
2910 
2911 //////////////////////////////////////////////////////////////////////////////////////////
2912 XsensResultValue Cmt3::setLogMode(bool active)
2913 {
2914  CMT3LOG("L3: setLogMode %u\n", active ? 1 : 0);
2915  CMT3EXITLOG;
2916 
2917  if (active && (m_readFromFile || !m_logFile.isOpen()))
2918  return m_lastResult = XRV_NOFILEOPEN;
2919  m_logging = active;
2920  return m_lastResult = XRV_OK;
2921 }
2922 
2923 //////////////////////////////////////////////////////////////////////////////////////////
2924 XsensResultValue Cmt3::resetLogFileReadPos(void)
2925 {
2926  CMT3LOG("L3: resetLogFileReadPos\n");
2927  CMT3EXITLOG;
2928 
2929  return m_lastResult = m_logFile.setReadPosition(0);
2930 }
2931 
2932 //////////////////////////////////////////////////////////////////////////////////////////
2933 XsensResultValue Cmt3::writeMessageToLogFile(const Message& msg)
2934 {
2935  CMT3LOG("L3: writeMessageToLogFile\n");
2936  CMT3EXITLOG;
2937 
2938  if (!m_logFile.isOpen()) return m_lastResult = XRV_NOFILEOPEN;
2939 
2940  return m_lastResult = m_logFile.writeMessage(&msg);
2941 }
2942 
2943 //////////////////////////////////////////////////////////////////////////////////////////
2944 XsensResultValue Cmt3::getAvailableScenarios(
2945  CmtScenario* scenarios, const CmtDeviceId deviceId)
2946 {
2947  CMT3LOG("L3: getAvailableScenarios %p %08x\n", scenarios, deviceId);
2948  CMT3EXITLOG;
2949 
2950  DO_DATA_REQUEST(CMT_MID_REQAVAILABLESCENARIOS);
2951 
2952  char tp = 0;
2953  switch (deviceId & CMT_DID_TYPEH_MASK)
2954  {
2955  case CMT_DID_TYPEH_MTIG:
2956  tp = '6';
2957  break;
2958  case CMT_DID_TYPEH_MTI_MTX:
2959  tp = '3';
2960  break;
2961  }
2962 
2963  for (int i = 0; i < CMT_MAX_SCENARIOS_IN_MT; ++i)
2964  {
2965  scenarios[i].m_type =
2966  rcv.getDataByte(0 + i * (1 + 1 + CMT_LEN_SCENARIOLABEL));
2967  scenarios[i].m_version =
2968  rcv.getDataByte(1 + i * (1 + 1 + CMT_LEN_SCENARIOLABEL));
2969  memcpy(
2970  scenarios[i].m_label,
2971  rcv.getDataBuffer(2 + i * (1 + 1 + CMT_LEN_SCENARIOLABEL)),
2972  CMT_LEN_SCENARIOLABEL);
2973  scenarios[i].m_label[CMT_LEN_SCENARIOLABEL] = 0;
2974  scenarios[i].m_filterType = tp;
2975  }
2976  return m_lastResult = XRV_OK;
2977 }
2978 
2979 XsensResultValue Cmt3::getScenario(
2980  uint8_t& scenarioType, uint8_t& scenarioVersion, const CmtDeviceId deviceId)
2981 {
2982  CMT3LOG("L3: getScenario %08x\n", deviceId);
2983  CMT3EXITLOG;
2984 
2985  DO_DATA_REQUEST(CMT_MID_REQSCENARIO);
2986  scenarioType = rcv.getDataByte(1);
2987  scenarioVersion = rcv.getDataByte(0);
2988  return m_lastResult = XRV_OK;
2989 }
2990 
2991 XsensResultValue Cmt3::setScenario(
2992  const uint8_t scenarioType, const CmtDeviceId deviceId)
2993 {
2994  CMT3LOG("L3: setScenario %u %08x\n", (uint32_t)scenarioType, deviceId);
2995  CMT3EXITLOG;
2996 
2997  uint16_t scenario = (uint16_t)scenarioType;
2998  DO_DATA_SET(CMT_MID_SETSCENARIO, CMT_LEN_SETSCENARIO, Short, scenario);
2999  return m_lastResult = XRV_OK;
3000 }
3001 
3002 XsensResultValue Cmt3::getGravityMagnitude(
3003  double& magnitude, const CmtDeviceId deviceId)
3004 {
3005  CMT3LOG("L3: getGravityMagnitude %08x\n", deviceId);
3006  CMT3EXITLOG;
3007 
3008  DO_DATA_REQUEST(CMT_MID_REQGRAVITYMAGNITUDE);
3009  magnitude = rcv.getDataFloat(0);
3010  return m_lastResult = XRV_OK;
3011 }
3012 
3013 XsensResultValue Cmt3::setGravityMagnitude(
3014  const double magnitude, const CmtDeviceId deviceId)
3015 {
3016  CMT3LOG("L3: setGravityMagnitude %f %08x\n", magnitude, deviceId);
3017  CMT3EXITLOG;
3018 
3019  DO_DATA_SET(
3020  CMT_MID_SETGRAVITYMAGNITUDE, CMT_LEN_GRAVITYMAGNITUDE, Float,
3021  (float)magnitude);
3022  return m_lastResult = XRV_OK;
3023 }
3024 
3025 XsensResultValue Cmt3::getGpsLeverArm(
3026  CmtVector& arm, const CmtDeviceId deviceId)
3027 {
3028  CMT3LOG("L3: getGpsLeverArm %08x\n", deviceId);
3029  CMT3EXITLOG;
3030 
3031  DO_DATA_REQUEST(CMT_MID_REQGPSLEVERARM);
3032  arm.m_data[0] = rcv.getDataFloat(0);
3033  arm.m_data[1] = rcv.getDataFloat(4);
3034  arm.m_data[2] = rcv.getDataFloat(8);
3035  return m_lastResult = XRV_OK;
3036 }
3037 
3038 XsensResultValue Cmt3::getGpsStatus(
3039  CmtGpsStatus& status, const CmtDeviceId deviceId)
3040 {
3041  CMT3LOG("L3: getGpsStatus %08x\n", deviceId);
3042  CMT3EXITLOG;
3043 
3044  DO_DATA_REQUEST(CMT_MID_REQGPSSTATUS);
3045  for (uint16_t i = 0; i < CMT_MAX_SVINFO; ++i)
3046  {
3047  status.m_svInfo[i].m_id = rcv.getDataByte(i * 5 + 2);
3048  status.m_svInfo[i].m_navigationStatus = rcv.getDataByte(i * 5 + 3);
3049  status.m_svInfo[i].m_signalQuality = rcv.getDataByte(i * 5 + 4);
3050  status.m_svInfo[i].m_signalStrength = rcv.getDataByte(i * 5 + 5);
3051  }
3052  return m_lastResult = XRV_OK;
3053 }
3054 
3055 XsensResultValue Cmt3::setGpsLeverArm(
3056  const CmtVector& arm, const CmtDeviceId deviceId)
3057 {
3058  CMT3LOG(
3059  "L3: setGpsLeverArm [%f %f %f] %08x\n", arm.m_data[0], arm.m_data[1],
3060  arm.m_data[2], deviceId);
3061  CMT3EXITLOG;
3062 
3063  uint8_t bid = getBusIdInternal(deviceId);
3064  if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)
3065  return (m_lastResult = XRV_INVALIDID);
3066 
3067  Message snd(CMT_MID_SETGPSLEVERARM, CMT_LEN_GPSLEVERARM);
3068  Message rcv;
3069  snd.setDataFloat((float)arm.m_data[0], 0);
3070  snd.setDataFloat((float)arm.m_data[1], 4);
3071  snd.setDataFloat((float)arm.m_data[2], 8);
3072 
3073  snd.setBusId(bid);
3074  m_serial.writeMessage(&snd);
3075  m_lastResult =
3076  m_serial.waitForMessage(&rcv, CMT_MID_SETGPSLEVERARMACK, 0, true);
3077  if (m_lastResult != XRV_OK) return m_lastResult;
3078  if (m_logging) m_logFile.writeMessage(&rcv);
3079  if (rcv.getMessageId() == CMT_MID_ERROR)
3080  {
3081  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
3082  if (rcv.getDataSize() >= 2)
3083  {
3084  uint8_t biddy = rcv.getDataByte(1);
3085  getDeviceId(biddy, m_lastHwErrorDeviceId);
3086  }
3087  return m_lastResult = m_lastHwError =
3088  (XsensResultValue)rcv.getDataByte();
3089  }
3090  return m_lastResult = XRV_OK;
3091 }
3092 
3093 XsensResultValue Cmt3::storeXkfState(const CmtDeviceId deviceId)
3094 {
3095  CMT3LOG("L3: storeXkfState %08x\n", deviceId);
3096  CMT3EXITLOG;
3097 
3098  uint8_t bid = getBusIdInternal(deviceId);
3099  if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)
3100  return (m_lastResult = XRV_INVALIDID);
3101 
3102  Message snd(CMT_MID_STOREXKFSTATE, CMT_LEN_STOREXKFSTATE);
3103  Message rcv;
3104 
3105  snd.setBusId(bid);
3106  m_serial.writeMessage(&snd);
3107  m_lastResult =
3108  m_serial.waitForMessage(&rcv, CMT_MID_STOREXKFSTATEACK, 0, true);
3109  if (m_lastResult != XRV_OK) return m_lastResult;
3110  if (m_logging) m_logFile.writeMessage(&rcv);
3111  if (rcv.getMessageId() == CMT_MID_ERROR)
3112  {
3113  m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
3114  if (rcv.getDataSize() >= 2)
3115  {
3116  uint8_t biddy = rcv.getDataByte(1);
3117  getDeviceId(biddy, m_lastHwErrorDeviceId);
3118  }
3119  return m_lastResult = m_lastHwError =
3120  (XsensResultValue)rcv.getDataByte();
3121  }
3122  return m_lastResult = XRV_OK;
3123 }
3124 
3125 } // end of xsens namespace
CMT_LEN_RESETORIENTATION
#define CMT_LEN_RESETORIENTATION
Definition: cmtdef.h:492
xsens::Cmt2s::close
XsensResultValue close(void)
Close the serial communication port.
Definition: cmt2.cpp:120
CMT_MID_REQLOCATIONID
#define CMT_MID_REQLOCATIONID
Definition: cmtdef.h:234
CMT_MID_SETSCENARIO
#define CMT_MID_SETSCENARIO
Definition: cmtdef.h:280
CMT_DID_BROADCAST
#define CMT_DID_BROADCAST
Definition: cmtdef.h:928
xsens::Cmt3::openLogFile
XsensResultValue openLogFile(const char *filename)
Open a log file for input.
Definition: cmt3.cpp:2835
xsens::Cmt2f::writeMessage
XsensResultValue writeMessage(const Message *msg)
Write a message to the end of the file.
Definition: cmt2.cpp:868
xsens::msleep
void msleep(uint32_t ms)
A platform-independent sleep routine.
Definition: xsens_time.cpp:82
xsens::Cmt3::m_config
CmtDeviceConfiguration m_config
The configuration of the connected devices.
Definition: cmt3.h:102
xsens::Packet::setDataFormat
bool setDataFormat(const CmtDataFormat &format, const uint16_t index=0)
Definition: cmtpacket.cpp:80
CMT_MID_GOTOCONFIGACK
#define CMT_MID_GOTOCONFIGACK
Definition: cmtdef.h:298
CmtGpsStatus
Definition: cmtdef.h:1283
xsens::Cmt3::getGravityMagnitude
XsensResultValue getGravityMagnitude(double &magnitude, const CmtDeviceId deviceId=CMT_DID_MASTER)
Retrieve the currently used magnitude of the gravity vector.
Definition: cmt3.cpp:3002
CmtUtcTime::m_minute
uint8_t m_minute
Definition: cmtdef.h:995
xsens::Cmt3::getMagneticDeclination
XsensResultValue getMagneticDeclination(double &declination, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return the stored magnetic declination.
Definition: cmt3.cpp:989
xsens::Cmt2s::setTimeout
XsensResultValue setTimeout(const uint32_t ms=CMT2_DEFAULT_TIMEOUT)
Set the default timeout value to use in blocking operations.
Definition: cmt2.cpp:370
xsens::Cmt3::getBatteryLevel
XsensResultValue getBatteryLevel(uint8_t &level)
Get an Xbus Master's battery level.
Definition: cmt3.cpp:420
CmtUtcTime::m_day
uint8_t m_day
Definition: cmtdef.h:993
xsens::Cmt3::readDataPacket
XsensResultValue readDataPacket(Packet *pack, bool acceptOther=false)
Retrieve a data message.
Definition: cmt3.cpp:1712
xsens::Cmt3::getLogFileName
XsensResultValue getLogFileName(char *filename)
Retrieve the name of the open log file or an empty string if no logfile is open.
Definition: cmt3.cpp:964
XRV_NOFILEOPEN
@ XRV_NOFILEOPEN
Definition: xsens_std.h:201
CMT_LEN_GPSLEVERARM
#define CMT_LEN_GPSLEVERARM
Definition: cmtdef.h:294
CmtDeviceMode2
A structure for storing device modes using period and skip factor (new default)
Definition: cmtdef.h:1051
xsens::Cmt3::setScenario
XsensResultValue setScenario(const uint8_t scenarioType, const CmtDeviceId deviceId=CMT_DID_MASTER)
Specify the scenario to use in the sensor.
Definition: cmt3.cpp:2991
CmtSyncInSettings::m_mode
uint16_t m_mode
Definition: cmtdef.h:946
xsens::Cmt2f::setReadPosition
XsensResultValue setReadPosition(CmtFilePos pos)
Set the read position to the given position.
Definition: cmt2.cpp:861
CMT_MID_REQSYNCOUTSETTINGSACK
#define CMT_MID_REQSYNCOUTSETTINGSACK
Definition: cmtdef.h:188
xsens::Cmt3::getDeviceMode
XsensResultValue getDeviceMode(CmtDeviceMode &mode, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return device mode.
Definition: cmt3.cpp:616
xsens::Cmt3::getTimeoutMeasurement
uint32_t getTimeoutMeasurement(void) const
Return the measurement mode timeout.
Definition: cmt3.cpp:1421
CMT_MID_REQAMD
#define CMT_MID_REQAMD
Definition: cmtdef.h:485
xsens::Cmt3::m_eMtsData
void * m_eMtsData[CMT_MAX_DEVICES_PER_PORT]
Cached eMTS data.
Definition: cmt3.h:98
CmtDeviceMode2::getRealSampleFrequency
double getRealSampleFrequency(void) const
Return the real sample frequency in Hz.
Definition: cmt3.cpp:151
xsens::Message::resizeData
void resizeData(const uint16_t newSize)
Resize the data area to the given size.
Definition: cmtmessage.cpp:424
xsens::Cmt3::m_lastResult
XsensResultValue m_lastResult
The last result of an operation.
Definition: cmt3.h:69
CMT_MID_REQGRAVITYMAGNITUDE
#define CMT_MID_REQGRAVITYMAGNITUDE
Definition: cmtdef.h:284
CMT_MID_REQERRORMODE
#define CMT_MID_REQERRORMODE
Definition: cmtdef.h:196
xsens::Message::getDataLong
uint32_t getDataLong(const uint16_t offset=0) const
Return the current value of the data as an uint32_t (32 bits).
Definition: cmtmessage.cpp:322
xsens::Cmt3::getDeviceMode2
XsensResultValue getDeviceMode2(CmtDeviceMode2 &mode, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return device mode2.
Definition: cmt3.cpp:636
xsens::Cmt3::createLogFile
XsensResultValue createLogFile(const char *filename, bool startLogging=false)
Create a log file for incoming messages.
Definition: cmt3.cpp:2717
xsens::Cmt1s::getPortNr
uint8_t getPortNr(void) const
Retrieve the port number that was last successfully opened.
Definition: cmt1.h:143
CMT3_DEFAULT_TIMEOUT_CONF
#define CMT3_DEFAULT_TIMEOUT_CONF
The default timeout value for L3 configuration settings.
Definition: cmtdef.h:811
CMT_DID_TYPEH_MTI_MTX
#define CMT_DID_TYPEH_MTI_MTX
Definition: cmtdef.h:85
CMT_MAX_SVINFO
#define CMT_MAX_SVINFO
Definition: cmtdef.h:1275
xsens::Cmt3::getSyncOutSettings
XsensResultValue getSyncOutSettings(CmtSyncOutSettings &settings)
Retrieve the outbound synchronization settings of the master MT device.
Definition: cmt3.cpp:1260
CMT_BAUD_RATE_921K6
#define CMT_BAUD_RATE_921K6
Definition: cmtdef.h:753
uint16_t
unsigned __int16 uint16_t
Definition: rptypes.h:44
xsens::Cmt3::setBusPowerState
XsensResultValue setBusPowerState(const bool enabled)
Switch the Xbus Master bus power on or off.
Definition: cmt3.cpp:1989
xsens::Cmt3::getBaudrate
XsensResultValue getBaudrate(uint32_t &baudrate)
Get the baudrate used by a port.
Definition: cmt3.cpp:432
CMT_MID_REQOUTPUTMODEACK
#define CMT_MID_REQOUTPUTMODEACK
Definition: cmtdef.h:162
xsens::Cmt3::setSyncInSkipFactor
XsensResultValue setSyncInSkipFactor(const uint16_t skipFactor)
Set the inbound synchronization skip factor of an MT device.
Definition: cmt3.cpp:2320
xsens::Cmt3::getXmOutputMode
XsensResultValue getXmOutputMode(uint8_t &mode)
Return the dual-output mode of the XM.
Definition: cmt3.cpp:1448
CMT_MID_REQBTDISABLE
#define CMT_MID_REQBTDISABLE
Definition: cmtdef.h:129
xsens::Cmt2s::getCmt1s
Cmt1s * getCmt1s(void)
Return a reference to the embedded Cmt1s object.
Definition: cmt2.h:110
xsens::Cmt3::peekLogMessageId
XsensResultValue peekLogMessageId(uint8_t &messageId)
Open a communication channel to.
Definition: cmt3.cpp:1686
CMT_MID_RESET
#define CMT_MID_RESET
Definition: cmtdef.h:472
xsens::Cmt3::setDataSource
XsensResultValue setDataSource(bool readFromFile)
Set whether to read from comm port or file.
Definition: cmt3.cpp:2881
CmtDeviceConfiguration::m_masterDeviceId
uint32_t m_masterDeviceId
Definition: cmtdef.h:867
cmt3.h
XRV_NOFILEORPORTOPEN
@ XRV_NOFILEORPORTOPEN
Definition: xsens_std.h:207
CmtDeviceConfiguration::m_time
uint8_t m_time[8]
Definition: cmtdef.h:874
CmtUtcTime::m_hour
uint8_t m_hour
Definition: cmtdef.h:994
xsens::Cmt3::setGravityMagnitude
XsensResultValue setGravityMagnitude(const double magnitude, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the currently used magnitude of the gravity vector.
Definition: cmt3.cpp:3013
xsens::Message::getDataSize
uint16_t getDataSize(void) const
Return the length of the data part of the message.
Definition: cmtmessage.cpp:352
CMT3EXITLOGDAT
#define CMT3EXITLOGDAT
Definition: cmt3.cpp:55
xsens::Cmt3::getMtCount
uint16_t getMtCount(void) const
Return the device Id of the first.
Definition: cmt3.cpp:1013
CMT_BAUDCODE_9K6
#define CMT_BAUDCODE_9K6
Definition: cmtdef.h:499
CMT_LEN_BUSPWR
#define CMT_LEN_BUSPWR
Definition: cmtdef.h:111
CMT_SYNC_CLOCK_TICKS_TO_NS
#define CMT_SYNC_CLOCK_TICKS_TO_NS
Definition: cmtdef.h:576
xsens::getTimeOfDay
uint32_t getTimeOfDay(tm *date_, time_t *secs_)
A platform-independent clock.
Definition: xsens_time.cpp:34
CMT_BAUDCODE_19K2
#define CMT_BAUDCODE_19K2
Definition: cmtdef.h:501
CmtScenario
A structure for storing scenario information.
Definition: cmtdef.h:1099
XRV_OK
@ XRV_OK
Operation was performed successfully.
Definition: xsens_std.h:34
xsens::Cmt3::m_rtcLastSc
CmtMtTimeStamp m_rtcLastSc
The last received sample counter, used to determine wrap-around.
Definition: cmt3.h:61
xsens_janitors.h
CMT_PARAM_SYNCOUT_MODE
#define CMT_PARAM_SYNCOUT_MODE
Definition: cmtdef.h:595
xsens::Cmt2f::readMessage
XsensResultValue readMessage(Message *msg, const uint8_t msgId=0)
Read the next message from the file, when msgId is non-zero, the first matching message will be retur...
Definition: cmt2.cpp:772
xsens::Cmt2s::waitForMessage
XsensResultValue waitForMessage(Message *rcv, const uint8_t msgId, uint32_t timeoutOverride, bool acceptErrorMessage)
Wait for a message to arrive.
Definition: cmt2.cpp:382
CmtSyncOutSettings::m_offset
uint32_t m_offset
Offset in ns.
Definition: cmtdef.h:968
CMT_LEN_SYNCMODE
#define CMT_LEN_SYNCMODE
Definition: cmtdef.h:148
CMT_LEN_LOCATIONID
#define CMT_LEN_LOCATIONID
Definition: cmtdef.h:236
CMT_MID_REQFILTERSETTINGSACK
#define CMT_MID_REQFILTERSETTINGSACK
Definition: cmtdef.h:481
xsens::Cmt3::setHeading
XsensResultValue setHeading(const double heading, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the heading offset.
Definition: cmt3.cpp:2191
xsens::Message::getDataFloat
float getDataFloat(const uint16_t offset=0) const
Return the current value of the data as a float (32 bits).
Definition: cmtmessage.cpp:180
CmtDeviceConfiguration::m_deviceInfo
struct CmtDeviceConfiguration::_devInfo m_deviceInfo[CMT_MAX_DEVICES_PER_PORT]
xsens::Cmt3::setSyncInMode
XsensResultValue setSyncInMode(const uint16_t mode)
Set the inbound synchronization mode of an MT device.
Definition: cmt3.cpp:2294
xsens::Cmt3::refreshCache
XsensResultValue refreshCache(const bool file=false)
Update device information stored on host PC.
Definition: cmt3.cpp:2594
CMT_BAUD_RATE_19K2
#define CMT_BAUD_RATE_19K2
Definition: cmtdef.h:746
xsens::Cmt3::m_rtcInitialized
bool m_rtcInitialized
Indicates if the rtc is initialised or not.
Definition: cmt3.h:84
xsens::Cmt3::getSyncOutSkipFactor
XsensResultValue getSyncOutSkipFactor(uint16_t &skipFactor)
Retrieve the outbound synchronization skip factor of an MT device.
Definition: cmt3.cpp:1369
CMT_MID_GOTOMEASUREMENT
#define CMT_MID_GOTOMEASUREMENT
Definition: cmtdef.h:123
xsens::Cmt2f
The mid-level file communication class.
Definition: cmt2.h:193
CmtUtcTime::m_year
uint16_t m_year
Definition: cmtdef.h:991
xsens::Cmt3::closeLogFile
XsensResultValue closeLogFile(bool del=false)
Close an open log file.
Definition: cmt3.cpp:2783
CMT_MID_EMTSDATA
#define CMT_MID_EMTSDATA
Definition: cmtdef.h:1296
CmtDeviceMode2::setSampleFrequency
void setSampleFrequency(uint16_t freq)
Compute the period and skip factor from a sample frequency.
Definition: cmt3.cpp:170
CMT_MID_XMPWROFF
#define CMT_MID_XMPWROFF
Definition: cmtdef.h:477
xsens::Cmt2s::getPortNr
XsensResultValue getPortNr(uint8_t &port) const
Retrieve the port that the object is connected to.
Definition: cmt2.cpp:144
xsens::Cmt3::setBaudrate
XsensResultValue setBaudrate(const uint32_t baudrate, bool reconnect=true)
Set the baudrate and possibly reconnect.
Definition: cmt3.cpp:1888
xsens::Packet::setXbus
void setXbus(bool xbus, bool convert=false)
Definition: cmtpacket.cpp:109
CMT_MID_SETGPSLEVERARMACK
#define CMT_MID_SETGPSLEVERARMACK
Definition: cmtdef.h:293
xsens::Cmt3::closePort
XsensResultValue closePort(bool gotoConfigFirst=true)
Close the communication port.
Definition: cmt3.cpp:359
CMT_MID_REQBAUDRATE
#define CMT_MID_REQBAUDRATE
Definition: cmtdef.h:140
CMT_MID_REQAVAILABLESCENARIOS
#define CMT_MID_REQAVAILABLESCENARIOS
Definition: cmtdef.h:272
CMT_LEN_HEADING
#define CMT_LEN_HEADING
Definition: cmtdef.h:224
CmtDeviceMode::getPeriodAndSkipFactor
void getPeriodAndSkipFactor(uint16_t &period, uint16_t &skip) const
Compute the period and skip factor.
Definition: cmt3.cpp:93
XRV_TIMEOUTNODATA
@ XRV_TIMEOUTNODATA
Definition: xsens_std.h:115
CMT3LOGDAT
#define CMT3LOGDAT(...)
Definition: cmt3.cpp:54
CMT3_DEFAULT_TIMEOUT_MEAS
#define CMT3_DEFAULT_TIMEOUT_MEAS
The default timeout value for L3 data reading.
Definition: cmtdef.h:809
XSENS_MS_PER_DAY
#define XSENS_MS_PER_DAY
The number of milliseconds in a normal day.
Definition: xsens_time.h:21
CmtDeviceMode::operator==
bool operator==(const CmtDeviceMode &dev) const
Check if all fields of the two structures are equal.
Definition: cmt3.cpp:142
CMT_MID_BUSPWR
#define CMT_MID_BUSPWR
Definition: cmtdef.h:110
xsens::Cmt3::getUtcTime
XsensResultValue getUtcTime(CmtUtcTime &utc, const CmtDeviceId deviceId)
Return the UTC time of the last received sample.
Definition: cmt3.cpp:1424
xsens::Packet::m_toa
TimeStamp m_toa
Time of arrival.
Definition: cmtpacket.h:82
xsens::Cmt3::setXmOutputMode
XsensResultValue setXmOutputMode(const uint8_t mode)
Set the dual-output mode of the XM.
Definition: cmt3.cpp:2581
XRV_NOPORTOPEN
@ XRV_NOPORTOPEN
Definition: xsens_std.h:204
xsens::Cmt3::getSyncOutOffset
XsensResultValue getSyncOutOffset(uint32_t &offset)
Retrieve the outbound synchronization offset of an MT device.
Definition: cmt3.cpp:1393
CMT_MID_REQPERIOD
#define CMT_MID_REQPERIOD
Definition: cmtdef.h:100
CmtVector::m_data
double m_data[3]
Definition: cmtdef.h:1251
xsens::TimeStamp
uint64_t TimeStamp
A real-time timestamp (ms)
Definition: xsens_time.h:24
xsens::Cmt3::getSyncOutPulseWidth
XsensResultValue getSyncOutPulseWidth(uint32_t &pulseWidth)
Retrieve the outbound synchronization pulse width of an MT device.
Definition: cmt3.cpp:1341
CMT_PARAM_SYNCOUT_SKIPFACTOR
#define CMT_PARAM_SYNCOUT_SKIPFACTOR
Definition: cmtdef.h:596
CMT_LEN_AMD
#define CMT_LEN_AMD
Definition: cmtdef.h:487
xsens::Cmt2s::open
XsensResultValue open(const char *portName, const uint32_t baudRate=CMT_DEFAULT_BAUD_RATE)
Open a communication channel to the given serial port name.
Definition: cmt2.cpp:162
CmtVersion::m_revision
uint8_t m_revision
Definition: cmtdef.h:940
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
CmtSyncOutSettings::m_skipFactor
uint16_t m_skipFactor
Definition: cmtdef.h:966
CmtDeviceConfiguration::m_reservedForHost
uint8_t m_reservedForHost[32]
Definition: cmtdef.h:875
xsens::Cmt3::getBusId
XsensResultValue getBusId(uint8_t &busId, const CmtDeviceId deviceId=CMT_DID_MASTER) const
Retrieve the BusId of a device.
Definition: cmt3.cpp:457
CMT_MID_REQSYNCMODE
#define CMT_MID_REQSYNCMODE
Definition: cmtdef.h:146
CMT_LEN_BAUDRATE
#define CMT_LEN_BAUDRATE
Definition: cmtdef.h:142
xsens::Cmt3::setSyncOutOffset
XsensResultValue setSyncOutOffset(const uint32_t offset)
Set the outbound synchronization offset of an MT device.
Definition: cmt3.cpp:2530
CMT3LOG
#define CMT3LOG(...)
Definition: cmt3.cpp:44
CMT_LEN_BTDISABLE
#define CMT_LEN_BTDISABLE
Definition: cmtdef.h:131
xsens::Cmt3::getSerialBaudrate
XsensResultValue getSerialBaudrate(uint32_t &baudrate)
Return the baud rate used for serial communication.
Definition: cmt3.cpp:1078
CMT_MID_REQPRODUCTCODE
#define CMT_MID_REQPRODUCTCODE
Definition: cmtdef.h:152
xsens::Cmt3::storeXkfState
XsensResultValue storeXkfState(const CmtDeviceId deviceId=CMT_DID_MASTER)
Store important components of the XKF filter state to non-volatile memory.
Definition: cmt3.cpp:3093
level
GLint level
Definition: glext.h:3600
CmtDeviceMode::m_outputMode
CmtOutputMode m_outputMode
Definition: cmtdef.h:1006
CmtSyncInSettings
A structure for storing sync in settings.
Definition: cmtdef.h:944
xsens::Cmt3::getProductCode
XsensResultValue getProductCode(char *productCode, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return product code.
Definition: cmt3.cpp:1049
CmtUtcTime
A structure for storing UTC Time values.
Definition: cmtdef.h:988
CmtDeviceConfiguration::m_syncinSkipFactor
uint16_t m_syncinSkipFactor
Definition: cmtdef.h:871
xsens::Cmt3::resetLogFileReadPos
XsensResultValue resetLogFileReadPos(void)
Restart reading from the start of the open log file.
Definition: cmt3.cpp:2924
xsens::Cmt2f::isOpen
bool isOpen(void) const
Return whether the file is open or not.
Definition: cmt2.cpp:749
xsens::Cmt3::getMtDeviceId
XsensResultValue getMtDeviceId(const uint8_t index, CmtDeviceId &deviceId) const
Definition: cmt3.cpp:1024
CMT_MID_REQHEADING
#define CMT_MID_REQHEADING
Definition: cmtdef.h:222
CmtScenario::m_filterType
char m_filterType
The type of filter this scenario is intended for '3': XKF-3, '6': XKF-6.
Definition: cmtdef.h:1115
xsens::Cmt3::m_logging
bool m_logging
Indicates whether to write all received messages to the logfile or not, automatically set to true by ...
Definition: cmt3.h:87
XRV_INVALIDOPERATION
@ XRV_INVALIDOPERATION
Definition: xsens_std.h:133
CMT_MID_REQBATLEVEL
#define CMT_MID_REQBATLEVEL
Definition: cmtdef.h:247
CmtVector
Definition: cmtdef.h:1249
length
GLuint GLsizei GLsizei * length
Definition: glext.h:4064
xsens::Cmt3::waitForDataMessage
XsensResultValue waitForDataMessage(Packet *pack)
Wait for a data message to arrive.
Definition: cmt3.cpp:2674
CmtSyncInSettings::m_skipFactor
uint16_t m_skipFactor
Definition: cmtdef.h:947
xsens::Cmt3::fillRtc
void fillRtc(Packet *pack)
Internal function to compute the RTC value.
Definition: cmt3.cpp:377
CmtVersion::m_major
uint8_t m_major
Definition: cmtdef.h:938
xsens::Cmt3::Cmt3
Cmt3()
Default constructor, initializes all members to their default values.
Definition: cmt3.cpp:325
CMT_LEN_STOREXKFSTATE
#define CMT_LEN_STOREXKFSTATE
Definition: cmtdef.h:263
xsens::Cmt3::initBus
XsensResultValue initBus(void)
Perform an initBus request.
Definition: cmt3.cpp:1545
CMT_MID_REQMAGNETICDECLINATION
#define CMT_MID_REQMAGNETICDECLINATION
Definition: cmtdef.h:228
xsens::Packet::m_rtc
TimeStamp m_rtc
Sample time in ms, based on the sample counter.
Definition: cmtpacket.h:80
CMT_MID_REQSYNCINSETTINGSACK
#define CMT_MID_REQSYNCINSETTINGSACK
Definition: cmtdef.h:180
CmtUtcTime::m_nano
uint32_t m_nano
Definition: cmtdef.h:990
CmtResetMethod
CmtResetMethod
Definition: cmtdef.h:835
xsens::Message::getDataShort
uint16_t getDataShort(const uint16_t offset=0) const
Return the current value of the data as an uint16_t (16 bits).
Definition: cmtmessage.cpp:338
XRV_BAUDRATEINVALID
@ XRV_BAUDRATEINVALID
Definition: xsens_std.h:89
xsens::Cmt2s::getLastResult
XsensResultValue getLastResult(void) const
Return the error code of the last operation.
Definition: cmt2.h:112
CmtSyncOutSettings::m_mode
uint16_t m_mode
Definition: cmtdef.h:965
xsens::Message::setDataFloat
void setDataFloat(const float data, const uint16_t offset=0)
Write a float (32 bits) into the data buffer.
Definition: cmtmessage.cpp:559
xsens::Cmt3::getLogFileReadPosition
XsensResultValue getLogFileReadPosition(CmtFilePos &pos)
Retrieve the read position of the log file.
Definition: cmt3.cpp:931
xsens::Cmt3::setSyncMode
XsensResultValue setSyncMode(const uint8_t mode)
Set the sync mode of the Xbus Master.
Definition: cmt3.cpp:2375
xsens::Cmt3::getSyncOutMode
XsensResultValue getSyncOutMode(uint16_t &mode)
Retrieve the outbound synchronization mode of an MT device.
Definition: cmt3.cpp:1317
xsens::Cmt3::getScenario
XsensResultValue getScenario(uint8_t &scenarioType, uint8_t &scenarioVersion, const CmtDeviceId deviceId=CMT_DID_MASTER)
Get the currently active scenario from a Motion Tracker.
Definition: cmt3.cpp:2979
xsens::Packet::m_itemCount
uint16_t m_itemCount
The number of data items in the message.
Definition: cmtpacket.h:76
CMT_LEN_SCENARIOLABEL
#define CMT_LEN_SCENARIOLABEL
Definition: cmtdef.h:270
CmtDeviceConfiguration::readFromMessage
void readFromMessage(const void *message)
Definition: cmt3.cpp:65
CmtSyncOutSettings::m_pulseWidth
uint32_t m_pulseWidth
Pulse width in ns.
Definition: cmtdef.h:970
xsens::Cmt3::m_baudrate
uint32_t m_baudrate
The baudrate that was last set to be used by the port.
Definition: cmt3.h:63
Byte
unsigned char Byte
Definition: zconf.h:266
CMT_BAUDCODE_38K4
#define CMT_BAUDCODE_38K4
Definition: cmtdef.h:503
xsens::Message::recomputeChecksum
void recomputeChecksum(void)
Compute the checksum field and fill it.
Definition: cmtmessage.h:270
CmtDeviceMode2::m_outputSettings
CmtOutputSettings m_outputSettings
Definition: cmtdef.h:1054
xsens::Cmt2f::close
XsensResultValue close(void)
Close the file.
Definition: cmt2.cpp:667
CmtDeviceConfiguration::m_date
uint8_t m_date[8]
Definition: cmtdef.h:873
CMT_BID_BROADCAST
#define CMT_BID_BROADCAST
Definition: cmtdef.h:1293
CmtSyncInSettings::m_offset
uint32_t m_offset
Offset in ns.
Definition: cmtdef.h:949
CMT_OUTPUTSETTINGS_TIMESTAMP_MASK
#define CMT_OUTPUTSETTINGS_TIMESTAMP_MASK
Definition: cmtdef.h:647
offset
GLintptr offset
Definition: glext.h:3925
xsens::Cmt3::getCmt2f
Cmt2f * getCmt2f(void)
Return a reference to the embedded Cmt2f (logfile) object.
Definition: cmt3.cpp:510
xsens::Cmt2f::open
XsensResultValue open(const char *filename, const bool readOnly=false)
Open a file and read the header.
Definition: cmt2.cpp:752
CMT_BAUDCODE_115K2
#define CMT_BAUDCODE_115K2
Definition: cmtdef.h:506
xsens::Cmt3::m_skip
uint16_t m_skip
The skip factor of the port.
Definition: cmt3.h:55
XRV_INVALIDID
@ XRV_INVALIDID
Definition: xsens_std.h:130
CmtDeviceMode
A structure for storing device modes.
Definition: cmtdef.h:1004
CMT_DEFAULT_PERIOD
#define CMT_DEFAULT_PERIOD
Definition: cmtdef.h:776
CMT_DID_TYPEH_MASK
#define CMT_DID_TYPEH_MASK
Definition: cmtdef.h:82
xsens::Cmt3::getSyncInMode
XsensResultValue getSyncInMode(uint16_t &mode)
Retrieve the inbound synchronization mode of an MT device.
Definition: cmt3.cpp:1173
xsens::Cmt3::setGotoConfigTries
XsensResultValue setGotoConfigTries(const uint16_t tries)
Set the number of times the gotoConfig function will attempt a gotoConfig before failing.
Definition: cmt3.cpp:2180
xsens::Cmt2s::getPortName
XsensResultValue getPortName(char *portname) const
Definition: cmt2.cpp:134
xsens::Cmt3::reset
XsensResultValue reset(void)
Request a data.
Definition: cmt3.cpp:1829
xsens::Message::setDataLong
void setDataLong(const uint32_t data, const uint16_t offset=0)
Write an uint32_t (32 bits) into the data buffer.
Definition: cmtmessage.cpp:690
xsens::Cmt3::m_rtcStart
TimeStamp m_rtcStart
The start of the RTC counter, the time of arrival of sample 0.
Definition: cmt3.h:57
CMT_MID_ERROR
#define CMT_MID_ERROR
Definition: cmtdef.h:474
xsens::Cmt3::getErrorMode
XsensResultValue getErrorMode(uint16_t &mode, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return the error mode of the device.
Definition: cmt3.cpp:819
CMT_MID_REQOUTPUTSETTINGS
#define CMT_MID_REQOUTPUTSETTINGS
Definition: cmtdef.h:167
xsens::Cmt3::setSyncOutSettings
XsensResultValue setSyncOutSettings(const CmtSyncOutSettings &settings)
Set the outbound synchronization settings of an MT device.
Definition: cmt3.cpp:2389
CMT_MID_REQOUTPUTMODE
#define CMT_MID_REQOUTPUTMODE
Definition: cmtdef.h:161
xsens::Cmt3::m_useRtc
bool m_useRtc
Indicates if the RTC should be computed or not (to save CPU time).
Definition: cmt3.h:96
CMT_MID_STOREXKFSTATE
#define CMT_MID_STOREXKFSTATE
Definition: cmtdef.h:262
CMT_MID_REQSYNCOUTSETTINGS
#define CMT_MID_REQSYNCOUTSETTINGS
Definition: cmtdef.h:187
xsens::Cmt3::setSyncOutPulseWidth
XsensResultValue setSyncOutPulseWidth(const uint32_t pulseWidth)
Set the outbound synchronization pulse width of an MT device.
Definition: cmt3.cpp:2477
CMT_LEN_ERRORMODE
#define CMT_LEN_ERRORMODE
Definition: cmtdef.h:198
CMT_PARAM_SYNCOUT_PULSEWIDTH
#define CMT_PARAM_SYNCOUT_PULSEWIDTH
Definition: cmtdef.h:598
CMT_PARAM_SYNCOUT_OFFSET
#define CMT_PARAM_SYNCOUT_OFFSET
Definition: cmtdef.h:597
CMT_LEN_GRAVITYMAGNITUDE
#define CMT_LEN_GRAVITYMAGNITUDE
Definition: cmtdef.h:288
CmtScenario::m_label
char m_label[CMT_LEN_SCENARIOLABEL+1]
The label of the scenario.
Definition: cmtdef.h:1107
xsensResultText
const char * xsensResultText(const XsensResultValue result)
Definition: xsens_std.cpp:13
CMT_MAX_FILENAME_LENGTH
#define CMT_MAX_FILENAME_LENGTH
Definition: cmtdef.h:926
CMT_MID_RESTOREFACTORYDEF
#define CMT_MID_RESTOREFACTORYDEF
Definition: cmtdef.h:120
CMT_MID_INITBUS
#define CMT_MID_INITBUS
Definition: cmtdef.h:97
xsens::Cmt2f::closeAndDelete
XsensResultValue closeAndDelete(void)
Close the file and delete it.
Definition: cmt2.cpp:680
xsens::Cmt3::writeMessageToLogFile
XsensResultValue writeMessageToLogFile(const Message &msg)
Definition: cmt3.cpp:2933
xsens::Cmt3::gotoMeasurement
XsensResultValue gotoMeasurement(void)
Place all connected devices into Measurement Mode.
Definition: cmt3.cpp:1520
xsens::Cmt3::m_lastHwError
XsensResultValue m_lastHwError
Contains the last error reported by hardware.
Definition: cmt3.h:89
xsens::Cmt3::getLogFileSize
XsensResultValue getLogFileSize(CmtFilePos &size)
Retrieve the size of the open log file in bytes.
Definition: cmt3.cpp:947
xsens::Cmt3::m_serial
Cmt2s m_serial
The (optional) CMT level 2 serial object that this class operates on.
Definition: cmt3.h:44
CmtDeviceConfiguration::m_outputSkipFactor
uint16_t m_outputSkipFactor
Definition: cmtdef.h:869
CMT_MID_REQSYNCINSETTINGS
#define CMT_MID_REQSYNCINSETTINGS
Definition: cmtdef.h:179
CMT_MID_REQGPSSTATUS
#define CMT_MID_REQGPSSTATUS
Definition: cmtdef.h:494
CMT_EMTS_SIZE
#define CMT_EMTS_SIZE
Definition: cmtdef.h:1297
xsens::Cmt3::getBusPowerState
XsensResultValue getBusPowerState(bool &enabled)
Get the state of the Xbus power.
Definition: cmt3.cpp:497
CMT_LEN_OPMODE
#define CMT_LEN_OPMODE
Definition: cmtdef.h:136
xsens::Cmt3::getConfiguration
XsensResultValue getConfiguration(CmtDeviceConfiguration &configuration)
Get device configuration.
Definition: cmt3.cpp:515
DO_DATA_REQUEST
#define DO_DATA_REQUEST(req)
Definition: cmt3.cpp:246
CMT_MID_REQXMERRORMODE
#define CMT_MID_REQXMERRORMODE
Definition: cmtdef.h:209
CMT_BAUD_RATE_9600
#define CMT_BAUD_RATE_9600
Definition: cmtdef.h:744
xsens::Message::getDataBuffer
uint8_t * getDataBuffer(const uint16_t offset=0)
Return a pointer to the data buffer.
Definition: cmtmessage.h:157
xsens::Cmt3::setBluetoothState
XsensResultValue setBluetoothState(const bool enabled)
Set the Bluetooth state of the Xbus Master.
Definition: cmt3.cpp:1976
xsens::Cmt3::getDataLength
XsensResultValue getDataLength(uint32_t &length, const CmtDeviceId deviceId=CMT_DID_MASTER)
Retrieve data size.
Definition: cmt3.cpp:569
CmtDeviceMode2::m_outputMode
CmtOutputMode m_outputMode
Definition: cmtdef.h:1053
xsens::Cmt3::m_lastHwErrorDeviceId
CmtDeviceId m_lastHwErrorDeviceId
Contains the Device ID of the device that caused the last hardware error.
Definition: cmt3.h:92
xsens::Message::getMessageId
uint8_t getMessageId(void) const
Return the current value of the m_messageId field.
Definition: cmtmessage.h:236
index
GLuint index
Definition: glext.h:4054
CMT_MID_RESETACK
#define CMT_MID_RESETACK
Definition: cmtdef.h:473
CMT_BAUD_RATE_38K4
#define CMT_BAUD_RATE_38K4
Definition: cmtdef.h:748
xsens::Cmt3::m_timeoutMeas
uint32_t m_timeoutMeas
The measurement mode timeout.
Definition: cmt3.h:67
xsens::Cmt1s::flushData
XsensResultValue flushData(void)
Flush all data to be transmitted / received.
Definition: cmt1.cpp:308
CmtSyncOutSettings
A structure for storing sync out settings.
Definition: cmtdef.h:963
CmtDeviceMode::getRealSampleFrequency
double getRealSampleFrequency(void) const
Return the real sample frequency in Hz.
Definition: cmt3.cpp:125
CmtScenario::m_version
uint8_t m_version
The version of the scenario.
Definition: cmtdef.h:1105
xsens::Cmt3::getAvailableScenarios
XsensResultValue getAvailableScenarios(CmtScenario *scenarios, const CmtDeviceId deviceId=CMT_DID_MASTER)
Write the specified message to the open.
Definition: cmt3.cpp:2944
xsens::Message::getMessageStart
const uint8_t * getMessageStart(void) const
Return the start of the message buffer.
Definition: cmtmessage.h:241
CmtDeviceConfiguration::_devInfo::m_outputSettings
uint32_t m_outputSettings
Definition: cmtdef.h:883
CmtDeviceMode::setPeriodAndSkipFactor
void setPeriodAndSkipFactor(uint16_t period, uint16_t skip)
Compute the sample frequency from a period and skip factor.
Definition: cmt3.cpp:134
CmtUtcTime::m_second
uint8_t m_second
Definition: cmtdef.h:996
XRV_NOTFOUND
@ XRV_NOTFOUND
Definition: xsens_std.h:124
xsens::Cmt3::setMagneticDeclination
XsensResultValue setMagneticDeclination(const double declination, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the stored magnetic declination.
Definition: cmt3.cpp:2216
CMT_PARAM_SYNCIN_MODE
#define CMT_PARAM_SYNCIN_MODE
Definition: cmtdef.h:579
xsens::Cmt2s::isOpen
bool isOpen(void) const
Return whether the communication port is open or not.
Definition: cmt2.h:120
xsens::Cmt3::resetOrientation
XsensResultValue resetOrientation(const CmtResetMethod method, const CmtDeviceId deviceId=CMT_DID_MASTER)
Perform an orientation reset on a device.
Definition: cmt3.cpp:1848
CMT_MID_MTDATA
#define CMT_MID_MTDATA
Definition: cmtdef.h:300
CMT_LEN_SETSCENARIO
#define CMT_LEN_SETSCENARIO
Definition: cmtdef.h:282
CMT_MID_REQEMTS
#define CMT_MID_REQEMTS
Definition: cmtdef.h:1295
xsens::Cmt2f::create
XsensResultValue create(const char *filename)
Create a new file with level 2 header.
Definition: cmt2.cpp:692
CMT_MID_REQUTCTIME
#define CMT_MID_REQUTCTIME
Definition: cmtdef.h:266
CmtVersion::m_minor
uint8_t m_minor
Definition: cmtdef.h:939
xsens::Cmt2f::getFileSize
CmtFilePos getFileSize(void)
Get the current file size.
Definition: cmt2.cpp:855
xsens::Cmt3::m_gotoConfigTries
uint16_t m_gotoConfigTries
The number of times a goto config is attempted before the function fails.
Definition: cmt3.h:74
DO_DATA_SET_BID
#define DO_DATA_SET_BID(req, size, type, data, bid)
Definition: cmt3.cpp:252
xsens::Cmt3::setGpsLeverArm
XsensResultValue setGpsLeverArm(const CmtVector &arm, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the currently used GPS lever arm.
Definition: cmt3.cpp:3055
xsens::Cmt3::m_rtcMsPerSample
double m_rtcMsPerSample
ms per sample = 1000 / sample frequency.
Definition: cmt3.h:49
CMT_BAUDCODE_460K8
#define CMT_BAUDCODE_460K8
Definition: cmtdef.h:508
xsens::Cmt3::setTimeoutMeasurement
XsensResultValue setTimeoutMeasurement(const uint32_t timeout=CMT3_DEFAULT_TIMEOUT_MEAS)
Set the measurement mode timeout.
Definition: cmt3.cpp:2569
status
_u8 status
Definition: rplidar_cmd.h:2
xsens::Cmt2f::getName
XsensResultValue getName(char *filename) const
Retrieve the filename that was last successfully opened.
Definition: cmt2.cpp:735
xsens::Cmt3::getSyncMode
XsensResultValue getSyncMode(uint8_t &mode)
Retrieve the sync mode of the Xbus Master.
Definition: cmt3.cpp:1248
CMT_MID_REQSCENARIO
#define CMT_MID_REQSCENARIO
Definition: cmtdef.h:277
CMT_MID_STOREXKFSTATEACK
#define CMT_MID_STOREXKFSTATEACK
Definition: cmtdef.h:264
xsens::Cmt3::getBluetoothState
XsensResultValue getBluetoothState(bool &enabled)
Get the state of the bluetooth communication.
Definition: cmt3.cpp:444
len
GLenum GLsizei len
Definition: glext.h:4712
CMT_MID_REQCONFIGURATION
#define CMT_MID_REQCONFIGURATION
Definition: cmtdef.h:117
CmtDeviceMode::m_outputSettings
CmtOutputSettings m_outputSettings
Definition: cmtdef.h:1007
CmtDeviceConfiguration
Structure containing a full device configuration as returned by the ReqConfig message.
Definition: cmtdef.h:865
CMT_MID_SETGRAVITYMAGNITUDE
#define CMT_MID_SETGRAVITYMAGNITUDE
Definition: cmtdef.h:286
CMT_MID_SETMAGNETICDECLINATION
#define CMT_MID_SETMAGNETICDECLINATION
Definition: cmtdef.h:231
CmtDeviceConfiguration::m_syncinMode
uint16_t m_syncinMode
Definition: cmtdef.h:870
xsens::Cmt3::gotoConfig
XsensResultValue gotoConfig(void)
Place all connected devices into Configuration Mode.
Definition: cmt3.cpp:1460
CMT_BAUD_RATE_460K8
#define CMT_BAUD_RATE_460K8
Definition: cmtdef.h:752
CMT_DID_MASTER
#define CMT_DID_MASTER
Definition: cmtdef.h:929
CMT_BAUD_RATE_57K6
#define CMT_BAUD_RATE_57K6
Definition: cmtdef.h:749
CMT_BAUDCODE_230K4
#define CMT_BAUDCODE_230K4
Definition: cmtdef.h:507
xsens::Cmt3::restoreFactoryDefaults
XsensResultValue restoreFactoryDefaults(const CmtDeviceId deviceId=CMT_DID_MASTER)
Restore the device to factory default settings.
Definition: cmt3.cpp:1862
DO_DATA_REQUEST_BID
#define DO_DATA_REQUEST_BID(req, bid)
Definition: cmt3.cpp:210
xsens::Cmt3::setSyncInSettings
XsensResultValue setSyncInSettings(const CmtSyncInSettings &settings)
Set the inbound synchronization settings of an MT device.
Definition: cmt3.cpp:2243
buffer
GLuint buffer
Definition: glext.h:3917
CmtDeviceMode::m_sampleFrequency
uint16_t m_sampleFrequency
Definition: cmtdef.h:1008
CMT_MID_GOTOMEASUREMENTACK
#define CMT_MID_GOTOMEASUREMENTACK
Definition: cmtdef.h:124
CmtDeviceMode2::m_skip
uint16_t m_skip
Definition: cmtdef.h:1056
CMT_MID_RESETORIENTATION
#define CMT_MID_RESETORIENTATION
Definition: cmtdef.h:490
xsens::Cmt3::setLocationId
XsensResultValue setLocationId(uint16_t locationId, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the location ID of a sensor.
Definition: cmt3.cpp:2203
XRV_CONFIGCHECKFAIL
@ XRV_CONFIGCHECKFAIL
Definition: xsens_std.h:220
CmtScenario::m_type
uint8_t m_type
The type of the scenario.
Definition: cmtdef.h:1103
int32_t
__int32 int32_t
Definition: rptypes.h:46
xsens::Cmt3::getSyncInSkipFactor
XsensResultValue getSyncInSkipFactor(uint16_t &skipFactor)
Retrieve the inbound synchronization skip factor of an MT device.
Definition: cmt3.cpp:1197
CmtDeviceMode2::getSampleFrequency
uint16_t getSampleFrequency(void) const
Return the sample frequency in Hz.
Definition: cmt3.cpp:161
xsens::Message::getDataByte
uint8_t getDataByte(const uint16_t offset=0) const
Return the current value of the data as an unsigned byte (8 bits).
Definition: cmtmessage.h:171
CMT_BAUD_RATE_115K2
#define CMT_BAUD_RATE_115K2
Definition: cmtdef.h:750
xsens::Cmt3::getDeviceId
XsensResultValue getDeviceId(const uint8_t busId, CmtDeviceId &deviceId) const
Retrieve the DeviceId of a device given its Bus ID.
Definition: cmt3.cpp:597
xsens::Packet::getSampleCounter
uint16_t getSampleCounter(const uint16_t index=0) const
Return the Sample Counter component of the packet.
Definition: cmtpacket.cpp:1217
xsens::Cmt3::getSampleFrequency
uint16_t getSampleFrequency(void)
Return current sample frequency.
Definition: cmt3.cpp:1066
CMT_DID_TYPEH_MTIG
#define CMT_DID_TYPEH_MTIG
Definition: cmtdef.h:86
xsens::Cmt3::getBusIdInternal
uint8_t getBusIdInternal(const CmtDeviceId devId) const
Find a device Id in the list and return its busId.
Definition: cmt3.cpp:479
xsens::Packet
A structure containing MT data + timestamp and formatting information.
Definition: cmtpacket.h:24
CmtVersion
A structure for storing the firmware version.
Definition: cmtdef.h:936
CMT_PARAM_SYNCIN_SKIPFACTOR
#define CMT_PARAM_SYNCIN_SKIPFACTOR
Definition: cmtdef.h:580
CMT_MID_CONFIGURATION
#define CMT_MID_CONFIGURATION
Definition: cmtdef.h:118
xsens::Cmt3::getCmt2s
Cmt2s * getCmt2s(void)
Return a reference to the embedded Cmt2s (comm port) object.
Definition: cmt3.cpp:512
xsens::Cmt3::m_rtcCount
uint32_t m_rtcCount
The long sample counter (normal counter wraps at 64k).
Definition: cmt3.h:59
CMT_MID_REQGPSLEVERARM
#define CMT_MID_REQGPSLEVERARM
Definition: cmtdef.h:290
xsens::Cmt3::setDeviceMode
XsensResultValue setDeviceMode(const CmtDeviceMode &mode, bool force, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the complete output mode of a device.
Definition: cmt3.cpp:2001
CMT_MID_REQDATALENGTH
#define CMT_MID_REQDATALENGTH
Definition: cmtdef.h:114
CmtUtcTime::m_month
uint8_t m_month
Definition: cmtdef.h:992
xsens::Cmt2s::readMessage
XsensResultValue readMessage(Message *rcv)
Read a message from the COM port.
Definition: cmt2.cpp:192
xsens::Cmt3::isLogFileOpen
bool isLogFileOpen(const char *filename) const
Return whether or not(true or false) the supplied file is open.
Definition: cmt3.cpp:2797
xsens::Cmt3::getDeviceCount
uint32_t getDeviceCount(void) const
Retrieve total device count.
Definition: cmt3.cpp:582
xsens::Cmt3::clearHwError
void clearHwError(void)
Reset the hardware error code.
Definition: cmt3.h:248
CmtDeviceId
uint32_t CmtDeviceId
The type of a Device Id.
Definition: cmtdef.h:854
xsens::Cmt3::m_timeoutConf
uint32_t m_timeoutConf
The config mode timeout.
Definition: cmt3.h:65
CMT_MID_REQOUTPUTSKIPFACTORACK
#define CMT_MID_REQOUTPUTSKIPFACTORACK
Definition: cmtdef.h:174
xsens::Cmt3::setErrorMode
XsensResultValue setErrorMode(const uint16_t mode)
Set the error mode of the device.
Definition: cmt3.cpp:2162
xsens::Cmt3::m_readFromFile
bool m_readFromFile
Indicates whether to read from the log file or from the serial port.
Definition: cmt3.h:82
CmtUtcTime::m_valid
uint8_t m_valid
When set to 1, the time is valid, when set to 2, the time part is valid, but the date may not be vali...
Definition: cmtdef.h:1000
CmtDeviceConfiguration::m_samplingPeriod
uint16_t m_samplingPeriod
Definition: cmtdef.h:868
CMT_DID_TYPEH_XM
#define CMT_DID_TYPEH_XM
Definition: cmtdef.h:84
mode
GLint mode
Definition: glext.h:5669
CmtDeviceMode2::operator==
bool operator==(const CmtDeviceMode2 &dev) const
Check if all fields of the two structures are equal.
Definition: cmt3.cpp:201
CmtDeviceConfiguration::m_reservedForClient
uint8_t m_reservedForClient[32]
Definition: cmtdef.h:876
xsens::Cmt3::getFirmwareRevision
XsensResultValue getFirmwareRevision(CmtVersion &revision, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return Firmware revision.
Definition: cmt3.cpp:889
XRV_INVALIDPARAM
@ XRV_INVALIDPARAM
Definition: xsens_std.h:95
xsens::Cmt3::setXmPowerOff
XsensResultValue setXmPowerOff(void)
Switch the connected Xbus Master.
Definition: cmt3.cpp:2230
xsens::Cmt3::openPort
XsensResultValue openPort(const char *portName, const uint32_t baudRate=CMT_DEFAULT_BAUD_RATE)
Definition: cmt3.cpp:1566
CMT3_CONFIG_TIMEOUT
#define CMT3_CONFIG_TIMEOUT
The timeout value for "goto config"-message acknowledgement.
Definition: cmtdef.h:803
xsens::Cmt2f::getReadPosition
CmtFilePos getReadPosition(void)
Get the current read position.
Definition: cmt2.cpp:858
CMT3EXITLOG
#define CMT3EXITLOG
Definition: cmt3.cpp:45
xsens::Cmt2s::writeMessage
XsensResultValue writeMessage(Message *msg)
Send a message over the COM port.
Definition: cmt2.cpp:599
xsens::Cmt3::isXm
bool isXm(void) const
Return whether the main device is an Xbus Master or not.
Definition: cmt3.cpp:1558
xsens::Message::setDataBuffer
void setDataBuffer(const uint8_t *data, const uint16_t offset=0, const uint16_t count=0)
Write a string of bytes into the data buffer.
Definition: cmtmessage.cpp:505
xsens::Cmt3::m_measuring
bool m_measuring
Keeps track of whether the connected device is measuring or being configured.
Definition: cmt3.h:77
xsens::Cmt3::requestData
XsensResultValue requestData(Packet *pack)
Definition: cmt3.cpp:1816
mrpt::system::os::_strnicmp
int _strnicmp(const char *str, const char *subStr, size_t count) noexcept
An OS-independent version of strnicmp.
Definition: os.cpp:344
xsens::Cmt3::setTimeoutConfig
XsensResultValue setTimeoutConfig(const uint32_t timeout=CMT3_DEFAULT_TIMEOUT_CONF)
Set the configuration mode timeout.
Definition: cmt3.cpp:2557
xsens::Cmt3::~Cmt3
~Cmt3()
Destructor, de-initializes, frees memory allocated for buffers, etc.
Definition: cmt3.cpp:347
CmtDeviceConfiguration::_devInfo::m_dataLength
uint16_t m_dataLength
Definition: cmtdef.h:881
CMT_MID_SETGPSLEVERARM
#define CMT_MID_SETGPSLEVERARM
Definition: cmtdef.h:292
xsens::Message
Class for storing a single message.
Definition: cmtmessage.h:83
CmtDeviceConfiguration::_devInfo::m_deviceId
uint32_t m_deviceId
Definition: cmtdef.h:880
CMT_MAX_SCENARIOS_IN_MT
#define CMT_MAX_SCENARIOS_IN_MT
Definition: cmtdef.h:1095
DO_DATA_SET
#define DO_DATA_SET(req, size, type, data)
Definition: cmt3.cpp:298
xsens::Cmt3::getSyncInOffset
XsensResultValue getSyncInOffset(uint32_t &offset)
Retrieve the inbound synchronization offset of an MT device.
Definition: cmt3.cpp:1221
CMT_MID_REQPERIODACK
#define CMT_MID_REQPERIODACK
Definition: cmtdef.h:101
CmtFilePos
__int64 CmtFilePos
Definition: cmtf.h:25
CMT_BAUD_RATE_230K4
#define CMT_BAUD_RATE_230K4
Definition: cmtdef.h:751
CMT_DEFAULT_SKIP
#define CMT_DEFAULT_SKIP
Definition: cmtdef.h:777
xsens::Cmt3::getSyncInSettings
XsensResultValue getSyncInSettings(CmtSyncInSettings &settings)
Retrieve the inbound synchronization settings of the master MT device.
Definition: cmt3.cpp:1127
CMT_MID_REQOUTPUTSETTINGSACK
#define CMT_MID_REQOUTPUTSETTINGSACK
Definition: cmtdef.h:168
CMT_MAX_DEVICES_PER_PORT
#define CMT_MAX_DEVICES_PER_PORT
Definition: cmtdef.h:774
xsens::Cmt3::setDeviceMode2
XsensResultValue setDeviceMode2(const CmtDeviceMode2 &mode, bool force, const CmtDeviceId deviceId=CMT_DID_MASTER)
Set the complete output mode2 of a device.
Definition: cmt3.cpp:2020
HANDLE_ERR_RESULT
#define HANDLE_ERR_RESULT
Definition: cmt3.cpp:303
xsens::Cmt3::getTimeoutConfig
uint32_t getTimeoutConfig(void) const
Return the configuration mode timeout.
Definition: cmt3.cpp:1419
xsens::Cmt3::getLocationId
XsensResultValue getLocationId(uint16_t &locationId, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return the location ID of a sensor.
Definition: cmt3.cpp:918
CMT_BAUDCODE_921K6
#define CMT_BAUDCODE_921K6
Definition: cmtdef.h:509
CmtDeviceConfiguration::_devInfo::m_outputMode
uint16_t m_outputMode
Definition: cmtdef.h:882
XRV_ALREADYOPEN
@ XRV_ALREADYOPEN
Definition: xsens_std.h:145
CMT_LEN_MAGNETICDECLINATION
#define CMT_LEN_MAGNETICDECLINATION
Definition: cmtdef.h:230
XsensResultValue
XsensResultValue
Xsens return values.
Definition: xsens_std.h:31
xsens::Cmt3::getGpsStatus
XsensResultValue getGpsStatus(CmtGpsStatus &status, const CmtDeviceId deviceId=CMT_DID_MASTER)
Request the status of the GPS satellites in orbit.
Definition: cmt3.cpp:3038
CMT_BID_INVALID
#define CMT_BID_INVALID
Definition: cmtdef.h:1294
xsens::Cmt3::getHeading
XsensResultValue getHeading(double &heading, const CmtDeviceId deviceId=CMT_DID_MASTER)
Return the heading offset.
Definition: cmt3.cpp:906
CmtDeviceConfiguration::m_syncinOffset
uint32_t m_syncinOffset
Definition: cmtdef.h:872
CMT_BAUDCODE_57K6
#define CMT_BAUDCODE_57K6
Definition: cmtdef.h:504
CMT_MID_REQOUTPUTSKIPFACTOR
#define CMT_MID_REQOUTPUTSKIPFACTOR
Definition: cmtdef.h:173
xsens::Cmt2s
Mid-level serial communication class.
Definition: cmt2.h:41
xsens::Cmt3::getGpsLeverArm
XsensResultValue getGpsLeverArm(CmtVector &arm, const CmtDeviceId deviceId=CMT_DID_MASTER)
Get the currently used GPS lever arm.
Definition: cmt3.cpp:3025
XRV_NULLPTR
@ XRV_NULLPTR
Definition: xsens_std.h:160
xsens::Message::setMessageId
void setMessageId(const uint8_t msgId)
Set the new value of the m_messageId field and update the checksum.
Definition: cmtmessage.cpp:726
CMT_MID_REQFWREV
#define CMT_MID_REQFWREV
Definition: cmtdef.h:125
xsens::Cmt3::m_period
uint16_t m_period
The sample period of the port.
Definition: cmt3.h:53
xsens::Packet::m_msg
Message m_msg
The message.
Definition: cmtpacket.h:78
CMT_MID_GOTOCONFIG
#define CMT_MID_GOTOCONFIG
Definition: cmtdef.h:297
deviceId
#define deviceId
Definition: CIMUXSens.cpp:40
xsens::timeStampNow
TimeStamp timeStampNow(void)
Definition: xsens_time.cpp:101
xsens::Cmt3::getMasterId
CmtDeviceId getMasterId(void)
Definition: cmt3.cpp:1002
xsens::Cmt3::setSyncOutSkipFactor
XsensResultValue setSyncOutSkipFactor(const uint16_t skipFactor)
Set the outbound synchronization skip factor of an MT device.
Definition: cmt3.cpp:2504
xsens::Cmt3::m_logFile
Cmt2f m_logFile
The (optional) CMT level 2 logfile object that this class operates on.
Definition: cmt3.h:47
XRV_OTHER
@ XRV_OTHER
Definition: xsens_std.h:197
CMT_PARAM_SYNCIN_OFFSET
#define CMT_PARAM_SYNCIN_OFFSET
Definition: cmtdef.h:581
CMT_MID_REQOPMODE
#define CMT_MID_REQOPMODE
Definition: cmtdef.h:134
size
GLsizeiptr size
Definition: glext.h:3923
CMT_MID_REQDATA
#define CMT_MID_REQDATA
Definition: cmtdef.h:304
CMT_CONF_BLOCKLEN
#define CMT_CONF_BLOCKLEN
Definition: cmtdef.h:553
xsens
The namespace of all Xsens software since 2006.
Definition: cmt1.cpp:95
CmtDeviceConfiguration::_devInfo::m_reserved
uint8_t m_reserved[8]
Definition: cmtdef.h:884
xsens::Message::setDataShort
void setDataShort(const uint16_t data, const uint16_t offset=0)
Write an uint16_t (16 bits) into the data buffer.
Definition: cmtmessage.cpp:710
CMT_BID_MASTER
#define CMT_BID_MASTER
Definition: cmtdef.h:61
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
xsens::Cmt3::getEMtsData
XsensResultValue getEMtsData(void *buffer, const CmtDeviceId deviceId=CMT_DID_MASTER)
Retrieve the eMts data of the specified sensor(s).
Definition: cmt3.cpp:658
CMT_MID_REQFILTERSETTINGS
#define CMT_MID_REQFILTERSETTINGS
Definition: cmtdef.h:480
CmtMtTimeStamp
uint16_t CmtMtTimeStamp
An MT timestamp (sample count)
Definition: cmtdef.h:924
xsens::Message::setBusId
void setBusId(const uint8_t busId)
Set the new value of the m_busId field and update the checksum.
Definition: cmtmessage.cpp:496
CmtDeviceConfiguration::m_numberOfDevices
uint16_t m_numberOfDevices
Definition: cmtdef.h:877
CMT_SYNC_CLOCK_NS_TO_TICKS
#define CMT_SYNC_CLOCK_NS_TO_TICKS
Definition: cmtdef.h:575
CmtDeviceMode2::m_period
uint16_t m_period
Definition: cmtdef.h:1055
xsens::Cmt3::setLogMode
XsensResultValue setLogMode(bool active)
Set the logging mode.
Definition: cmt3.cpp:2912
xsens::Message::setDataByte
void setDataByte(const uint8_t data, const uint16_t offset=0)
Write an unsigned byte (8 bits) into the data buffer.
Definition: cmtmessage.cpp:523
xsens::Cmt3::getPortNr
XsensResultValue getPortNr(uint8_t &port) const
Return the device Id of an MT device.
Definition: cmt3.cpp:1038
xsens::Cmt3::setSyncOutMode
XsensResultValue setSyncOutMode(const uint16_t mode)
Set the outbound synchronization mode of an MT device.
Definition: cmt3.cpp:2451
xsens::Cmt3::setSyncInOffset
XsensResultValue setSyncInOffset(const uint32_t offset)
Set the inbound synchronization offset of an MT device.
Definition: cmt3.cpp:2346
mrpt::system::os::memcpy
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356
CMT_GOTO_CONFIG_TRIES
#define CMT_GOTO_CONFIG_TRIES
Definition: cmtdef.h:772


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST