Main MRPT website > C++ reference for MRPT 1.5.7
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 
41 namespace xsens {
42 
43 //////////////////////////////////////////////////////////////////////////////////////////
44 ////////////////////////////////////// Packet class //////////////////////////////////////
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 = NULL;
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 == NULL)
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 //////////////////////////////////////////////////////////////////////////////////////////
92 bool Packet::setDataFormat(const CmtOutputMode outputMode, const CmtOutputSettings outputSettings, const uint16_t index)
93 {
94  if (index < m_itemCount)
95  {
96  m_formatList[index].m_outputMode = outputMode;
97  m_formatList[index].m_outputSettings = outputSettings;
98  LSTCHKDELNUL(m_infoList);
99  return true;
100  }
101  return false;
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////
105 bool Packet::getXbus(void) const
106 {
107  return m_xm;
108 }
109 
110 //////////////////////////////////////////////////////////////////////////////////////////
111 void Packet::setXbus(bool xbus, bool convert)
112 {
113  if (xbus != m_xm)
114  {
115  if (convert)
116  {
117  CmtMtTimeStamp stamp = getSampleCounter(0);
118 
119  // move the time stamp value(s) around
120  if (xbus)
121  {
122  // new version is Xbus, so old version is regular format -> place timestamp at start of packet and remove from all 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 -> place timestamp at end of all individual data units and remove from start of packet
132  // append the timestamp to all data units
133 
134  // remove timestamp from the start
135  m_msg.deleteData(2,0);
136  }
137  }
138  // update the state cache
139  m_xm = xbus;
140  LSTCHKDELNUL(m_infoList);
141  }
142 }
143 
144 //////////////////////////////////////////////////////////////////////////////////////////
145 // Return the floating/fixed point value size
147 {
148  uint16_t ds;
149  switch (m_formatList[index].m_outputSettings & CMT_OUTPUTSETTINGS_DATAFORMAT_MASK)
150  {
152  ds = 4;
153  break;
154 
156  ds = 8;
157  break;
159  ds = 6;
160  break;
161 
163  ds = 4;
164  break;
165 
166  default:
167  ds = 0;
168  break;
169  }
170  return ds;
171 }
172 
173 //////////////////////////////////////////////////////////////////////////////////////////
174 // Return the data size.
176 {
177  if (m_infoList == NULL)
178  {
179  // allocate list
181  uint16_t totalOffset;
182  if (m_xm)
183  totalOffset = 2;
184  else
185  totalOffset = 0;
186 
187  // fill lists
188  for (uint16_t i = 0;i < m_itemCount;++i)
189  {
190  m_infoList[i].m_offset = totalOffset;
191  m_infoList[i].m_size = 0;
192 
193  uint16_t ds = getFPValueSize(i);
194 
195  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_RAW)
196  {
197  m_infoList[i].m_rawData = totalOffset;
198  m_infoList[i].m_rawAcc = totalOffset;
199  m_infoList[i].m_rawGyr = totalOffset+6;
200  m_infoList[i].m_rawMag = totalOffset+12;
201  m_infoList[i].m_rawTemp = totalOffset+18;
202 
203  m_infoList[i].m_size += 20;
204  totalOffset += 20;
205  }
206  else
207  {
213  }
214 
215 
216  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_RAWGPSPRINT)
217  {
218  m_infoList[i].m_rawGpsData = totalOffset;
219  m_infoList[i].m_rawGpsPressure = totalOffset;
220  m_infoList[i].m_rawGpsPressureAge = totalOffset + 2;
221  m_infoList[i].m_size += 3;
222  totalOffset += 3;
223 
224  m_infoList[i].m_rawGpsGpsData = totalOffset;
225  m_infoList[i].m_rawGpsItow = totalOffset;
226  m_infoList[i].m_rawGpsLatitude = totalOffset + 4;
227  m_infoList[i].m_rawGpsLongitude = totalOffset + 8;
228  m_infoList[i].m_rawGpsHeight = totalOffset + 12;
229  m_infoList[i].m_rawGpsVeln = totalOffset + 16;
230  m_infoList[i].m_rawGpsVele = totalOffset + 20;
231  m_infoList[i].m_rawGpsVeld = totalOffset + 24;
232  m_infoList[i].m_rawGpsHacc = totalOffset + 28;
233  m_infoList[i].m_rawGpsVacc = totalOffset + 32;
234  m_infoList[i].m_rawGpsSacc = totalOffset + 36;
235  m_infoList[i].m_rawGpsGpsAge = totalOffset + 40;
236  m_infoList[i].m_size += 41;
237  totalOffset += 41;
238  }
239  else
240  {
244 
257  }
258 
260  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_TEMP)
261  {
262  m_infoList[i].m_temp = totalOffset;
263  m_infoList[i].m_size += ds;
264  totalOffset += ds;
265  }
266 
267  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_CALIB)
268  {
269  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
270  {
271  m_infoList[i].m_calAcc = totalOffset;
272  m_infoList[i].m_size += 3*ds;
273  totalOffset += 3*ds;
274  }
275  else
277 
278  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
279  {
280  m_infoList[i].m_calGyr = totalOffset;
281  m_infoList[i].m_size += 3*ds;
282  totalOffset += 3*ds;
283  }
284  else
286 
287  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
288  {
289  m_infoList[i].m_calMag = totalOffset;
290  m_infoList[i].m_size += 3*ds;
291  totalOffset += 3*ds;
292  }
293  else
295 
296  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG) == 0)
298  else
300  }
301  else
302  {
307  }
308 
312 
313  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_ORIENT)
314  {
315  switch (m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK)
316  {
318  m_infoList[i].m_oriEul = totalOffset;
319  m_infoList[i].m_size += 3*ds;
320  totalOffset += 3*ds;
321  break;
323  m_infoList[i].m_oriQuat = totalOffset;
324  m_infoList[i].m_size += 4*ds;
325  totalOffset += 4*ds;
326  break;
328  m_infoList[i].m_oriMat = totalOffset;
329  m_infoList[i].m_size += 9*ds;
330  totalOffset += 9*ds;
331  break;
332  default:
333  break;
334  }
335  }
336 
338  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_AUXILIARY)
339  {
340  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN1_MASK) == 0)
341  {
342  m_infoList[i].m_analogIn1 = totalOffset;
343  m_infoList[i].m_size += 2;
344  totalOffset += 2;
345  }
346  else
348 
349  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN2_MASK) == 0)
350  {
351  m_infoList[i].m_analogIn2 = totalOffset;
352  m_infoList[i].m_size += 2;
353  totalOffset += 2;
354  }
355  else
357  }
358 
360  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_POSITION)
361  {
362  switch (m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_POSITIONMODE_MASK)
363  {
365  m_infoList[i].m_posLLA = totalOffset;
366  m_infoList[i].m_size += 3*ds;
367  totalOffset += 3*ds;
368  }
369  }
370 
372  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_VELOCITY)
373  {
374  switch (m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_VELOCITYMODE_MASK)
375  {
377  m_infoList[i].m_velNEDorNWU = totalOffset;
378  m_infoList[i].m_size += 3*ds;
379  totalOffset += 3*ds;
380  }
381  }
382 
384  if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_STATUS)
385  {
386  m_infoList[i].m_status = totalOffset;
387  m_infoList[i].m_size += 1;
388  totalOffset += 1;
389  }
390 
392  if (m_xm)
393  m_infoList[i].m_sc = 0;
394  if ((m_formatList[i].m_outputSettings & CMT_OUTPUTSETTINGS_TIMESTAMP_MASK) ==
396  {
397  if (!m_xm)
398  m_infoList[i].m_sc = totalOffset;
399  m_infoList[i].m_size += 2;
400  totalOffset += 2;
401  }
402 
403  // post-processing data is never available at this point
405  }
406  }
407 
408  if (index < m_itemCount)
409  return m_infoList[index].m_size;
410  return 0;
411 }
412 
413 //////////////////////////////////////////////////////////////////////////////////////////
414 // Return the Raw Accelerometer component of a data item.
416 {
418  if (containsRawAcc(index))
419  for (uint16_t i=0;i<3;++i)
420  buffer.m_data[i] = m_msg.getDataShort(m_infoList[index].m_rawAcc + (2*i));
421 
422  return buffer;
423 }
425 {
426  if (getDataSize(index) == 0)
427  return false;
429  return false;
430  return true;
431 }
433 {
434  if (getDataSize(index) == 0)
435  return false;
437  {
438  // add
441  m_infoList[index].m_size += 3*2;
442  }
443  // update
444  for (uint16_t i=0;i<3;++i)
445  m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawAcc + (2*i));
446  return true;
447 }
448 
449 //////////////////////////////////////////////////////////////////////////////////////////
450 // Return the Raw Gyroscope component of a data item.
452 {
454  if (containsRawGyr(index))
455  for (uint16_t i=0;i<3;++i)
456  buffer.m_data[i] = m_msg.getDataShort(m_infoList[index].m_rawGyr + (2*i));
457 
458  return buffer;
459 }
461 {
462  if (getDataSize(index) == 0)
463  return false;
465  return false;
466  return true;
467 }
469 {
470  if (getDataSize(index) == 0)
471  return false;
473  {
474  // add
477  m_infoList[index].m_size += 3*2;
478  }
479  // update
480  for (uint16_t i=0;i<3;++i)
481  m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawGyr + (2*i));
482  return true;
483 }
484 
485 //////////////////////////////////////////////////////////////////////////////////////////
486 // Return the Raw Magnetometer component of a data item.
488 {
490  if (containsRawMag(index))
491  for (uint16_t i=0;i<3;++i)
492  buffer.m_data[i] = m_msg.getDataShort(m_infoList[index].m_rawMag + (2*i));
493 
494  return buffer;
495 }
497 {
498  if (getDataSize(index) == 0)
499  return false;
501  return false;
502  return true;
503 }
505 {
506  if (getDataSize(index) == 0)
507  return false;
509  {
510  // add
513  m_infoList[index].m_size += 3*2;
514  }
515  // update
516  for (uint16_t i=0;i<3;++i)
517  m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawMag + (2*i));
518  return true;
519 }
520 
521 //////////////////////////////////////////////////////////////////////////////////////////
522 // Return the Raw Temperature component of a data item.
524 {
525  if (!containsRawTemp(index))
526  return 0;
527 
528  return m_msg.getDataShort(m_infoList[index].m_rawTemp);
529 }
531 {
532  if (getDataSize(index) == 0)
533  return false;
534  if (m_infoList[index].m_rawTemp == CMT_DATA_ITEM_NOT_AVAILABLE)
535  return false;
536  return true;
537 }
539 {
540  if (getDataSize(index) == 0)
541  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(*sh);// m_msg.getDataShort(m_infoList[index].m_rawData + (2*i));
567  }
568  return buffer;
569 }
571 {
572  if (getDataSize(index) == 0)
573  return false;
574  if (m_infoList[index].m_rawData == CMT_DATA_ITEM_NOT_AVAILABLE)
575  return false;
576  return true;
577 }
579 {
580  if (getDataSize(index) == 0)
581  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 = m_msg.getDataBuffer(m_infoList[index].m_rawGpsData);
608  //const uint16_t* sh = (const uint16_t*) tmp;
609  //uint16_t* bare = (uint16_t*) &buffer;
610 
611  // pressure data
612  buffer.m_pressure = m_msg.getDataShort(m_infoList[index].m_rawGpsPressure);
613  // pressAge
614  buffer.m_pressureAge = m_msg.getDataByte(m_infoList[index].m_rawGpsPressureAge);
615 
616  // lon,lat,height,hacc,vacc,veln,vele,veld
617  //tmp = m_msg.getDataBuffer(m_infoList[index].m_rawGpsGpsData);
618  //const uint32_t* ln = (const uint32_t*) tmp;
619  uint32_t *bareln = (uint32_t*) &buffer.m_itow;
620  for (uint16_t i=0; i < 10; ++i)
621  bareln[i] = m_msg.getDataLong(m_infoList[index].m_rawGpsGpsData + (4*i));
622 
623  // gpsAge
624  buffer.m_gpsAge = m_msg.getDataByte(m_infoList[index].m_rawGpsGpsAge);
625  }
626  return buffer;
627 }
629 {
630  if (getDataSize(index) == 0)
631  return false;
632  if (m_infoList[index].m_rawGpsData == CMT_DATA_ITEM_NOT_AVAILABLE)
633  return false;
634  return true;
635 }
637 {
638  if (getDataSize(index) == 0)
639  return false;
640  if (m_infoList[index].m_rawGpsData == CMT_DATA_ITEM_NOT_AVAILABLE)
641  {
642  // add
644  m_msg.resizeData(m_msg.getDataSize() + (2+1) + (40 + 1));
647 
660 
661  m_infoList[index].m_size += (2+1) + (40 + 1);
662  }
663  // update
666 
667  // lon,lat,height,hacc,vacc,veln,vele,veld
668  int32_t* bareln = (int32_t*)&data.m_itow;
669  for (uint16_t i=0; i<10;++i)
670  m_msg.setDataLong(bareln[i],m_infoList[index].m_rawGpsGpsData + (4*i));
671 
672  // gpsAge
674  return true;
675 }
676 
677 //////////////////////////////////////////////////////////////////////////////////////////
678 // Return the Temperature component of a data item.
679 double Packet::getTemp(const uint16_t index) const
680 {
681  if (containsTemp(index))
682  return m_msg.getDataFPValue(m_formatList[index].m_outputSettings, m_infoList[index].m_temp);
683 
684  return 0.0;
685 }
687 {
688  if (getDataSize(index) == 0)
689  return false;
691  return false;
692  return true;
693 }
694 bool Packet::updateTemp(const double& temp, const uint16_t index)
695 {
696  if (getDataSize(index) == 0)
697  return false;
698 
700 
702  {
703  // add
706  m_infoList[index].m_size += ds;
707  }
708  // update
709  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, temp, m_infoList[index].m_temp);
710  return true;
711 }
712 
713 //////////////////////////////////////////////////////////////////////////////////////////
714 // Return the Calibrated Accelerometer component of a data item.
716 {
718  if (containsCalAcc(index))
720  else
721  memset(&buffer, 0, sizeof(buffer));
722  return buffer;
723 }
725 {
726  if (getDataSize(index) == 0)
727  return false;
729  return false;
730  return true;
731 }
733 {
734  const uint16_t numValues = 3;
735  if (getDataSize(index) == 0)
736  return false;
737 
739 
741  {
742  // add
744  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
745  m_infoList[index].m_size += numValues*ds;
746  }
747  // update
748  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &vec.m_data[0], m_infoList[index].m_calAcc, numValues);
749  return true;
750 }
751 
752 //////////////////////////////////////////////////////////////////////////////////////////
753 // Return the Calibrated Gyroscope component of a data item.
755 {
757  if (containsCalGyr(index))
759  else
760  memset(&buffer, 0, sizeof(buffer));
761  return buffer;
762 }
764 {
765  if (getDataSize(index) == 0)
766  return false;
768  return false;
769  return true;
770 }
772 {
773  const uint16_t numValues = 3;
774  if (getDataSize(index) == 0)
775  return false;
776 
778 
780  {
781  // add
783  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
784  m_infoList[index].m_size += numValues*ds;
785  }
786  // update
787  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &vec.m_data[0], m_infoList[index].m_calGyr, numValues);
788  return true;
789 }
790 
791 //////////////////////////////////////////////////////////////////////////////////////////
792 // Return the Calibrated Magnetometer component of a data item.
794 {
796  if (containsCalMag(index))
798  else
799  memset(&buffer, 0, sizeof(buffer));
800  return buffer;
801 }
803 {
804  if (getDataSize(index) == 0)
805  return false;
807  return false;
808  return true;
809 }
811 {
812  const uint16_t numValues = 3;
813  if (getDataSize(index) == 0)
814  return false;
815 
817 
819  {
820  // add
822  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
823  m_infoList[index].m_size += numValues*ds;
824  }
825  // update
826  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &vec.m_data[0], m_infoList[index].m_calMag, numValues);
827  return true;
828 }
829 
830 //////////////////////////////////////////////////////////////////////////////////////////
831 // Return the Calibrated Accelerometer component of a data item.
833 {
835  double* bare = (double*) &buffer;
836  if (containsCalData(index))
838  else
839  memset(&buffer, 0, sizeof(buffer));
840  return buffer;
841 }
843 {
844  if (getDataSize(index) == 0)
845  return false;
846  if (m_infoList[index].m_calData == CMT_DATA_ITEM_NOT_AVAILABLE)
847  return false;
848  return true;
849 }
851 {
852  const uint16_t numValues = 9;
853  if (getDataSize(index) == 0)
854  return false;
855 
857 
858  if (m_infoList[index].m_calData == CMT_DATA_ITEM_NOT_AVAILABLE)
859  {
860  // add
861  m_msg.m_autoUpdateChecksum = false;
863  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
867  m_infoList[index].m_size += numValues*ds;
868  }
869  // update
870 
871  double* bare = (double*) &data;
872  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, bare, m_infoList[index].m_calData, numValues);
873  return true;
874 }
875 
876 //////////////////////////////////////////////////////////////////////////////////////////
877 // Return the Orientation component of a data item as a Quaternion.
879 {
880  CmtQuat buffer;
881  if (containsOriQuat(index))
883  else
884  memset(&buffer, 0, sizeof(buffer));
885  return buffer;
886 }
887 
889 {
890  if (getDataSize(index) == 0)
891  return false;
892  if (m_infoList[index].m_oriQuat == CMT_DATA_ITEM_NOT_AVAILABLE)
893  return false;
894  return true;
895 }
897 {
898  const uint16_t numValues = 4;
899  if (getDataSize(index) == 0)
900  return false;
901 
903 
904  if (m_infoList[index].m_oriQuat == CMT_DATA_ITEM_NOT_AVAILABLE)
905  {
906  // add
908  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
909  m_infoList[index].m_size += numValues*ds;
910  }
911  // update
912  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &data.m_data[0], m_infoList[index].m_oriQuat, numValues);
913  return true;
914 }
915 
916 //////////////////////////////////////////////////////////////////////////////////////////
917 // Return the Orientation component of a data item as CmtEuler angles.
919 {
921  if (containsOriEuler(index))
922  {
924  buffer.m_roll = m_msg.getDataFPValue(m_formatList[index].m_outputSettings, m_infoList[index].m_oriEul);
925  buffer.m_pitch = m_msg.getDataFPValue(m_formatList[index].m_outputSettings, m_infoList[index].m_oriEul + ds);
926  buffer.m_yaw = m_msg.getDataFPValue(m_formatList[index].m_outputSettings, m_infoList[index].m_oriEul + 2*ds);
927  }
928  return buffer;
929 }
931 {
932  if (getDataSize(index) == 0)
933  return false;
935  return false;
936  return true;
937 }
939 {
940  const uint16_t numValues = 3;
941  if (getDataSize(index) == 0)
942  return false;
943 
945 
947  {
948  // add
950  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
951  m_infoList[index].m_size += numValues*ds;
952  }
953  // update
954  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, data.m_roll, m_infoList[index].m_oriEul);
955  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, data.m_pitch, m_infoList[index].m_oriEul + ds);
956  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, data.m_yaw, m_infoList[index].m_oriEul + 2*ds);
957  return true;
958 }
959 
960 //////////////////////////////////////////////////////////////////////////////////////////
961 // Return the Orientation component of a data item as an Orientation Matrix.
963 {
965  uint16_t k = 0;
967  {
969  for (int32_t i=0;i<3;++i)
970  for (int32_t j=0;j<3;++j, k+=ds)
972  }
973  else
974  memset(&buffer, 0, sizeof(buffer));
975 
976  return buffer;
977 }
979 {
980  if (getDataSize(index) == 0)
981  return false;
983  return false;
984  return true;
985 }
987 {
988  const uint16_t numValues = 9;
989  if (getDataSize(index) == 0)
990  return false;
991 
993 
995  {
996  // add
998  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
999  m_infoList[index].m_size += numValues*ds;
1000  }
1001  // update
1002  uint16_t k = 0;
1003  for (int32_t i=0;i<3;++i)
1004  for (int32_t j=0;j<3;++j, k+=ds)
1006  return true;
1007 }
1008 
1009 //////////////////////////////////////////////////////////////////////////////////////////
1010 // Return the AnalogIn 1 component of a data item.
1012 {
1014  if (containsAnalogIn1(index))
1015  buffer.m_data = m_msg.getDataShort(m_infoList[index].m_analogIn1);
1016 
1017  return buffer;
1018 }
1020 {
1021  if (getDataSize(index) == 0)
1022  return false;
1023  if (m_infoList[index].m_analogIn1 == CMT_DATA_ITEM_NOT_AVAILABLE)
1024  return false;
1025  return true;
1026 }
1028 {
1029  if (getDataSize(index) == 0)
1030  return false;
1031  if (m_infoList[index].m_analogIn1 == CMT_DATA_ITEM_NOT_AVAILABLE)
1032  {
1033  // add
1036  m_infoList[index].m_size += 2;
1037  }
1038  // update
1040  return true;
1041 }
1042 
1043 //////////////////////////////////////////////////////////////////////////////////////////
1044 // Return the AnalogIn 2 component of a data item.
1046 {
1048  if (containsAnalogIn2(index))
1049  buffer.m_data = m_msg.getDataShort(m_infoList[index].m_analogIn2);
1050 
1051  return buffer;
1052 }
1054 {
1055  if (getDataSize(index) == 0)
1056  return false;
1057  if (m_infoList[index].m_analogIn2 == CMT_DATA_ITEM_NOT_AVAILABLE)
1058  return false;
1059  return true;
1060 }
1062 {
1063  if (getDataSize(index) == 0)
1064  return false;
1065  if (m_infoList[index].m_analogIn2 == CMT_DATA_ITEM_NOT_AVAILABLE)
1066  {
1067  // add
1070  m_infoList[index].m_size += 2;
1071  }
1072  // update
1074  return true;
1075 }
1076 
1077 //////////////////////////////////////////////////////////////////////////////////////////
1078 // Return the Position LLA component of a data item.
1080 {
1081  CmtVector buffer;
1084  else
1085  memset(&buffer, 0, sizeof(buffer));
1086  return buffer;
1087 }
1089 {
1090  if (getDataSize(index) == 0)
1091  return false;
1092  if (m_infoList[index].m_posLLA == CMT_DATA_ITEM_NOT_AVAILABLE)
1093  return false;
1094  return true;
1095 }
1097 {
1098  const uint16_t numValues = 3;
1099  if (getDataSize(index) == 0)
1100  return false;
1101 
1103 
1104  if (m_infoList[index].m_posLLA == CMT_DATA_ITEM_NOT_AVAILABLE)
1105  {
1106  // add
1108  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
1109  m_infoList[index].m_size += numValues*ds;
1110  }
1111  // update
1112  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &data.m_data[0], m_infoList[index].m_posLLA, numValues);
1113  return true;
1114 }
1115 
1116 //////////////////////////////////////////////////////////////////////////////////////////
1117 // Return the Velocity NED component of a data item.
1119 {
1120  CmtVector buffer;
1121  if (containsVelocity(index))
1123  else
1124  memset(&buffer, 0, sizeof(buffer));
1125 
1126  return buffer;
1127 }
1129 {
1130  if (getDataSize(index) == 0)
1131  return false;
1132  if (m_infoList[index].m_velNEDorNWU == CMT_DATA_ITEM_NOT_AVAILABLE)
1133  return false;
1134  return true;
1135 }
1137 {
1138  const uint16_t numValues = 3;
1139  if (getDataSize(index) == 0)
1140  return false;
1141 
1143 
1144  if (m_infoList[index].m_velNEDorNWU == CMT_DATA_ITEM_NOT_AVAILABLE)
1145  {
1146  // add
1148  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
1149  m_infoList[index].m_size += numValues*ds;
1150  }
1151  // update
1152  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &data.m_data[0], m_infoList[index].m_velNEDorNWU, numValues);
1153  return true;
1154 }
1155 
1156 //////////////////////////////////////////////////////////////////////////////////////////
1157 // Return the Status component of a data item.
1159 {
1160  if (containsStatus(index))
1161  return m_msg.getDataByte(m_infoList[index].m_status);
1162  return 0;
1163 }
1165 {
1166  if (getDataSize(index) == 0)
1167  return false;
1168  if (m_infoList[index].m_status == CMT_DATA_ITEM_NOT_AVAILABLE)
1169  return false;
1170  return true;
1171 }
1173 {
1174  if (getDataSize(index) == 0)
1175  return false;
1176 
1177  if (m_infoList[index].m_status == CMT_DATA_ITEM_NOT_AVAILABLE)
1178  {
1179  // add
1182  m_infoList[index].m_size += 1;
1183  }
1184  // update
1185  m_msg.setDataByte(data,m_infoList[index].m_status);
1186  return true;
1187 }
1188 
1189 //////////////////////////////////////////////////////////////////////////////////////////
1190 // Return the Sample Counter component of the packet.
1192 {
1194  return 0;
1195  return m_msg.getDataShort(m_infoList[index].m_sc);
1196 }
1198 {
1199  if (getDataSize(index) == 0)
1200  return false;
1202  return false;
1203  return true;
1204 }
1206 {
1207  if (getDataSize(index) == 0)
1208  return false;
1210  {
1211  // add
1214  m_infoList[index].m_size += 2;
1215  }
1216  // update
1218  return true;
1219 }
1221 {
1223  return m_rtc;
1224 }
1225 
1226 //////////////////////////////////////////////////////////////////////////////////////////
1227 // Return the XKF-3 Acc G component of the packet.
1229 {
1230  CmtVector buffer;
1231  if (containsAccG(index))
1233  else
1234  memset(&buffer, 0, sizeof(buffer));
1235 
1236  return buffer;
1237 }
1239 {
1240  if (getDataSize(index) == 0)
1241  return false;
1243  return false;
1244  return true;
1245 }
1247 {
1248  const uint16_t numValues = 3;
1249  if (getDataSize(index) == 0)
1250  return false;
1251 
1253 
1255  {
1256  // add space
1258  m_msg.resizeData(m_msg.getDataSize() + numValues*ds);
1259  m_infoList[index].m_size += numValues*ds;
1260  }
1261  // update
1262  m_msg.setDataFPValue(m_formatList[index].m_outputSettings, &g.m_data[0], m_infoList[index].m_acc_g, numValues);
1263  return true;
1264 }
1265 
1267 {
1268  PACKETLOG("Create new packet from Packet %p\n",&pack);
1269 
1270  m_itemCount = 0;
1271  m_formatList = NULL;
1272  m_infoList = NULL;
1273  *this = pack;
1274 }
1275 
1276 void Packet::operator = (const Packet& pack)
1277 {
1278  PACKETLOG("Copy packet from Packet %p\n",this);
1279 
1280  if (m_itemCount != pack.m_itemCount)
1281  {
1282  LSTCHKDELNUL(m_formatList);
1283  m_itemCount = pack.m_itemCount;
1285  }
1286  for (uint16_t i = 0; i < m_itemCount; ++i)
1287  m_formatList[i] = pack.m_formatList[i];
1288  LSTCHKDELNUL(m_infoList);
1289  m_toa = pack.m_toa;
1290  m_rtc = pack.m_rtc;
1291  m_msg = pack.m_msg;
1292  m_xm = pack.m_xm;
1293 }
1294 
1295 #ifdef _CMT_DLL_EXPORT
1296 void Packet::interpolate(const Packet& pa, const Packet& pb, const double f)
1297 {
1298  pa.getDataSize();
1299  pb.getDataSize();
1300 
1301  *this = pb;
1302 
1303  if (m_itemCount != pa.m_itemCount)
1304  return;
1305 
1306  getDataSize();
1307  CmtShortVector va, vb, vc;
1308  CmtVector fa, fb, fc;
1309  CmtQuat qf;
1310  CmtEuler ea, eb, ec;
1311  CmtMatrix mf;
1312 
1313  Quaternion qa, qb, qc;
1314  Matrix3x3 m;
1315 
1316  double g = 1.0-f;
1317  double da, db, dc;
1318  uint16_t sa, sb, sc;
1319 
1320  // walk over all items and interpolate all data items
1321  for (uint16_t i = 0; i < m_itemCount; ++i)
1322  {
1323  //// raw data
1325  {
1326  va = pa.getRawAcc(i); vb = pb.getRawAcc(i);
1327  vc.m_data[0] = (uint16_t) (g*(double)va.m_data[0] + f*(double)vb.m_data[0]);
1328  vc.m_data[1] = (uint16_t) (g*(double)va.m_data[1] + f*(double)vb.m_data[1]);
1329  vc.m_data[2] = (uint16_t) (g*(double)va.m_data[2] + f*(double)vb.m_data[2]);
1330  updateRawAcc(vc,i);
1331  }
1332 
1334  {
1335  va = pa.getRawGyr(i); vb = pb.getRawGyr(i);
1336  vc.m_data[0] = (uint16_t) (g*(double)va.m_data[0] + f*(double)vb.m_data[0]);
1337  vc.m_data[1] = (uint16_t) (g*(double)va.m_data[1] + f*(double)vb.m_data[1]);
1338  vc.m_data[2] = (uint16_t) (g*(double)va.m_data[2] + f*(double)vb.m_data[2]);
1339  updateRawGyr(vc,i);
1340  }
1341 
1343  {
1344  va = pa.getRawMag(i); vb = pb.getRawMag(i);
1345  vc.m_data[0] = (uint16_t) (g*(double)va.m_data[0] + f*(double)vb.m_data[0]);
1346  vc.m_data[1] = (uint16_t) (g*(double)va.m_data[1] + f*(double)vb.m_data[1]);
1347  vc.m_data[2] = (uint16_t) (g*(double)va.m_data[2] + f*(double)vb.m_data[2]);
1348  updateRawMag(vc,i);
1349  }
1350 
1352  {
1353  da = (double) pa.getRawTemp(i); db = (double) pb.getRawTemp(i);
1354  dc = g*da + f*db;
1355  updateRawTemp((uint16_t) dc,i);
1356  }
1357 
1358  //// calibrated data
1360  {
1361  fa = pa.getCalAcc(i); fb = pb.getCalAcc(i);
1362  fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
1363  fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
1364  fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
1365  updateCalAcc(fc,i);
1366  }
1367 
1369  {
1370  fa = pa.getCalGyr(i); fb = pb.getCalGyr(i);
1371  fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
1372  fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
1373  fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
1374  updateCalGyr(fc,i);
1375  }
1376 
1378  {
1379  fa = pa.getCalMag(i); fb = pb.getCalMag(i);
1380  fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
1381  fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
1382  fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
1383  updateCalMag(fc,i);
1384  }
1385 
1386  //// orientations
1388  {
1389  qf = pa.getOriQuat(i);
1390  qa.isArray(qf.m_data);
1391 
1392  qf = pb.getOriQuat(i);
1393  qb.isArray(qf.m_data);
1394 
1395  qc.isSlerp(qa,qb,f);
1396  qc.setArray(qf.m_data);
1397 
1398  updateOriQuat(qf,i);
1399  }
1400 
1402  {
1403  ea = pa.getOriEuler(i); eb = pb.getOriEuler(i);
1404  ec.m_roll = (g*(double)ea.m_roll + f*(double)eb.m_roll);
1405  ec.m_pitch = (g*(double)ea.m_pitch + f*(double)eb.m_pitch);
1406  ec.m_yaw = (g*(double)ea.m_yaw + f*(double)eb.m_yaw);
1407  updateOriEuler(ec,i);
1408  }
1409 
1411  {
1412  mf = pa.getOriMatrix(i);
1413  m.isArray(&mf.m_data[0][0]);
1414  qa.isRmat(m);
1415 
1416  mf = pb.getOriMatrix(i);
1417  m.isArray(&mf.m_data[0][0]);
1418  qb.isRmat(m);
1419 
1420  qc.isSlerp(qa,qb,f);
1421  m.isQuat(qc);
1422 
1423  m.setArray(&mf.m_data[0][0]);
1424  updateOriMatrix(mf,i);
1425  }
1426 
1427  //// other
1429  {
1430  sa = pa.getSampleCounter(i); sb = pb.getSampleCounter(i);
1431  sc = sb - sa;
1432  dc = f*(double) sc;
1433  sc = sa + (uint16_t) doubleToLong(dc);
1434  updateSampleCounter(sc,i);
1435  }
1436 
1438  {
1439  fa = pa.getAccG(i); fb = pb.getAccG(i);
1440  fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
1441  fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
1442  fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
1443  updateAccG(fc,i);
1444  }
1445  }
1446  // some remaining stuff
1447  // TOA remains equal to pb TOA
1448  m_rtc = (TimeStamp) (g*(double) pa.m_rtc + f*(double) pb.m_rtc);
1449 }
1450 #endif
1451 
1452 } // 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:296
bool updateOriQuat(const CmtQuat &data, const uint16_t index=0)
Add/update Quaternion Orientation data for the item.
Definition: cmtpacket.cpp:896
bool updatePositionLLA(const CmtVector &data, const uint16_t index=0)
Add/update Position Lat Lon Alt for the item.
Definition: cmtpacket.cpp:1096
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:245
bool updateRawTemp(uint16_t temp, const uint16_t index=0)
Add/update Raw Temperature data for the item.
Definition: cmtpacket.cpp:538
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:606
bool containsCalAcc(const uint16_t index=0) const
Check if data item contains Calibrated Accelerometer data.
Definition: cmtpacket.cpp:724
double getTemp(const uint16_t index=0) const
Return the Temperature component of a data item.
Definition: cmtpacket.cpp:679
CmtVector getPositionLLA(const uint16_t index=0) const
Return the Position Lat Lon Alt component of a data item.
Definition: cmtpacket.cpp:1079
TimeStamp m_toa
Time of arrival.
Definition: cmtpacket.h:74
#define CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632
Definition: cmtdef.h:638
unsigned __int16 uint16_t
Definition: rptypes.h:46
#define CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX
Definition: cmtdef.h:628
A structure for storing data formats.
Definition: cmtdef.h:900
bool containsAccG(const uint16_t index=0) const
Check if data item contains XKF-3 Acc-G data.
Definition: cmtpacket.cpp:1238
#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
struct xsens::Packet::PacketInfo * m_infoList
bool updateAnalogIn1(const CmtAnalogInData &data, const uint16_t index=0)
Add/update AnalogIn 1 for the item.
Definition: cmtpacket.cpp:1027
void resizeData(const uint16_t newSize)
Resize the data area to the given size.
Definition: cmtmessage.cpp:394
CmtVector getAccG(const uint16_t index=0) const
Return the XKF-3 Acc-G component of the packet.
Definition: cmtpacket.cpp:1228
GLuint buffer
Definition: glext.h:3775
bool containsAnalogIn2(const uint16_t index=0) const
Check if data item contains AnalogIn 2.
Definition: cmtpacket.cpp:1053
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:716
bool containsRawAcc(const uint16_t index=0) const
Check if data item contains Raw Accelerometer data.
Definition: cmtpacket.cpp:424
CmtVector getVelocity(const uint16_t index=0) const
Return the Velocity component of a data item.
Definition: cmtpacket.cpp:1118
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:619
bool updateCalData(const CmtCalData &data, const uint16_t index=0)
Add/update Calibrated Data for the item.
Definition: cmtpacket.cpp:850
uint16_t getDataSize(void) const
Return the length of the data part of the message.
Definition: cmtmessage.cpp:326
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:523
double m_pitch
The pitch (rotation around y-axis / right-left-line)
Definition: cmtdef.h:1170
uint16_t m_itemCount
The number of data items in the message.
Definition: cmtpacket.h:71
bool containsPositionLLA(const uint16_t index=0) const
Check if data item contains Position Lat Lon Alt.
Definition: cmtpacket.cpp:1088
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:741
uint16_t getSampleCounter(const uint16_t index=0) const
Return the Sample Counter component of the packet.
Definition: cmtpacket.cpp:1191
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:432
#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:614
#define CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE
Definition: cmtdef.h:639
#define CMT_OUTPUTMODE_VELOCITY
Definition: cmtdef.h:618
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
#define CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG
Definition: cmtdef.h:629
#define CMT_OUTPUTMODE_RAW
Definition: cmtdef.h:611
bool containsCalData(const uint16_t index=0) const
Check if data item contains Calibrated Data.
Definition: cmtpacket.cpp:842
bool containsRawGpsData(const uint16_t index=0) const
Check if data item contains Raw GpsPrint Data.
Definition: cmtpacket.cpp:628
#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:43
bool updateRawGpsData(const CmtRawGpsData &data, const uint16_t index=0)
Add/update Raw GpsPrint Data for the item.
Definition: cmtpacket.cpp:636
bool containsRawTemp(const uint16_t index=0) const
Check if data item contains Raw Temperature data.
Definition: cmtpacket.cpp:530
bool containsOriMatrix(const uint16_t index=0) const
Check if data item contains Matrix Orientation data.
Definition: cmtpacket.cpp:978
TimeStamp m_rtc
Sample time in ms, based on the sample counter.
Definition: cmtpacket.h:73
CmtDataFormat * m_formatList
A list of the formats of the data items.
Definition: cmtpacket.h:67
bool updateOriEuler(const CmtEuler &data, const uint16_t index=0)
Add/update Euler Orientation data for the item.
Definition: cmtpacket.cpp:938
bool updateCalGyr(const CmtVector &vec, const uint16_t index=0)
Add/update Calibrated Gyroscope data for the item.
Definition: cmtpacket.cpp:771
bool containsCalGyr(const uint16_t index=0) const
Check if data item contains Calibrated Gyroscope data.
Definition: cmtpacket.cpp:763
__int16 int16_t
Definition: rptypes.h:45
#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:754
bool updateVelocity(const CmtVector &data, const uint16_t index=0)
Add/update Velocity for the item.
Definition: cmtpacket.cpp:1136
GLuint index
Definition: glext.h:3891
bool updateAccG(const CmtVector &g, const uint16_t index=0)
Add/update XKF-3 Acc-G data for the item.
Definition: cmtpacket.cpp:1246
bool containsCalMag(const uint16_t index=0) const
Check if data item contains Calibrated Magnetometer data.
Definition: cmtpacket.cpp:802
CmtQuat getOriQuat(const uint16_t index=0) const
Return the Orientation component of a data item as a Quaternion.
Definition: cmtpacket.cpp:878
bool updateCalAcc(const CmtVector &vec, const uint16_t index=0)
Add/update Calibrated Accelerometer data for the item.
Definition: cmtpacket.cpp:732
#define CMT_OUTPUTSETTINGS_TIMESTAMP_MASK
Definition: cmtdef.h:647
bool m_xm
Indicates that xbus-formatting is used.
Definition: cmtpacket.h:68
double m_data[4]
Definition: cmtdef.h:1166
GLubyte g
Definition: glext.h:5575
bool containsRawGyr(const uint16_t index=0) const
Check if data item contains Raw Gyroscope data.
Definition: cmtpacket.cpp:460
CmtShortVector getRawAcc(const uint16_t index=0) const
Return the Raw Accelerometer component of a data item.
Definition: cmtpacket.cpp:415
double m_data[3][3]
Definition: cmtdef.h:1174
double m_roll
The roll (rotation around x-axis / back-front-line)
Definition: cmtdef.h:1169
#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:694
uint16_t m_rawGpsPressureAge
Definition: cmtpacket.h:52
bool containsOriQuat(const uint16_t index=0) const
Check if data item contains Quaternion Orientation data.
Definition: cmtpacket.cpp:888
bool containsTemp(const uint16_t index=0) const
Check if data item contains Temperature data.
Definition: cmtpacket.cpp:686
#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:175
#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:962
#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:653
#define CMT_OUTPUTSETTINGS_ORIENTMODE_EULER
Definition: cmtdef.h:627
uint16_t m_data[3]
Definition: cmtdef.h:1132
Contains information about data in the packet and the format of that data.
Definition: cmtpacket.h:28
CmtShortVector getRawGyr(const uint16_t index=0) const
Return the Raw Gyroscope component of a data item.
Definition: cmtpacket.cpp:451
double m_data[3]
Definition: cmtdef.h:1160
#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:146
uint8_t * getDataBuffer(const uint16_t offset=0)
Return a pointer to the data buffer.
Definition: cmtmessage.h:133
__int32 int32_t
Definition: rptypes.h:48
#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:496
bool containsVelocity(const uint16_t index=0) const
Check if data item contains Velocity.
Definition: cmtpacket.cpp:1128
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:986
#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:504
#define CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN1_MASK
Definition: cmtdef.h:654
#define CMT_OUTPUTMODE_RAWGPSPRINT
Definition: cmtdef.h:612
bool updateSampleCounter(uint16_t counter, const uint16_t index=0)
Add/update Sample Counter for all items.
Definition: cmtpacket.cpp:1205
#define CMT_OUTPUTMODE_TEMP
Definition: cmtdef.h:613
uint32_t CmtOutputMode
An output mode bit-field.
Definition: cmtdef.h:895
bool containsOriEuler(const uint16_t index=0) const
Check if data item contains Euler Orientation data.
Definition: cmtpacket.cpp:930
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
bool containsAnalogIn1(const uint16_t index=0) const
Check if data item contains AnalogIn 1.
Definition: cmtpacket.cpp:1019
void setXbus(bool xbus, bool convert=false)
Definition: cmtpacket.cpp:111
bool containsStatus(const uint16_t index=0) const
Check if data item contains Velocity.
Definition: cmtpacket.cpp:1164
CmtOutputMode m_outputMode
Definition: cmtdef.h:901
The namespace of all Xsens software since 2006.
Definition: cmt1.cpp:92
#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:1171
bool updateRawData(const CmtRawData &data, const uint16_t index=0)
Add/update Raw Data for the item.
Definition: cmtpacket.cpp:578
GLenum GLsizei GLenum format
Definition: glext.h:3513
CmtOutputSettings m_outputSettings
Definition: cmtdef.h:902
CmtVector getCalMag(const uint16_t index=0) const
Return the Calibrated Magnetometer component of a data item.
Definition: cmtpacket.cpp:793
bool updateCalMag(const CmtVector &vec, const uint16_t index=0)
Add/update Calibrated Magnetometer data for the item.
Definition: cmtpacket.cpp:810
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:1061
bool getXbus(void) const
Definition: cmtpacket.cpp:105
bool containsSampleCounter(const uint16_t index=0) const
Check if data item contains Sample Counter.
Definition: cmtpacket.cpp:1197
CmtTimeStamp getRtc(const uint16_t index=0) const
Return the RTC of the packet.
Definition: cmtpacket.cpp:1220
#define CMT_OUTPUTMODE_AUXILIARY
Definition: cmtdef.h:616
bool m_autoUpdateChecksum
Definition: cmtmessage.h:94
#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:1045
bool containsRawData(const uint16_t index=0) const
Check if data item contains Raw Data.
Definition: cmtpacket.cpp:570
CmtVector getCalAcc(const uint16_t index=0) const
Return the Calibrated Accelerometer component of a data item.
Definition: cmtpacket.cpp:715
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define CMT_OUTPUTMODE_ORIENT
Definition: cmtdef.h:615
#define swapEndian16(src)
Definition: cmtmessage.h:71
CmtAnalogInData getAnalogIn1(const uint16_t index=0) const
Return the AnalogIn 1 component of a data item.
Definition: cmtpacket.cpp:1011
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
CmtCalData getCalData(const uint16_t index=0) const
Return the Calibrated Data component of a data item.
Definition: cmtpacket.cpp:832
CmtShortVector getRawMag(const uint16_t index=0) const
Return the Raw Magnetometer component of a data item.
Definition: cmtpacket.cpp:487
#define CMT_OUTPUTMODE_POSITION
Definition: cmtdef.h:617
#define PACKETLOG(...)
Definition: cmtpacket.cpp:37
uint16_t CmtMtTimeStamp
An MT timestamp (sample count)
Definition: cmtdef.h:913
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
bool updateRawGyr(const CmtShortVector &vec, const uint16_t index=0)
Add/update Raw Gyroscope data for the item.
Definition: cmtpacket.cpp:468
uint64_t CmtOutputSettings
An output settings bit-field.
Definition: cmtdef.h:897
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
void operator=(const Packet &pack)
Definition: cmtpacket.cpp:1276
CmtEuler getOriEuler(const uint16_t index=0) const
Return the Orientation component of a data item as Euler angles.
Definition: cmtpacket.cpp:918
Message m_msg
The message.
Definition: cmtpacket.h:72
bool updateStatus(const uint8_t data, const uint16_t index=0)
Add/update Status information for the item.
Definition: cmtpacket.cpp:1172
uint8_t getStatus(const uint16_t index=0) const
Return the Status component of a data item.
Definition: cmtpacket.cpp:1158



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019