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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018