Main MRPT website > C++ reference for MRPT 1.9.9
cmtpacket.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 cmtpacket.cpp
11 
12  For information about objects in this file, see the appropriate header:
13  \ref Cmtpacket.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 
28 #include "cmtpacket.h"
29 
30 #ifdef _CMT_DLL_EXPORT
31 #include "xsens_math.h"
32 #endif
33 
34 #ifdef _LOG_PACKET
35 #define PACKETLOG CMTLOG
36 #else
37 #define PACKETLOG(...)
38 #endif
39 
40 namespace xsens
41 {
42 //////////////////////////////////////////////////////////////////////////////////////////
43 ////////////////////////////////////// Packet class
44 /////////////////////////////////////////
45 //////////////////////////////////////////////////////////////////////////////////////////
46 
47 //////////////////////////////////////////////////////////////////////////////////////////
48 Packet::Packet(uint16_t items, bool xbus)
49 {
50  PACKETLOG("Create Packet %p\n", this);
51 
52  m_itemCount = items;
53  m_infoList = nullptr;
55  m_toa = 0;
56  m_rtc = 0;
57  m_xm = xbus;
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////
62 {
63  PACKETLOG("Destroy Packet %p\n", this);
64  LSTCHKDELNUL(m_formatList);
65  LSTCHKDELNUL(m_infoList);
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////
70 {
71  if (index >= m_itemCount || m_formatList == nullptr)
72  {
73  CmtDataFormat temp;
74  return temp;
75  }
76  return m_formatList[index];
77 }
78 
79 //////////////////////////////////////////////////////////////////////////////////////////
81 {
82  if (index < m_itemCount)
83  {
85  LSTCHKDELNUL(m_infoList);
86  return true;
87  }
88  return false;
89 }
90 
91 //////////////////////////////////////////////////////////////////////////////////////////
93  const CmtOutputMode outputMode, const CmtOutputSettings outputSettings,
94  const uint16_t index)
95 {
96  if (index < m_itemCount)
97  {
98  m_formatList[index].m_outputMode = outputMode;
99  m_formatList[index].m_outputSettings = outputSettings;
100  LSTCHKDELNUL(m_infoList);
101  return true;
102  }
103  return false;
104 }
105 
106 //////////////////////////////////////////////////////////////////////////////////////////
107 bool Packet::getXbus(void) const { return m_xm; }
108 //////////////////////////////////////////////////////////////////////////////////////////
109 void Packet::setXbus(bool xbus, bool convert)
110 {
111  if (xbus != m_xm)
112  {
113  if (convert)
114  {
115  CmtMtTimeStamp stamp = getSampleCounter(0);
116 
117  // move the time stamp value(s) around
118  if (xbus)
119  {
120  // new version is Xbus, so old version is regular format ->
121  // place timestamp at start of packet and remove from all
122  // individual data units
123  // remove the timestamp from all data units
124 
125  // insert timestamp at the start
126  m_msg.insertData(2, 0);
127  m_msg.setDataShort(stamp, 0);
128  }
129  else
130  {
131  // new version is regular, so old version is Xbus format ->
132  // place timestamp at end of all individual data units and
133  // remove from start of packet
134  // append the timestamp to all data units
135 
136  // remove timestamp from the start
137  m_msg.deleteData(2, 0);
138  }
139  }
140  // update the state cache
141  m_xm = xbus;
142  LSTCHKDELNUL(m_infoList);
143  }
144 }
145 
146 //////////////////////////////////////////////////////////////////////////////////////////
147 // Return the floating/fixed point value size
149 {
150  uint16_t ds;
151  switch (m_formatList[index].m_outputSettings &
153  {
155  ds = 4;
156  break;
157 
159  ds = 8;
160  break;
162  ds = 6;
163  break;
164 
166  ds = 4;
167  break;
168 
169  default:
170  ds = 0;
171  break;
172  }
173  return ds;
174 }
175 
176 //////////////////////////////////////////////////////////////////////////////////////////
177 // Return the data size.
179 {
180  if (m_infoList == nullptr)
181  {
182  // allocate list
184  uint16_t totalOffset;
185  if (m_xm)
186  totalOffset = 2;
187  else
188  totalOffset = 0;
189 
190  // fill lists
191  for (uint16_t i = 0; i < m_itemCount; ++i)
192  {
193  m_infoList[i].m_offset = totalOffset;
194  m_infoList[i].m_size = 0;
195 
196  uint16_t ds = getFPValueSize(i);
197 
198  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_RAW)
199  {
200  m_infoList[i].m_rawData = totalOffset;
201  m_infoList[i].m_rawAcc = totalOffset;
202  m_infoList[i].m_rawGyr = totalOffset + 6;
203  m_infoList[i].m_rawMag = totalOffset + 12;
204  m_infoList[i].m_rawTemp = totalOffset + 18;
205 
206  m_infoList[i].m_size += 20;
207  totalOffset += 20;
208  }
209  else
210  {
216  }
217 
218  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_RAWGPSPRINT)
219  {
220  m_infoList[i].m_rawGpsData = totalOffset;
221  m_infoList[i].m_rawGpsPressure = totalOffset;
222  m_infoList[i].m_rawGpsPressureAge = totalOffset + 2;
223  m_infoList[i].m_size += 3;
224  totalOffset += 3;
225 
226  m_infoList[i].m_rawGpsGpsData = totalOffset;
227  m_infoList[i].m_rawGpsItow = totalOffset;
228  m_infoList[i].m_rawGpsLatitude = totalOffset + 4;
229  m_infoList[i].m_rawGpsLongitude = totalOffset + 8;
230  m_infoList[i].m_rawGpsHeight = totalOffset + 12;
231  m_infoList[i].m_rawGpsVeln = totalOffset + 16;
232  m_infoList[i].m_rawGpsVele = totalOffset + 20;
233  m_infoList[i].m_rawGpsVeld = totalOffset + 24;
234  m_infoList[i].m_rawGpsHacc = totalOffset + 28;
235  m_infoList[i].m_rawGpsVacc = totalOffset + 32;
236  m_infoList[i].m_rawGpsSacc = totalOffset + 36;
237  m_infoList[i].m_rawGpsGpsAge = totalOffset + 40;
238  m_infoList[i].m_size += 41;
239  totalOffset += 41;
240  }
241  else
242  {
246 
259  }
260 
262  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_TEMP)
263  {
264  m_infoList[i].m_temp = totalOffset;
265  m_infoList[i].m_size += ds;
266  totalOffset += ds;
267  }
268 
269  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_CALIB)
270  {
271  if ((m_formatList[i].m_outputSettings &
273  {
274  m_infoList[i].m_calAcc = totalOffset;
275  m_infoList[i].m_size += 3 * ds;
276  totalOffset += 3 * ds;
277  }
278  else
280 
281  if ((m_formatList[i].m_outputSettings &
283  {
284  m_infoList[i].m_calGyr = totalOffset;
285  m_infoList[i].m_size += 3 * ds;
286  totalOffset += 3 * ds;
287  }
288  else
290 
291  if ((m_formatList[i].m_outputSettings &
293  {
294  m_infoList[i].m_calMag = totalOffset;
295  m_infoList[i].m_size += 3 * ds;
296  totalOffset += 3 * ds;
297  }
298  else
300 
301  if ((m_formatList[i].m_outputSettings &
304  else
306  }
307  else
308  {
313  }
314 
318 
319  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_ORIENT)
320  {
321  switch (m_formatList[i].m_outputSettings &
323  {
325  m_infoList[i].m_oriEul = totalOffset;
326  m_infoList[i].m_size += 3 * ds;
327  totalOffset += 3 * ds;
328  break;
330  m_infoList[i].m_oriQuat = totalOffset;
331  m_infoList[i].m_size += 4 * ds;
332  totalOffset += 4 * ds;
333  break;
335  m_infoList[i].m_oriMat = totalOffset;
336  m_infoList[i].m_size += 9 * ds;
337  totalOffset += 9 * ds;
338  break;
339  default:
340  break;
341  }
342  }
343 
345  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_AUXILIARY)
346  {
347  if ((m_formatList[i].m_outputSettings &
349  {
350  m_infoList[i].m_analogIn1 = totalOffset;
351  m_infoList[i].m_size += 2;
352  totalOffset += 2;
353  }
354  else
356 
357  if ((m_formatList[i].m_outputSettings &
359  {
360  m_infoList[i].m_analogIn2 = totalOffset;
361  m_infoList[i].m_size += 2;
362  totalOffset += 2;
363  }
364  else
366  }
367 
369  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_POSITION)
370  {
371  switch (m_formatList[i].m_outputSettings &
373  {
375  m_infoList[i].m_posLLA = totalOffset;
376  m_infoList[i].m_size += 3 * ds;
377  totalOffset += 3 * ds;
378  }
379  }
380 
382  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_VELOCITY)
383  {
384  switch (m_formatList[i].m_outputSettings &
386  {
388  m_infoList[i].m_velNEDorNWU = totalOffset;
389  m_infoList[i].m_size += 3 * ds;
390  totalOffset += 3 * ds;
391  }
392  }
393 
395  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_STATUS)
396  {
397  m_infoList[i].m_status = totalOffset;
398  m_infoList[i].m_size += 1;
399  totalOffset += 1;
400  }
401 
403  if (m_xm) m_infoList[i].m_sc = 0;
404  if ((m_formatList[i].m_outputSettings &
407  {
408  if (!m_xm) m_infoList[i].m_sc = totalOffset;
409  m_infoList[i].m_size += 2;
410  totalOffset += 2;
411  }
412 
413  // post-processing data is never available at this point
415  }
416  }
417 
418  if (index < m_itemCount) return m_infoList[index].m_size;
419  return 0;
420 }
421 
422 //////////////////////////////////////////////////////////////////////////////////////////
423 // Return the Raw Accelerometer component of a data item.
425 {
427  if (containsRawAcc(index))
428  for (uint16_t i = 0; i < 3; ++i)
429  buffer.m_data[i] =
431 
432  return buffer;
433 }
435 {
436  if (getDataSize(index) == 0) return false;
437  if (m_infoList[index].m_rawAcc == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
438  return true;
439 }
441 {
442  if (getDataSize(index) == 0) return false;
444  {
445  // add
447  m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
448  m_infoList[index].m_size += 3 * 2;
449  }
450  // update
451  for (uint16_t i = 0; i < 3; ++i)
452  m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawAcc + (2 * i));
453  return true;
454 }
455 
456 //////////////////////////////////////////////////////////////////////////////////////////
457 // Return the Raw Gyroscope component of a data item.
459 {
461  if (containsRawGyr(index))
462  for (uint16_t i = 0; i < 3; ++i)
463  buffer.m_data[i] =
465 
466  return buffer;
467 }
469 {
470  if (getDataSize(index) == 0) return false;
471  if (m_infoList[index].m_rawGyr == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
472  return true;
473 }
475 {
476  if (getDataSize(index) == 0) return false;
478  {
479  // add
481  m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
482  m_infoList[index].m_size += 3 * 2;
483  }
484  // update
485  for (uint16_t i = 0; i < 3; ++i)
486  m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawGyr + (2 * i));
487  return true;
488 }
489 
490 //////////////////////////////////////////////////////////////////////////////////////////
491 // Return the Raw Magnetometer component of a data item.
493 {
495  if (containsRawMag(index))
496  for (uint16_t i = 0; i < 3; ++i)
497  buffer.m_data[i] =
499 
500  return buffer;
501 }
503 {
504  if (getDataSize(index) == 0) return false;
505  if (m_infoList[index].m_rawMag == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
506  return true;
507 }
509 {
510  if (getDataSize(index) == 0) return false;
512  {
513  // add
515  m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
516  m_infoList[index].m_size += 3 * 2;
517  }
518  // update
519  for (uint16_t i = 0; i < 3; ++i)
520  m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawMag + (2 * i));
521  return true;
522 }
523 
524 //////////////////////////////////////////////////////////////////////////////////////////
525 // Return the Raw Temperature component of a data item.
527 {
528  if (!containsRawTemp(index)) return 0;
529 
530  return m_msg.getDataShort(m_infoList[index].m_rawTemp);
531 }
533 {
534  if (getDataSize(index) == 0) return false;
535  if (m_infoList[index].m_rawTemp == CMT_DATA_ITEM_NOT_AVAILABLE)
536  return false;
537  return true;
538 }
540 {
541  if (getDataSize(index) == 0) return false;
542  if (m_infoList[index].m_rawTemp == CMT_DATA_ITEM_NOT_AVAILABLE)
543  {
544  // add
547  m_infoList[index].m_size += 2;
548  }
549  // update
550  m_msg.setDataShort(temp, m_infoList[index].m_rawTemp);
551  return true;
552 }
553 
554 //////////////////////////////////////////////////////////////////////////////////////////
555 // Return the Raw Data component of a data item.
557 {
559  if (containsRawData(index))
560  {
561  const uint8_t* tmp = m_msg.getDataBuffer(m_infoList[index].m_rawData);
562  const uint16_t* sh = (const uint16_t*)tmp;
563  uint16_t* bare = (uint16_t*)&buffer;
564 
565  for (uint16_t i = 0; i < 10; ++i, ++sh, ++bare)
566  *bare = swapEndian16(
567  *sh); // m_msg.getDataShort(m_infoList[index].m_rawData +
568  // (2*i));
569  }
570  return buffer;
571 }
573 {
574  if (getDataSize(index) == 0) return false;
575  if (m_infoList[index].m_rawData == CMT_DATA_ITEM_NOT_AVAILABLE)
576  return false;
577  return true;
578 }
580 {
581  if (getDataSize(index) == 0) return false;
582  if (m_infoList[index].m_rawData == CMT_DATA_ITEM_NOT_AVAILABLE)
583  {
584  // add
586  m_msg.resizeData(m_msg.getDataSize() + 3 * 3 * 2 + 2);
591  m_infoList[index].m_size += 3 * 3 * 2 + 2;
592  }
593  // update
594  int16_t* bare = (int16_t*)&data;
595  for (uint16_t i = 0; i < 10; ++i)
596  m_msg.setDataShort(bare[i], m_infoList[index].m_rawData + (2 * i));
597  return true;
598 }
599 
600 //////////////////////////////////////////////////////////////////////////////////////////
601 // Return the Raw Data component of a data item.
603 {
606  {
607  // const uint8_t* tmp =
608  // m_msg.getDataBuffer(m_infoList[index].m_rawGpsData);
609  // const uint16_t* sh = (const uint16_t*) tmp;
610  // uint16_t* bare = (uint16_t*) &buffer;
611 
612  // pressure data
613  buffer.m_pressure =
614  m_msg.getDataShort(m_infoList[index].m_rawGpsPressure);
615  // pressAge
616  buffer.m_pressureAge =
617  m_msg.getDataByte(m_infoList[index].m_rawGpsPressureAge);
618 
619  // lon,lat,height,hacc,vacc,veln,vele,veld
620  // tmp = m_msg.getDataBuffer(m_infoList[index].m_rawGpsGpsData);
621  // const uint32_t* ln = (const uint32_t*) tmp;
622  uint32_t* bareln = (uint32_t*)&buffer.m_itow;
623  for (uint16_t i = 0; i < 10; ++i)
624  bareln[i] =
626 
627  // gpsAge
628  buffer.m_gpsAge = m_msg.getDataByte(m_infoList[index].m_rawGpsGpsAge);
629  }
630  return buffer;
631 }
633 {
634  if (getDataSize(index) == 0) return false;
635  if (m_infoList[index].m_rawGpsData == CMT_DATA_ITEM_NOT_AVAILABLE)
636  return false;
637  return true;
638 }
640 {
641  if (getDataSize(index) == 0) return false;
642  if (m_infoList[index].m_rawGpsData == CMT_DATA_ITEM_NOT_AVAILABLE)
643  {
644  // add
646  m_msg.resizeData(m_msg.getDataSize() + (2 + 1) + (40 + 1));
650 
654  m_infoList[index].m_rawGpsData + 3 + 4;
656  m_infoList[index].m_rawGpsData + 3 + 8;
658  m_infoList[index].m_rawGpsData + 3 + 12;
660  m_infoList[index].m_rawGpsData + 3 + 16;
662  m_infoList[index].m_rawGpsData + 3 + 20;
664  m_infoList[index].m_rawGpsData + 3 + 24;
666  m_infoList[index].m_rawGpsData + 3 + 28;
668  m_infoList[index].m_rawGpsData + 3 + 32;
670  m_infoList[index].m_rawGpsData + 3 + 36;
672  m_infoList[index].m_rawGpsData + 3 + 40;
673 
674  m_infoList[index].m_size += (2 + 1) + (40 + 1);
675  }
676  // update
679  data.m_pressureAge, m_infoList[index].m_rawGpsPressureAge);
680 
681  // lon,lat,height,hacc,vacc,veln,vele,veld
682  int32_t* bareln = (int32_t*)&data.m_itow;
683  for (uint16_t i = 0; i < 10; ++i)
685  bareln[i], m_infoList[index].m_rawGpsGpsData + (4 * i));
686 
687  // gpsAge
689  return true;
690 }
691 
692 //////////////////////////////////////////////////////////////////////////////////////////
693 // Return the Temperature component of a data item.
694 double Packet::getTemp(const uint16_t index) const
695 {
696  if (containsTemp(index))
697  return m_msg.getDataFPValue(
698  m_formatList[index].m_outputSettings, m_infoList[index].m_temp);
699 
700  return 0.0;
701 }
703 {
704  if (getDataSize(index) == 0) return false;
705  if (m_infoList[index].m_temp == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
706  return true;
707 }
708 bool Packet::updateTemp(const double& temp, const uint16_t index)
709 {
710  if (getDataSize(index) == 0) return false;
711 
713 
715  {
716  // add
719  m_infoList[index].m_size += ds;
720  }
721  // update
723  m_formatList[index].m_outputSettings, temp, m_infoList[index].m_temp);
724  return true;
725 }
726 
727 //////////////////////////////////////////////////////////////////////////////////////////
728 // Return the Calibrated Accelerometer component of a data item.
730 {
732  if (containsCalAcc(index))
736  else
737  memset(&buffer, 0, sizeof(buffer));
738  return buffer;
739 }
741 {
742  if (getDataSize(index) == 0) return false;
743  if (m_infoList[index].m_calAcc == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
744  return true;
745 }
747 {
748  const uint16_t numValues = 3;
749  if (getDataSize(index) == 0) return false;
750 
752 
754  {
755  // add
757  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
758  m_infoList[index].m_size += numValues * ds;
759  }
760  // update
762  m_formatList[index].m_outputSettings, &vec.m_data[0],
763  m_infoList[index].m_calAcc, numValues);
764  return true;
765 }
766 
767 //////////////////////////////////////////////////////////////////////////////////////////
768 // Return the Calibrated Gyroscope component of a data item.
770 {
772  if (containsCalGyr(index))
776  else
777  memset(&buffer, 0, sizeof(buffer));
778  return buffer;
779 }
781 {
782  if (getDataSize(index) == 0) return false;
783  if (m_infoList[index].m_calGyr == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
784  return true;
785 }
787 {
788  const uint16_t numValues = 3;
789  if (getDataSize(index) == 0) return false;
790 
792 
794  {
795  // add
797  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
798  m_infoList[index].m_size += numValues * ds;
799  }
800  // update
802  m_formatList[index].m_outputSettings, &vec.m_data[0],
803  m_infoList[index].m_calGyr, numValues);
804  return true;
805 }
806 
807 //////////////////////////////////////////////////////////////////////////////////////////
808 // Return the Calibrated Magnetometer component of a data item.
810 {
812  if (containsCalMag(index))
816  else
817  memset(&buffer, 0, sizeof(buffer));
818  return buffer;
819 }
821 {
822  if (getDataSize(index) == 0) return false;
823  if (m_infoList[index].m_calMag == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
824  return true;
825 }
827 {
828  const uint16_t numValues = 3;
829  if (getDataSize(index) == 0) return false;
830 
832 
834  {
835  // add
837  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
838  m_infoList[index].m_size += numValues * ds;
839  }
840  // update
842  m_formatList[index].m_outputSettings, &vec.m_data[0],
843  m_infoList[index].m_calMag, numValues);
844  return true;
845 }
846 
847 //////////////////////////////////////////////////////////////////////////////////////////
848 // Return the Calibrated Accelerometer component of a data item.
850 {
852  double* bare = (double*)&buffer;
853  if (containsCalData(index))
857  else
858  memset(&buffer, 0, sizeof(buffer));
859  return buffer;
860 }
862 {
863  if (getDataSize(index) == 0) return false;
864  if (m_infoList[index].m_calData == CMT_DATA_ITEM_NOT_AVAILABLE)
865  return false;
866  return true;
867 }
869 {
870  const uint16_t numValues = 9;
871  if (getDataSize(index) == 0) return false;
872 
874 
875  if (m_infoList[index].m_calData == CMT_DATA_ITEM_NOT_AVAILABLE)
876  {
877  // add
878  m_msg.m_autoUpdateChecksum = false;
880  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
884  m_infoList[index].m_size += numValues * ds;
885  }
886  // update
887 
888  double* bare = (double*)&data;
890  m_formatList[index].m_outputSettings, bare, m_infoList[index].m_calData,
891  numValues);
892  return true;
893 }
894 
895 //////////////////////////////////////////////////////////////////////////////////////////
896 // Return the Orientation component of a data item as a Quaternion.
898 {
899  CmtQuat buffer;
900  if (containsOriQuat(index))
904  else
905  memset(&buffer, 0, sizeof(buffer));
906  return buffer;
907 }
908 
910 {
911  if (getDataSize(index) == 0) return false;
912  if (m_infoList[index].m_oriQuat == CMT_DATA_ITEM_NOT_AVAILABLE)
913  return false;
914  return true;
915 }
917 {
918  const uint16_t numValues = 4;
919  if (getDataSize(index) == 0) return false;
920 
922 
923  if (m_infoList[index].m_oriQuat == CMT_DATA_ITEM_NOT_AVAILABLE)
924  {
925  // add
927  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
928  m_infoList[index].m_size += numValues * ds;
929  }
930  // update
932  m_formatList[index].m_outputSettings, &data.m_data[0],
933  m_infoList[index].m_oriQuat, numValues);
934  return true;
935 }
936 
937 //////////////////////////////////////////////////////////////////////////////////////////
938 // Return the Orientation component of a data item as CmtEuler angles.
940 {
942  if (containsOriEuler(index))
943  {
945  buffer.m_roll = m_msg.getDataFPValue(
946  m_formatList[index].m_outputSettings, m_infoList[index].m_oriEul);
947  buffer.m_pitch = m_msg.getDataFPValue(
948  m_formatList[index].m_outputSettings,
949  m_infoList[index].m_oriEul + ds);
950  buffer.m_yaw = m_msg.getDataFPValue(
951  m_formatList[index].m_outputSettings,
952  m_infoList[index].m_oriEul + 2 * ds);
953  }
954  return buffer;
955 }
957 {
958  if (getDataSize(index) == 0) return false;
959  if (m_infoList[index].m_oriEul == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
960  return true;
961 }
963 {
964  const uint16_t numValues = 3;
965  if (getDataSize(index) == 0) return false;
966 
968 
970  {
971  // add
973  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
974  m_infoList[index].m_size += numValues * ds;
975  }
976  // update
978  m_formatList[index].m_outputSettings, data.m_roll,
981  m_formatList[index].m_outputSettings, data.m_pitch,
982  m_infoList[index].m_oriEul + ds);
984  m_formatList[index].m_outputSettings, data.m_yaw,
985  m_infoList[index].m_oriEul + 2 * ds);
986  return true;
987 }
988 
989 //////////////////////////////////////////////////////////////////////////////////////////
990 // Return the Orientation component of a data item as an Orientation Matrix.
992 {
994  uint16_t k = 0;
996  {
998  for (int32_t i = 0; i < 3; ++i)
999  for (int32_t j = 0; j < 3; ++j, k += ds)
1000  buffer.m_data[i][j] = m_msg.getDataFPValue(
1002  m_infoList[index].m_oriMat + k);
1003  }
1004  else
1005  memset(&buffer, 0, sizeof(buffer));
1006 
1007  return buffer;
1008 }
1010 {
1011  if (getDataSize(index) == 0) return false;
1012  if (m_infoList[index].m_oriMat == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
1013  return true;
1014 }
1016 {
1017  const uint16_t numValues = 9;
1018  if (getDataSize(index) == 0) return false;
1019 
1021 
1022  if (m_infoList[index].m_oriMat == CMT_DATA_ITEM_NOT_AVAILABLE)
1023  {
1024  // add
1026  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1027  m_infoList[index].m_size += numValues * ds;
1028  }
1029  // update
1030  uint16_t k = 0;
1031  for (int32_t i = 0; i < 3; ++i)
1032  for (int32_t j = 0; j < 3; ++j, k += ds)
1034  m_formatList[index].m_outputSettings, data.m_data[i][j],
1035  m_infoList[index].m_oriMat + k);
1036  return true;
1037 }
1038 
1039 //////////////////////////////////////////////////////////////////////////////////////////
1040 // Return the AnalogIn 1 component of a data item.
1042 {
1044  if (containsAnalogIn1(index))
1045  buffer.m_data = m_msg.getDataShort(m_infoList[index].m_analogIn1);
1046 
1047  return buffer;
1048 }
1050 {
1051  if (getDataSize(index) == 0) return false;
1052  if (m_infoList[index].m_analogIn1 == CMT_DATA_ITEM_NOT_AVAILABLE)
1053  return false;
1054  return true;
1055 }
1057 {
1058  if (getDataSize(index) == 0) return false;
1059  if (m_infoList[index].m_analogIn1 == CMT_DATA_ITEM_NOT_AVAILABLE)
1060  {
1061  // add
1064  m_infoList[index].m_size += 2;
1065  }
1066  // update
1068  return true;
1069 }
1070 
1071 //////////////////////////////////////////////////////////////////////////////////////////
1072 // Return the AnalogIn 2 component of a data item.
1074 {
1076  if (containsAnalogIn2(index))
1077  buffer.m_data = m_msg.getDataShort(m_infoList[index].m_analogIn2);
1078 
1079  return buffer;
1080 }
1082 {
1083  if (getDataSize(index) == 0) return false;
1084  if (m_infoList[index].m_analogIn2 == CMT_DATA_ITEM_NOT_AVAILABLE)
1085  return false;
1086  return true;
1087 }
1089 {
1090  if (getDataSize(index) == 0) return false;
1091  if (m_infoList[index].m_analogIn2 == CMT_DATA_ITEM_NOT_AVAILABLE)
1092  {
1093  // add
1096  m_infoList[index].m_size += 2;
1097  }
1098  // update
1100  return true;
1101 }
1102 
1103 //////////////////////////////////////////////////////////////////////////////////////////
1104 // Return the Position LLA component of a data item.
1106 {
1107  CmtVector buffer;
1111  m_infoList[index].m_posLLA, 3);
1112  else
1113  memset(&buffer, 0, sizeof(buffer));
1114  return buffer;
1115 }
1117 {
1118  if (getDataSize(index) == 0) return false;
1119  if (m_infoList[index].m_posLLA == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
1120  return true;
1121 }
1123 {
1124  const uint16_t numValues = 3;
1125  if (getDataSize(index) == 0) return false;
1126 
1128 
1129  if (m_infoList[index].m_posLLA == CMT_DATA_ITEM_NOT_AVAILABLE)
1130  {
1131  // add
1133  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1134  m_infoList[index].m_size += numValues * ds;
1135  }
1136  // update
1138  m_formatList[index].m_outputSettings, &data.m_data[0],
1139  m_infoList[index].m_posLLA, numValues);
1140  return true;
1141 }
1142 
1143 //////////////////////////////////////////////////////////////////////////////////////////
1144 // Return the Velocity NED component of a data item.
1146 {
1147  CmtVector buffer;
1148  if (containsVelocity(index))
1152  else
1153  memset(&buffer, 0, sizeof(buffer));
1154 
1155  return buffer;
1156 }
1158 {
1159  if (getDataSize(index) == 0) return false;
1160  if (m_infoList[index].m_velNEDorNWU == CMT_DATA_ITEM_NOT_AVAILABLE)
1161  return false;
1162  return true;
1163 }
1165 {
1166  const uint16_t numValues = 3;
1167  if (getDataSize(index) == 0) return false;
1168 
1170 
1171  if (m_infoList[index].m_velNEDorNWU == CMT_DATA_ITEM_NOT_AVAILABLE)
1172  {
1173  // add
1175  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1176  m_infoList[index].m_size += numValues * ds;
1177  }
1178  // update
1180  m_formatList[index].m_outputSettings, &data.m_data[0],
1181  m_infoList[index].m_velNEDorNWU, numValues);
1182  return true;
1183 }
1184 
1185 //////////////////////////////////////////////////////////////////////////////////////////
1186 // Return the Status component of a data item.
1188 {
1189  if (containsStatus(index))
1190  return m_msg.getDataByte(m_infoList[index].m_status);
1191  return 0;
1192 }
1194 {
1195  if (getDataSize(index) == 0) return false;
1196  if (m_infoList[index].m_status == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
1197  return true;
1198 }
1200 {
1201  if (getDataSize(index) == 0) return false;
1202 
1203  if (m_infoList[index].m_status == CMT_DATA_ITEM_NOT_AVAILABLE)
1204  {
1205  // add
1208  m_infoList[index].m_size += 1;
1209  }
1210  // update
1211  m_msg.setDataByte(data, m_infoList[index].m_status);
1212  return true;
1213 }
1214 
1215 //////////////////////////////////////////////////////////////////////////////////////////
1216 // Return the Sample Counter component of the packet.
1218 {
1219  if (!containsSampleCounter(index)) return 0;
1220  return m_msg.getDataShort(m_infoList[index].m_sc);
1221 }
1223 {
1224  if (getDataSize(index) == 0) return false;
1225  if (m_infoList[index].m_sc == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
1226  return true;
1227 }
1229 {
1230  if (getDataSize(index) == 0) return false;
1232  {
1233  // add
1236  m_infoList[index].m_size += 2;
1237  }
1238  // update
1239  m_msg.setDataShort(counter, m_infoList[index].m_sc);
1240  return true;
1241 }
1243 {
1245  return m_rtc;
1246 }
1247 
1248 //////////////////////////////////////////////////////////////////////////////////////////
1249 // Return the XKF-3 Acc G component of the packet.
1251 {
1252  CmtVector buffer;
1253  if (containsAccG(index))
1256  m_infoList[index].m_acc_g, 3);
1257  else
1258  memset(&buffer, 0, sizeof(buffer));
1259 
1260  return buffer;
1261 }
1263 {
1264  if (getDataSize(index) == 0) return false;
1265  if (m_infoList[index].m_acc_g == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
1266  return true;
1267 }
1269 {
1270  const uint16_t numValues = 3;
1271  if (getDataSize(index) == 0) return false;
1272 
1274 
1276  {
1277  // add space
1279  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1280  m_infoList[index].m_size += numValues * ds;
1281  }
1282  // update
1284  m_formatList[index].m_outputSettings, &g.m_data[0],
1285  m_infoList[index].m_acc_g, numValues);
1286  return true;
1287 }
1288 
1290 {
1291  PACKETLOG("Create new packet from Packet %p\n", &pack);
1292 
1293  m_itemCount = 0;
1294  m_formatList = nullptr;
1295  m_infoList = nullptr;
1296  *this = pack;
1297 }
1298 
1299 void Packet::operator=(const Packet& pack)
1300 {
1301  PACKETLOG("Copy packet from Packet %p\n", this);
1302 
1303  if (m_itemCount != pack.m_itemCount)
1304  {
1305  LSTCHKDELNUL(m_formatList);
1306  m_itemCount = pack.m_itemCount;
1308  }
1309  for (uint16_t i = 0; i < m_itemCount; ++i)
1310  m_formatList[i] = pack.m_formatList[i];
1311  LSTCHKDELNUL(m_infoList);
1312  m_toa = pack.m_toa;
1313  m_rtc = pack.m_rtc;
1314  m_msg = pack.m_msg;
1315  m_xm = pack.m_xm;
1316 }
1317 
1318 #ifdef _CMT_DLL_EXPORT
1319 void Packet::interpolate(const Packet& pa, const Packet& pb, const double f)
1320 {
1321  pa.getDataSize();
1322  pb.getDataSize();
1323 
1324  *this = pb;
1325 
1326  if (m_itemCount != pa.m_itemCount) return;
1327 
1328  getDataSize();
1329  CmtShortVector va, vb, vc;
1330  CmtVector fa, fb, fc;
1331  CmtQuat qf;
1332  CmtEuler ea, eb, ec;
1333  CmtMatrix mf;
1334 
1335  Quaternion qa, qb, qc;
1336  Matrix3x3 m;
1337 
1338  double g = 1.0 - f;
1339  double da, db, dc;
1340  uint16_t sa, sb, sc;
1341 
1342  // walk over all items and interpolate all data items
1343  for (uint16_t i = 0; i < m_itemCount; ++i)
1344  {
1345  //// raw data
1348  {
1349  va = pa.getRawAcc(i);
1350  vb = pb.getRawAcc(i);
1351  vc.m_data[0] =
1352  (uint16_t)(g * (double)va.m_data[0] + f * (double)vb.m_data[0]);
1353  vc.m_data[1] =
1354  (uint16_t)(g * (double)va.m_data[1] + f * (double)vb.m_data[1]);
1355  vc.m_data[2] =
1356  (uint16_t)(g * (double)va.m_data[2] + f * (double)vb.m_data[2]);
1357  updateRawAcc(vc, i);
1358  }
1359 
1362  {
1363  va = pa.getRawGyr(i);
1364  vb = pb.getRawGyr(i);
1365  vc.m_data[0] =
1366  (uint16_t)(g * (double)va.m_data[0] + f * (double)vb.m_data[0]);
1367  vc.m_data[1] =
1368  (uint16_t)(g * (double)va.m_data[1] + f * (double)vb.m_data[1]);
1369  vc.m_data[2] =
1370  (uint16_t)(g * (double)va.m_data[2] + f * (double)vb.m_data[2]);
1371  updateRawGyr(vc, i);
1372  }
1373 
1376  {
1377  va = pa.getRawMag(i);
1378  vb = pb.getRawMag(i);
1379  vc.m_data[0] =
1380  (uint16_t)(g * (double)va.m_data[0] + f * (double)vb.m_data[0]);
1381  vc.m_data[1] =
1382  (uint16_t)(g * (double)va.m_data[1] + f * (double)vb.m_data[1]);
1383  vc.m_data[2] =
1384  (uint16_t)(g * (double)va.m_data[2] + f * (double)vb.m_data[2]);
1385  updateRawMag(vc, i);
1386  }
1387 
1390  {
1391  da = (double)pa.getRawTemp(i);
1392  db = (double)pb.getRawTemp(i);
1393  dc = g * da + f * db;
1394  updateRawTemp((uint16_t)dc, i);
1395  }
1396 
1397  //// calibrated data
1400  {
1401  fa = pa.getCalAcc(i);
1402  fb = pb.getCalAcc(i);
1403  fc.m_data[0] =
1404  (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
1405  fc.m_data[1] =
1406  (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
1407  fc.m_data[2] =
1408  (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
1409  updateCalAcc(fc, i);
1410  }
1411 
1414  {
1415  fa = pa.getCalGyr(i);
1416  fb = pb.getCalGyr(i);
1417  fc.m_data[0] =
1418  (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
1419  fc.m_data[1] =
1420  (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
1421  fc.m_data[2] =
1422  (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
1423  updateCalGyr(fc, i);
1424  }
1425 
1428  {
1429  fa = pa.getCalMag(i);
1430  fb = pb.getCalMag(i);
1431  fc.m_data[0] =
1432  (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
1433  fc.m_data[1] =
1434  (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
1435  fc.m_data[2] =
1436  (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
1437  updateCalMag(fc, i);
1438  }
1439 
1440  //// orientations
1443  {
1444  qf = pa.getOriQuat(i);
1445  qa.isArray(qf.m_data);
1446 
1447  qf = pb.getOriQuat(i);
1448  qb.isArray(qf.m_data);
1449 
1450  qc.isSlerp(qa, qb, f);
1451  qc.setArray(qf.m_data);
1452 
1453  updateOriQuat(qf, i);
1454  }
1455 
1458  {
1459  ea = pa.getOriEuler(i);
1460  eb = pb.getOriEuler(i);
1461  ec.m_roll = (g * (double)ea.m_roll + f * (double)eb.m_roll);
1462  ec.m_pitch = (g * (double)ea.m_pitch + f * (double)eb.m_pitch);
1463  ec.m_yaw = (g * (double)ea.m_yaw + f * (double)eb.m_yaw);
1464  updateOriEuler(ec, i);
1465  }
1466 
1469  {
1470  mf = pa.getOriMatrix(i);
1471  m.isArray(&mf.m_data[0][0]);
1472  qa.isRmat(m);
1473 
1474  mf = pb.getOriMatrix(i);
1475  m.isArray(&mf.m_data[0][0]);
1476  qb.isRmat(m);
1477 
1478  qc.isSlerp(qa, qb, f);
1479  m.isQuat(qc);
1480 
1481  m.setArray(&mf.m_data[0][0]);
1482  updateOriMatrix(mf, i);
1483  }
1484 
1485  //// other
1488  {
1489  sa = pa.getSampleCounter(i);
1490  sb = pb.getSampleCounter(i);
1491  sc = sb - sa;
1492  dc = f * (double)sc;
1493  sc = sa + (uint16_t)doubleToLong(dc);
1494  updateSampleCounter(sc, i);
1495  }
1496 
1499  {
1500  fa = pa.getAccG(i);
1501  fb = pb.getAccG(i);
1502  fc.m_data[0] =
1503  (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
1504  fc.m_data[1] =
1505  (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
1506  fc.m_data[2] =
1507  (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
1508  updateAccG(fc, i);
1509  }
1510  }
1511  // some remaining stuff
1512  // TOA remains equal to pb TOA
1513  m_rtc = (TimeStamp)(g * (double)pa.m_rtc + f * (double)pb.m_rtc);
1514 }
1515 #endif
1516 
1517 } // end of xsens namespace
#define CMT_OUTPUTSETTINGS_DATAFORMAT_FLOAT
Definition: cmtdef.h:636
uint32_t getDataLong(const uint16_t offset=0) const
Return the current value of the data as an uint32_t (32 bits).
Definition: cmtmessage.cpp:322
bool updateOriQuat(const CmtQuat &data, const uint16_t index=0)
Add/update Quaternion Orientation data for the item.
Definition: cmtpacket.cpp:916
bool updatePositionLLA(const CmtVector &data, const uint16_t index=0)
Add/update Position Lat Lon Alt for the item.
Definition: cmtpacket.cpp:1122
double getDataFPValue(const uint64_t outputSettings, const uint16_t offset=0) const
Return current data value as double, conversion depends on outputSettings.
Definition: cmtmessage.cpp:267
bool updateRawTemp(uint16_t temp, const uint16_t index=0)
Add/update Raw Temperature data for the item.
Definition: cmtpacket.cpp:539
void setDataFPValue(const uint64_t outputSettings, const double data, const uint16_t offset=0)
Write a floating/fixed point value into to the data buffer, conversion depends on outputSettings...
Definition: cmtmessage.cpp:639
bool containsCalAcc(const uint16_t index=0) const
Check if data item contains Calibrated Accelerometer data.
Definition: cmtpacket.cpp:740
double getTemp(const uint16_t index=0) const
Return the Temperature component of a data item.
Definition: cmtpacket.cpp:694
CmtVector getPositionLLA(const uint16_t index=0) const
Return the Position Lat Lon Alt component of a data item.
Definition: cmtpacket.cpp:1105
TimeStamp m_toa
Time of arrival.
Definition: cmtpacket.h:82
#define CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632
Definition: cmtdef.h:638
unsigned __int16 uint16_t
Definition: rptypes.h:44
#define CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX
Definition: cmtdef.h:628
A structure for storing data formats.
Definition: cmtdef.h:907
bool containsAccG(const uint16_t index=0) const
Check if data item contains XKF-3 Acc-G data.
Definition: cmtpacket.cpp:1262
#define CMT_OUTPUTSETTINGS_CALIBMODE_MAG_MASK
Definition: cmtdef.h:651
CmtRawData getRawData(const uint16_t index=0) const
Return the Raw Data component of a data item.
Definition: cmtpacket.cpp:556
bool updateAnalogIn1(const CmtAnalogInData &data, const uint16_t index=0)
Add/update AnalogIn 1 for the item.
Definition: cmtpacket.cpp:1056
void resizeData(const uint16_t newSize)
Resize the data area to the given size.
Definition: cmtmessage.cpp:424
CmtVector getAccG(const uint16_t index=0) const
Return the XKF-3 Acc-G component of the packet.
Definition: cmtpacket.cpp:1250
GLuint buffer
Definition: glext.h:3917
bool containsAnalogIn2(const uint16_t index=0) const
Check if data item contains AnalogIn 2.
Definition: cmtpacket.cpp:1081
void deleteData(uint16_t size, uint16_t offset=0)
Remove a number of bytes from the message (this will reduce the message size)
Definition: cmtmessage.cpp:752
bool containsRawAcc(const uint16_t index=0) const
Check if data item contains Raw Accelerometer data.
Definition: cmtpacket.cpp:434
CmtVector getVelocity(const uint16_t index=0) const
Return the Velocity component of a data item.
Definition: cmtpacket.cpp:1145
T interpolate(const T &x, const VECTOR &ys, const T &x0, const T &x1)
Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximat...
Definition: interp_fit.hpp:17
#define CMT_OUTPUTMODE_STATUS
Definition: cmtdef.h:620
bool updateCalData(const CmtCalData &data, const uint16_t index=0)
Add/update Calibrated Data for the item.
Definition: cmtpacket.cpp:868
uint16_t getDataSize(void) const
Return the length of the data part of the message.
Definition: cmtmessage.cpp:352
for(ctr=DCTSIZE;ctr > 0;ctr--)
Definition: jidctflt.cpp:56
CmtDataFormat getDataFormat(const uint16_t index=0) const
Definition: cmtpacket.cpp:69
uint16_t getRawTemp(const uint16_t index=0) const
Return the Raw Temperature component of a data item.
Definition: cmtpacket.cpp:526
double m_pitch
The pitch (rotation around y-axis / right-left-line)
Definition: cmtdef.h:1266
uint16_t m_itemCount
The number of data items in the message.
Definition: cmtpacket.h:76
bool containsPositionLLA(const uint16_t index=0) const
Check if data item contains Position Lat Lon Alt.
Definition: cmtpacket.cpp:1116
void insertData(uint16_t size, uint16_t offset=0)
Insert a number of bytes into the message (this will increase the message size)
Definition: cmtmessage.cpp:775
uint16_t getSampleCounter(const uint16_t index=0) const
Return the Sample Counter component of the packet.
Definition: cmtpacket.cpp:1217
CmtRawGpsData getRawGpsData(const uint16_t index=0) const
Return the Raw GpsPrint Data component of a data item. TODO: Implement in COM interface!!! ...
Definition: cmtpacket.cpp:602
bool updateRawAcc(const CmtShortVector &vec, const uint16_t index=0)
Add/update Raw Accelerometer data for the item.
Definition: cmtpacket.cpp:440
#define CMT_OUTPUTSETTINGS_POSITIONMODE_LLA_WGS84
Definition: cmtdef.h:642
#define CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
Definition: cmtdef.h:625
#define CMT_OUTPUTMODE_CALIB
Definition: cmtdef.h:615
#define CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE
Definition: cmtdef.h:639
#define CMT_OUTPUTMODE_VELOCITY
Definition: cmtdef.h:619
uint16_t getDataShort(const uint16_t offset=0) const
Return the current value of the data as an uint16_t (16 bits).
Definition: cmtmessage.cpp:338
#define CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG
Definition: cmtdef.h:629
#define CMT_OUTPUTMODE_RAW
Definition: cmtdef.h:612
bool containsCalData(const uint16_t index=0) const
Check if data item contains Calibrated Data.
Definition: cmtpacket.cpp:861
bool containsRawGpsData(const uint16_t index=0) const
Check if data item contains Raw GpsPrint Data.
Definition: cmtpacket.cpp:632
#define CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN2_MASK
Definition: cmtdef.h:655
A structure containing MT data + timestamp and formatting information.
Definition: cmtpacket.h:24
unsigned char uint8_t
Definition: rptypes.h:41
bool updateRawGpsData(const CmtRawGpsData &data, const uint16_t index=0)
Add/update Raw GpsPrint Data for the item.
Definition: cmtpacket.cpp:639
bool containsRawTemp(const uint16_t index=0) const
Check if data item contains Raw Temperature data.
Definition: cmtpacket.cpp:532
bool containsOriMatrix(const uint16_t index=0) const
Check if data item contains Matrix Orientation data.
Definition: cmtpacket.cpp:1009
TimeStamp m_rtc
Sample time in ms, based on the sample counter.
Definition: cmtpacket.h:80
CmtDataFormat * m_formatList
A list of the formats of the data items.
Definition: cmtpacket.h:70
struct xsens::Packet::PacketInfo * m_infoList
bool updateOriEuler(const CmtEuler &data, const uint16_t index=0)
Add/update Euler Orientation data for the item.
Definition: cmtpacket.cpp:962
bool updateCalGyr(const CmtVector &vec, const uint16_t index=0)
Add/update Calibrated Gyroscope data for the item.
Definition: cmtpacket.cpp:786
bool containsCalGyr(const uint16_t index=0) const
Check if data item contains Calibrated Gyroscope data.
Definition: cmtpacket.cpp:780
__int16 int16_t
Definition: rptypes.h:43
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
CmtVector getCalGyr(const uint16_t index=0) const
Return the Calibrated Gyroscope component of a data item.
Definition: cmtpacket.cpp:769
bool updateVelocity(const CmtVector &data, const uint16_t index=0)
Add/update Velocity for the item.
Definition: cmtpacket.cpp:1164
GLuint index
Definition: glext.h:4054
bool updateAccG(const CmtVector &g, const uint16_t index=0)
Add/update XKF-3 Acc-G data for the item.
Definition: cmtpacket.cpp:1268
bool containsCalMag(const uint16_t index=0) const
Check if data item contains Calibrated Magnetometer data.
Definition: cmtpacket.cpp:820
CmtQuat getOriQuat(const uint16_t index=0) const
Return the Orientation component of a data item as a Quaternion.
Definition: cmtpacket.cpp:897
bool updateCalAcc(const CmtVector &vec, const uint16_t index=0)
Add/update Calibrated Accelerometer data for the item.
Definition: cmtpacket.cpp:746
#define CMT_OUTPUTSETTINGS_TIMESTAMP_MASK
Definition: cmtdef.h:647
bool m_xm
Indicates that xbus-formatting is used.
Definition: cmtpacket.h:72
double m_data[4]
Definition: cmtdef.h:1259
GLubyte g
Definition: glext.h:6279
bool containsRawGyr(const uint16_t index=0) const
Check if data item contains Raw Gyroscope data.
Definition: cmtpacket.cpp:468
CmtShortVector getRawAcc(const uint16_t index=0) const
Return the Raw Accelerometer component of a data item.
Definition: cmtpacket.cpp:424
double m_data[3][3]
Definition: cmtdef.h:1272
double m_roll
The roll (rotation around x-axis / back-front-line)
Definition: cmtdef.h:1264
#define CMT_OUTPUTSETTINGS_VELOCITYMODE_MASK
Definition: cmtdef.h:658
bool updateTemp(const double &temp, const uint16_t index=0)
Add/update Calibrated Accelerometer data for the item.
Definition: cmtpacket.cpp:708
uint16_t m_rawGpsPressureAge
Definition: cmtpacket.h:54
bool containsOriQuat(const uint16_t index=0) const
Check if data item contains Quaternion Orientation data.
Definition: cmtpacket.cpp:909
bool containsTemp(const uint16_t index=0) const
Check if data item contains Temperature data.
Definition: cmtpacket.cpp:702
#define CMT_OUTPUTSETTINGS_POSITIONMODE_MASK
Definition: cmtdef.h:657
uint64_t TimeStamp
A real-time timestamp (ms)
Definition: xsens_time.h:24
uint16_t getDataSize(const uint16_t index=0) const
Return the data size.
Definition: cmtpacket.cpp:178
#define CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION
Definition: cmtdef.h:626
CmtMatrix getOriMatrix(const uint16_t index=0) const
Return the Orientation component of a data item as an Orientation Matrix.
Definition: cmtpacket.cpp:991
#define CMT_OUTPUTSETTINGS_DATAFORMAT_MASK
Definition: cmtdef.h:653
void setDataLong(const uint32_t data, const uint16_t offset=0)
Write an uint32_t (32 bits) into the data buffer.
Definition: cmtmessage.cpp:690
#define CMT_OUTPUTSETTINGS_ORIENTMODE_EULER
Definition: cmtdef.h:627
uint16_t m_data[3]
Definition: cmtdef.h:1221
Contains information about data in the packet and the format of that data.
Definition: cmtpacket.h:29
CmtShortVector getRawGyr(const uint16_t index=0) const
Return the Raw Gyroscope component of a data item.
Definition: cmtpacket.cpp:458
double m_data[3]
Definition: cmtdef.h:1251
#define CMT_DATA_ITEM_NOT_AVAILABLE
Indicates that a data item is not available in the packet.
Definition: cmtpacket.h:21
#define CMT_OUTPUTSETTINGS_CALIBMODE_ACC_MASK
Definition: cmtdef.h:649
uint16_t getFPValueSize(const uint16_t index) const
Return the floating/fixed point value size.
Definition: cmtpacket.cpp:148
uint8_t * getDataBuffer(const uint16_t offset=0)
Return a pointer to the data buffer.
Definition: cmtmessage.h:157
__int32 int32_t
Definition: rptypes.h:46
#define CMT_OUTPUTSETTINGS_DATAFORMAT_F1220
Definition: cmtdef.h:637
bool containsRawMag(const uint16_t index=0) const
Check if data item contains Raw Magnetometer data.
Definition: cmtpacket.cpp:502
bool containsVelocity(const uint16_t index=0) const
Check if data item contains Velocity.
Definition: cmtpacket.cpp:1157
bool setDataFormat(const CmtDataFormat &format, const uint16_t index=0)
Definition: cmtpacket.cpp:80
bool updateOriMatrix(const CmtMatrix &data, const uint16_t index=0)
Add/update Matrix Orientation data for the item.
Definition: cmtpacket.cpp:1015
#define CMT_OUTPUTSETTINGS_VELOCITYMODE_NED
Definition: cmtdef.h:643
bool updateRawMag(const CmtShortVector &vec, const uint16_t index=0)
Add/update Raw Magnetometer data for the item.
Definition: cmtpacket.cpp:508
#define CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN1_MASK
Definition: cmtdef.h:654
#define CMT_OUTPUTMODE_RAWGPSPRINT
Definition: cmtdef.h:613
bool updateSampleCounter(uint16_t counter, const uint16_t index=0)
Add/update Sample Counter for all items.
Definition: cmtpacket.cpp:1228
#define CMT_OUTPUTMODE_TEMP
Definition: cmtdef.h:614
uint32_t CmtOutputMode
An output mode bit-field.
Definition: cmtdef.h:902
bool containsOriEuler(const uint16_t index=0) const
Check if data item contains Euler Orientation data.
Definition: cmtpacket.cpp:956
uint8_t getDataByte(const uint16_t offset=0) const
Return the current value of the data as an unsigned byte (8 bits).
Definition: cmtmessage.h:171
bool containsAnalogIn1(const uint16_t index=0) const
Check if data item contains AnalogIn 1.
Definition: cmtpacket.cpp:1049
void setXbus(bool xbus, bool convert=false)
Definition: cmtpacket.cpp:109
bool containsStatus(const uint16_t index=0) const
Check if data item contains Velocity.
Definition: cmtpacket.cpp:1193
CmtOutputMode m_outputMode
Definition: cmtdef.h:909
The namespace of all Xsens software since 2006.
Definition: cmt1.cpp:95
#define CMT_OUTPUTSETTINGS_CALIBMODE_GYR_MASK
Definition: cmtdef.h:650
double m_yaw
The yaw (rotation around z-axis / down-up-line)
Definition: cmtdef.h:1268
bool updateRawData(const CmtRawData &data, const uint16_t index=0)
Add/update Raw Data for the item.
Definition: cmtpacket.cpp:579
GLenum GLsizei GLenum format
Definition: glext.h:3531
CmtOutputSettings m_outputSettings
Definition: cmtdef.h:910
CmtVector getCalMag(const uint16_t index=0) const
Return the Calibrated Magnetometer component of a data item.
Definition: cmtpacket.cpp:809
bool updateCalMag(const CmtVector &vec, const uint16_t index=0)
Add/update Calibrated Magnetometer data for the item.
Definition: cmtpacket.cpp:826
Packet(uint16_t items, bool xbus)
Definition: cmtpacket.cpp:48
bool updateAnalogIn2(const CmtAnalogInData &data, const uint16_t index=0)
Add/update AnalogIn 2 for the item.
Definition: cmtpacket.cpp:1088
bool getXbus(void) const
Definition: cmtpacket.cpp:107
bool containsSampleCounter(const uint16_t index=0) const
Check if data item contains Sample Counter.
Definition: cmtpacket.cpp:1222
CmtTimeStamp getRtc(const uint16_t index=0) const
Return the RTC of the packet.
Definition: cmtpacket.cpp:1242
#define CMT_OUTPUTMODE_AUXILIARY
Definition: cmtdef.h:617
bool m_autoUpdateChecksum
Definition: cmtmessage.h:105
#define CMT_OUTPUTSETTINGS_ORIENTMODE_MASK
Definition: cmtdef.h:648
CmtAnalogInData getAnalogIn2(const uint16_t index=0) const
Return the AnalogIn 2 component of a data item.
Definition: cmtpacket.cpp:1073
bool containsRawData(const uint16_t index=0) const
Check if data item contains Raw Data.
Definition: cmtpacket.cpp:572
CmtVector getCalAcc(const uint16_t index=0) const
Return the Calibrated Accelerometer component of a data item.
Definition: cmtpacket.cpp:729
unsigned __int32 uint32_t
Definition: rptypes.h:47
#define CMT_OUTPUTMODE_ORIENT
Definition: cmtdef.h:616
#define swapEndian16(src)
Definition: cmtmessage.h:76
CmtAnalogInData getAnalogIn1(const uint16_t index=0) const
Return the AnalogIn 1 component of a data item.
Definition: cmtpacket.cpp:1041
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
CmtCalData getCalData(const uint16_t index=0) const
Return the Calibrated Data component of a data item.
Definition: cmtpacket.cpp:849
CmtShortVector getRawMag(const uint16_t index=0) const
Return the Raw Magnetometer component of a data item.
Definition: cmtpacket.cpp:492
#define CMT_OUTPUTMODE_POSITION
Definition: cmtdef.h:618
#define PACKETLOG(...)
Definition: cmtpacket.cpp:37
uint16_t CmtMtTimeStamp
An MT timestamp (sample count)
Definition: cmtdef.h:924
void setDataByte(const uint8_t data, const uint16_t offset=0)
Write an unsigned byte (8 bits) into the data buffer.
Definition: cmtmessage.cpp:523
bool updateRawGyr(const CmtShortVector &vec, const uint16_t index=0)
Add/update Raw Gyroscope data for the item.
Definition: cmtpacket.cpp:474
uint64_t CmtOutputSettings
An output settings bit-field.
Definition: cmtdef.h:904
void setDataShort(const uint16_t data, const uint16_t offset=0)
Write an uint16_t (16 bits) into the data buffer.
Definition: cmtmessage.cpp:710
void operator=(const Packet &pack)
Definition: cmtpacket.cpp:1299
CmtEuler getOriEuler(const uint16_t index=0) const
Return the Orientation component of a data item as Euler angles.
Definition: cmtpacket.cpp:939
Message m_msg
The message.
Definition: cmtpacket.h:78
bool updateStatus(const uint8_t data, const uint16_t index=0)
Add/update Status information for the item.
Definition: cmtpacket.cpp:1199
uint8_t getStatus(const uint16_t index=0) const
Return the Status component of a data item.
Definition: cmtpacket.cpp:1187



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019