MRPT  1.9.9
legacydatapacket.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #include <xsens/xstypesconfig.h>
10 #include "legacydatapacket.h"
11 #include "packetfixeddata.h"
12 #include "mtwsdidata.h"
13 #include <xsens/xsgpspvtdata.h>
14 #include <xsens/xspressure.h>
15 #include <xsens/xscalibrateddata.h>
16 #include <xsens/xseuler.h>
17 #include <xsens/xsmatrix3x3.h>
18 #include <xsens/xsvector3.h>
19 #include <xsens/xsanalogindata.h>
20 #include <xsens/xsutctime.h>
21 
22 #ifdef LOG_PACKET
23 #include "xslog.h"
24 #define PACKETLOG XSENSLOG
25 #else
26 #define PACKETLOG(...) (void)0
27 #endif
28 
29 //////////////////////////////////////////////////////////////////////////////////////////
30 ////////////////////////////////////// Packet class
31 /////////////////////////////////////////
32 //////////////////////////////////////////////////////////////////////////////////////////
33 #define FORMAT_DOUBLE \
34  ((m_fixedData->m_formatList[index].m_outputSettings & \
35  XOS_Dataformat_Mask) | \
36  XOS_Dataformat_Double)
37 #define ISDOUBLE(a) \
38  (m_fixedData->m_infoList[index].a >= \
39  m_fixedData->m_infoList[index].m_doubleBoundary)
40 #define CHECKIFDOUBLE(a) \
41  ISDOUBLE(a) ? FORMAT_DOUBLE \
42  : m_fixedData->m_formatList[index].m_outputSettings
43 
44 /*! \struct LegacyDataPacket
45  \brief Contains an MTData XsMessage and supports functions for extracting
46  contained data.
47 */
48 
49 /*! \brief Construct a new %LegacyDataPacket without data. The %LegacyDataPacket
50  * will be invalid. */
51 LegacyDataPacket::LegacyDataPacket()
52  : m_fixedData(new PacketFixedData),
53  m_lastFoundId(0),
54  m_lastFoundIndex(0),
55  m_rtc(0),
56  m_toa(0),
57  m_packetId(0),
58  m_triggerIn1(0),
59  m_triggerIn2(0)
60 {
61  PACKETLOG("%s creating default %p\n", __FUNCTION__, this);
62 }
63 
64 /*! \brief Construct a new %LegacyDataPacket with room for data from \a count
65  devices.
66  \param count The number of devices whose data is in the object
67  \param xbus Whether the message was sent by an Xbus Master or by a
68  standalone MT. This matters
69  because the sample counter is in a different place.
70 */
71 LegacyDataPacket::LegacyDataPacket(uint16_t count, bool xbus)
72  : m_fixedData(new PacketFixedData(count)),
73  m_lastFoundId(0),
74  m_lastFoundIndex(0),
75  m_rtc(0),
76  m_toa(0),
77  m_packetId(0),
78  m_triggerIn1(0),
79  m_triggerIn2(0)
80 
81 {
82  PACKETLOG(
83  "%s creating %p with %d items and xbus option %d\n", __FUNCTION__, this,
84  count, xbus);
85  m_fixedData->m_xm = xbus;
86 }
87 
88 /*! \brief Construct a LegacyDataPacket as a copy of \a other. */
89 LegacyDataPacket::LegacyDataPacket(const LegacyDataPacket& other)
90  : m_fixedData(0),
91  m_lastFoundId(0),
92  m_lastFoundIndex(0),
93  m_rtc(0),
94  m_toa(0),
95  m_packetId(0),
96  m_triggerIn1(0),
97  m_triggerIn2(0)
98 
99 {
100  PACKETLOG("%s creating %p from %p\n", __FUNCTION__, this, &other);
101  *this = other;
102  PACKETLOG("%s creating %p from %p done\n", __FUNCTION__, this, &other);
103 }
104 
105 /*! \brief Assignment operator, copies the contents of \a pack into this
106 */
107 const LegacyDataPacket& LegacyDataPacket::operator=(
108  const LegacyDataPacket& pack)
109 {
110  PACKETLOG("%s copying from %p to %p\n", __FUNCTION__, &pack, this);
111  if (this == &pack) return *this;
112 
113  delete m_fixedData;
114  m_fixedData = nullptr;
115 
116  if (pack.m_fixedData) // Can be empty
117  {
118  m_fixedData = new PacketFixedData(*pack.m_fixedData);
119  m_lastFoundId = pack.m_lastFoundId;
120  m_lastFoundIndex = pack.m_lastFoundIndex;
121  }
122 
123  m_toa = pack.m_toa;
124  m_rtc = pack.m_rtc;
125  m_msg = pack.m_msg;
126  m_packetId = pack.m_packetId;
127  m_triggerIn1 = pack.m_triggerIn1;
128  m_triggerIn2 = pack.m_triggerIn2;
129 
130  PACKETLOG("%s copying from %p to %p done\n", __FUNCTION__, &pack, this);
131  return *this;
132 }
133 
134 /*! \brief Destructor */
135 LegacyDataPacket::~LegacyDataPacket()
136 {
137  try
138  {
139  PACKETLOG("%s destroying %p\n", __FUNCTION__, this);
140  delete m_fixedData;
141  m_fixedData = nullptr;
142  PACKETLOG("%s destroyed %p\n", __FUNCTION__, this);
143  }
144  catch (...)
145  {
146  }
147 }
148 
149 // lint -esym(613, LegacyDataPacket::m_fixedData) assert and dataSize take care
150 // of this
151 /*! \brief Returns the number of devices whose data is contained in the object
152 */
153 uint16_t LegacyDataPacket::itemCount(void) const
154 {
155  assert(m_fixedData);
156  return m_fixedData->m_itemCount;
157 }
158 
159 /*! \brief Set the number of devices whose data is contained in this object to
160  * \a count
161 */
162 void LegacyDataPacket::setItemCount(uint16_t count)
163 {
164  assert(m_fixedData);
165  m_fixedData->m_itemCount = count;
166 }
167 
168 /*! \brief Returns the Time Of Arrival value as stored in the object
169 */
170 XsTimeStamp LegacyDataPacket::timeOfArrival(void) const { return m_toa; }
171 /*! \brief Set the Time Of Arrival value to \a timeofarrival
172 */
173 void LegacyDataPacket::setTimeOfArrival(XsTimeStamp timeofarrival)
174 {
175  m_toa = timeofarrival;
176 }
177 
178 /*! \brief Returns the Real Time Clock value as stored in the object
179 */
180 XsTimeStamp LegacyDataPacket::rtc(void) const { return m_rtc; }
181 /*! \brief Set the Real Time Clock value to \a realtimeclock
182 */
183 void LegacyDataPacket::setRtc(const XsTimeStamp realtimeclock)
184 {
185  m_rtc = realtimeclock;
186 }
187 
188 /*! \brief Return the 64-bit sample counter associated with this packet
189  \returns The 64-bit sample counter associated with this packet
190  \note This sample counter may differ a lot from the normal 16-bit sample
191  counter
192 */
193 int64_t LegacyDataPacket::largePacketCounter(void) const
194 {
195  return m_packetId.msTime();
196 }
197 
198 /*! \brief Set the 64-bit sample counter associated with this packet
199 */
200 void LegacyDataPacket::setLargePacketCounter(const int64_t sc)
201 {
202  m_packetId = sc;
203 }
204 
205 /*! \brief Returns a copy of the %XsMessage contained by the object, including
206  * computed and added data. */
207 XsMessage LegacyDataPacket::message(void) const { return m_msg; }
208 /*! \brief Returns the original message as it was received, without computed and
209  * added data (except for SDI interval reconstruction)
210 */
211 XsMessage LegacyDataPacket::originalMessage(void) const
212 {
213  assert(m_fixedData);
214  uint16_t originalSize = 0;
215  for (uint16_t i = 0; i < m_fixedData->m_itemCount; ++i)
216  if (m_fixedData->m_infoList[i].m_doubleBoundary > originalSize)
217  originalSize = m_fixedData->m_infoList[i].m_doubleBoundary;
218 
219  if (originalSize == m_msg.getDataSize()) return m_msg;
220 
221  XsMessage m = m_msg;
222  m.resizeData(originalSize);
223  m.setDataBuffer(m_msg.getDataBuffer(), originalSize);
224  m.recomputeChecksum();
225  return m;
226 }
227 
228 /*! \brief Set the source message to \a msg
229  \param msg The message to use
230  \note Call updateInfoList() to actually start using the new message
231 */
232 void LegacyDataPacket::setMessage(const XsMessage& msg) { m_msg = msg; }
233 /*! \brief Returns the packet info for the \a index'th device in the packet.
234  \details This describes what data is contained by the object.
235  \param index The index of the device whose packet info should be returned
236  \returns The packet info for device \a index
237 */
238 PacketInfo LegacyDataPacket::packetInfo(int32_t index) const
239 {
240  assert(m_fixedData);
241  if (index < 0 || index >= m_fixedData->m_itemCount) return PacketInfo();
242 
243  return m_fixedData->m_infoList[index];
244 }
245 
246 /*! \brief Return the frame counter (previously: sample counter) of the packet
247  \details For SDI data, this function will return the Last Frame Number in
248  the SDI interval.
249  For other data, this function will return the plain Sample Counter.
250  \returns The frame counter of the packet or 0 if it isn't present in the
251  packet (note that 0 is
252  a valid sample counter, so don't use it for an error check).
253 */
254 uint16_t LegacyDataPacket::frameCounter() const
255 {
256  assert(m_fixedData);
257  if (containsPacketCounter(0))
258  return m_msg.getDataShort(m_fixedData->m_infoList[0].m_sc);
259  else if (containsMtwSdiData(0))
260  return m_msg.getDataShort(
261  m_fixedData->m_infoList[0].m_wLastFrameNumber);
262  return 0;
263 }
264 
265 //////////////////////////////////////////////////////////////////////////////////////////
266 /*! \brief Returns the index of the fixed data with id \a dev
267  \details If the result is not -1, it can be used as the \a index parameter
268  in other functions
269  that require it (see the list below).
270  \param dev The device ID to find
271  \returns The index of the fixed data
272  \sa deviceId \sa setDeviceId \sa dataFormat \sa setDataFormat(const
273  XsDataFormat&, int32_t)
274  \sa setDataFormat(XsOutputMode, XsOutputSettings, int32_t)
275 */
276 int32_t LegacyDataPacket::findDeviceId(XsDeviceId dev) const
277 {
278  assert(m_fixedData);
279  if (!dev.isValid()) return 0;
280  if (dev == m_lastFoundId) return m_lastFoundIndex;
281  for (uint16_t i = 0; i < m_fixedData->m_itemCount; ++i)
282  {
283  if (m_fixedData->m_idList[i] == dev)
284  {
285  m_lastFoundId = dev;
286  m_lastFoundIndex = i;
287  return i;
288  }
289  }
290  return -1;
291 }
292 
293 /*! \brief Returns the device ID of the device with the given \a index
294  \param index The index of the device whose device ID should be returned
295  \returns The device ID of the device
296  \sa findDeviceId
297 */
298 XsDeviceId LegacyDataPacket::deviceId(int32_t index) const
299 {
300  assert(m_fixedData);
301  return m_fixedData->m_idList[index];
302 }
303 
304 /*! \brief Sets the device ID of the device with the given \a index to \a
305  deviceid
306  \param deviceid The device ID to set
307  \param index The index of the device whose device ID should be updated
308  \returns true if the device ID was successfully updated
309  \sa findDeviceId
310 */
311 void LegacyDataPacket::setDeviceId(XsDeviceId deviceid, int32_t index)
312 {
313  assert(m_fixedData);
314  m_fixedData->m_idList[index] = deviceid;
315 }
316 
317 //////////////////////////////////////////////////////////////////////////////////////////
318 /*! \brief Returns the data format of the device with the given \a index
319  \param index The index of the device whose data format should be returned
320  \returns The data format of the device
321  \sa findDeviceId
322 */
323 XsDataFormat LegacyDataPacket::dataFormat(int32_t index) const
324 {
325  if (!m_fixedData || index >= m_fixedData->m_itemCount ||
326  m_fixedData->m_formatList == nullptr)
327  return XsDataFormat();
328 
329  return m_fixedData->m_formatList[index];
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////////////////
333 /*! \brief Sets the data format of the device with the given \a index to \a
334  format
335  \param format The data format of the device
336  \param index The index of the device whose data format should be updated
337  \returns true if the data format was successfully updated
338  \sa findDeviceId
339 */
340 bool LegacyDataPacket::setDataFormat(const XsDataFormat& format, int32_t index)
341 {
342  assert(m_fixedData);
343 
344  if (index < m_fixedData->m_itemCount)
345  {
346  m_fixedData->m_formatList[index] = format;
347  updateInfoList();
348  return true;
349  }
350  return false;
351 }
352 
353 //////////////////////////////////////////////////////////////////////////////////////////
354 /*! \brief Sets the data format of the device with the given \a index to \a
355  outputMode and \a outputSettings
356  \param outputMode The output mode of the device (see low level communication
357  documentation)
358  \param outputSettings The output settings of the device (see low level
359  communication documentation)
360  \param index The index of the device whose data format should be updated
361  \returns true if the data format was successfully updated
362  \sa findDeviceId
363 */
364 bool LegacyDataPacket::setDataFormat(
365  XsOutputMode outputMode, XsOutputSettings outputSettings, int32_t index)
366 {
367  assert(m_fixedData);
368 
369  if (index < m_fixedData->m_itemCount)
370  {
371  m_fixedData->m_formatList[index].m_outputMode = outputMode;
372  m_fixedData->m_formatList[index].m_outputSettings = outputSettings;
373  updateInfoList();
374  return true;
375  }
376  return false;
377 }
378 
379 //////////////////////////////////////////////////////////////////////////////////////////
380 /*! \brief Returns whether the xbus flag is set or not
381  \returns trur if the xbus flag is set or not
382  \sa setXbusSystem
383 */
384 bool LegacyDataPacket::isXbusSystem(void) const
385 {
386  assert(m_fixedData);
387  return m_fixedData->m_xm;
388 }
389 
390 //////////////////////////////////////////////////////////////////////////////////////////
391 /*! \brief Sets the xbus flag
392  \details The xbus flag determines where the packet counter is stored in the
393  message. Setting
394  this value incorrectly can lead to corrupted data reads.
395  \param xbus The desired setting
396  \param convert When set to true, the sample counter will be moved from its
397  old place to the new
398  place.
399 */
400 void LegacyDataPacket::setXbusSystem(bool xbus, bool convert)
401 {
402  assert(m_fixedData);
403 
404  if (xbus != m_fixedData->m_xm)
405  {
406  if (convert)
407  {
408  XsMtTimeStamp stamp = packetCounter(0);
409 
410  // move the time stamp value(s) around
411  if (xbus)
412  {
413  // new version is Xbus, so old version is regular format ->
414  // place timestamp at start of packet and remove from all
415  // individual data units
416  // remove the timestamp from all data units
417 
418  // insert timestamp at the start
419  m_msg.insertData(2, 0);
420  m_msg.setDataShort(stamp, 0);
421  }
422  else
423  {
424  // new version is regular, so old version is Xbus format ->
425  // place timestamp at end of all individual data units and
426  // remove from start of packet
427  // append the timestamp to all data units
428 
429  // remove timestamp from the start
430  m_msg.deleteData(2, 0);
431  }
432  }
433  // update the state cache
434  m_fixedData->m_xm = xbus;
435  updateInfoList();
436  }
437 }
438 
439 /*! \brief Returns the floating/fixed point value size in bytes
440  \param index The index of the item whose fp size should be returned.
441  \returns The floating/fixed point value size in bytes
442 */
443 uint16_t LegacyDataPacket::getFPValueSize(int32_t index) const
444 {
445  assert(m_fixedData);
446  uint16_t ds = 4;
447  switch (m_fixedData->m_formatList[index].m_outputSettings &
448  XOS_Dataformat_Mask)
449  {
450  case XOS_Dataformat_Float:
451  ds = 4;
452  break;
453 
454  case XOS_Dataformat_Double:
455  ds = 8;
456  break;
457 
458  case XOS_Dataformat_Fp1632:
459  ds = 6;
460  break;
461 
462  case XOS_Dataformat_F1220:
463  ds = 4;
464  break;
465  default:
466  break;
467  }
468  return ds;
469 }
470 
471 /*! \brief Returns the size of the data
472  \param index The index of the item of which the size should be returned.
473  \returns The size of the data
474 */
475 XsSize LegacyDataPacket::dataSize(int32_t index) const
476 {
477  if (m_fixedData && index < m_fixedData->m_itemCount)
478  return m_fixedData->m_infoList[index].m_size;
479  return 0;
480 }
481 
482 /*! \brief Update the internal info list by analyzing the known XsDataFormat for
483  * each device.
484 */
485 void LegacyDataPacket::updateInfoList()
486 {
487  assert(m_fixedData);
488  if (m_fixedData->m_infoList != nullptr)
489  {
490  delete[] m_fixedData->m_infoList;
491  m_fixedData->m_infoList = nullptr;
492  }
493 
494  assert(m_fixedData->m_itemCount);
495  // allocate list
496  m_fixedData->m_infoList = new PacketInfo[m_fixedData->m_itemCount];
497  uint16_t totalOffset;
498  if (m_fixedData->m_xm)
499  totalOffset = 2;
500  else
501  totalOffset = 0;
502 
503  // fill lists
504  for (uint16_t i = 0; i < m_fixedData->m_itemCount; ++i)
505  {
506  m_fixedData->m_infoList[i].m_offset = totalOffset;
507  m_fixedData->m_infoList[i].m_size = 0;
508 
509  uint16_t ds = getFPValueSize(i);
510 
511  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Raw)
512  {
513  m_fixedData->m_infoList[i].m_rawData = totalOffset;
514  m_fixedData->m_infoList[i].m_rawAcc = totalOffset;
515  m_fixedData->m_infoList[i].m_rawGyr = totalOffset + 6;
516  m_fixedData->m_infoList[i].m_rawMag = totalOffset + 12;
517  m_fixedData->m_infoList[i].m_rawTemp[0] = totalOffset + 18;
518 
519  m_fixedData->m_infoList[i].m_size += 20;
520  totalOffset += 20;
521 
522  if (m_fixedData->m_formatList[i].m_outputSettings &
523  XOS_ExtendedTemperature_Mask)
524  {
525  for (int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++)
526  {
527  m_fixedData->m_infoList[i].m_rawTemp[j + 1] = totalOffset;
528  m_fixedData->m_infoList[i].m_size += 2;
529  totalOffset += 2;
530  }
531  }
532  else
533  {
534  for (int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++)
535  {
536  m_fixedData->m_infoList[i].m_rawTemp[j + 1] =
537  XS_DATA_ITEM_NOT_AVAILABLE;
538  }
539  }
540  }
541  else
542  {
543  m_fixedData->m_infoList[i].m_rawData = XS_DATA_ITEM_NOT_AVAILABLE;
544  m_fixedData->m_infoList[i].m_rawAcc = XS_DATA_ITEM_NOT_AVAILABLE;
545  m_fixedData->m_infoList[i].m_rawGyr = XS_DATA_ITEM_NOT_AVAILABLE;
546  m_fixedData->m_infoList[i].m_rawMag = XS_DATA_ITEM_NOT_AVAILABLE;
547  for (int j = 0; j < XS_MAX_TEMPERATURE_CHANNELS; j++)
548  m_fixedData->m_infoList[i].m_rawTemp[j] =
549  XS_DATA_ITEM_NOT_AVAILABLE;
550  }
551 
552  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Sdi)
553  {
554  m_fixedData->m_infoList[i].m_mtwSdiData = totalOffset;
555  m_fixedData->m_infoList[i].m_wClientId = totalOffset;
556  m_fixedData->m_infoList[i].m_wTimeSync = totalOffset + 4;
557  m_fixedData->m_infoList[i].m_wFirstFrameNumber = totalOffset + 5;
558  m_fixedData->m_infoList[i].m_wLastFrameNumber = totalOffset + 7;
559  m_fixedData->m_infoList[i].m_wCurrentBias = totalOffset + 9;
560  m_fixedData->m_infoList[i].m_wOrientationIncrement =
561  totalOffset + 9 + 3 * ds;
562  m_fixedData->m_infoList[i].m_wVelocityIncrement =
563  totalOffset + 9 + 7 * ds;
564  m_fixedData->m_infoList[i].m_wAidingData =
565  totalOffset + 9 + 10 * ds;
566  m_fixedData->m_infoList[i].m_wBaroMeter =
567  totalOffset + 10 + 10 * ds;
568  m_fixedData->m_infoList[i].m_wMagnetoMeter =
569  totalOffset + 12 + 10 * ds;
570  m_fixedData->m_infoList[i].m_wRssi = totalOffset + 12 + 13 * ds;
571  m_fixedData->m_infoList[i].m_size += 13 + 13 * ds;
572  totalOffset += 13 + 13 * ds;
573  }
574  else
575  {
576  m_fixedData->m_infoList[i].m_mtwSdiData =
577  XS_DATA_ITEM_NOT_AVAILABLE;
578  m_fixedData->m_infoList[i].m_wClientId = XS_DATA_ITEM_NOT_AVAILABLE;
579  m_fixedData->m_infoList[i].m_wTimeSync = XS_DATA_ITEM_NOT_AVAILABLE;
580  m_fixedData->m_infoList[i].m_wFirstFrameNumber =
581  XS_DATA_ITEM_NOT_AVAILABLE;
582  m_fixedData->m_infoList[i].m_wLastFrameNumber =
583  XS_DATA_ITEM_NOT_AVAILABLE;
584  m_fixedData->m_infoList[i].m_wCurrentBias =
585  XS_DATA_ITEM_NOT_AVAILABLE;
586  m_fixedData->m_infoList[i].m_wOrientationIncrement =
587  XS_DATA_ITEM_NOT_AVAILABLE;
588  m_fixedData->m_infoList[i].m_wVelocityIncrement =
589  XS_DATA_ITEM_NOT_AVAILABLE;
590  m_fixedData->m_infoList[i].m_wAidingData =
591  XS_DATA_ITEM_NOT_AVAILABLE;
592  m_fixedData->m_infoList[i].m_wBaroMeter =
593  XS_DATA_ITEM_NOT_AVAILABLE;
594  m_fixedData->m_infoList[i].m_wMagnetoMeter =
595  XS_DATA_ITEM_NOT_AVAILABLE;
596  m_fixedData->m_infoList[i].m_wRssi = XS_DATA_ITEM_NOT_AVAILABLE;
597  }
598 
599  if (m_fixedData->m_formatList[i].m_outputMode & XOM_GpsPvt_Pressure)
600  {
601  m_fixedData->m_infoList[i].m_gpsPvtData = totalOffset;
602  m_fixedData->m_infoList[i].m_gpsPvtPressure = totalOffset;
603  m_fixedData->m_infoList[i].m_gpsPvtPressureAge = totalOffset + 2;
604  m_fixedData->m_infoList[i].m_size += 3;
605  totalOffset += 3;
606 
607  if ((m_fixedData->m_formatList[i].m_outputSettings &
608  XOS_NoGpsInGpsPvt) == 0)
609  {
610  m_fixedData->m_infoList[i].m_gpsPvtGpsData = totalOffset;
611  m_fixedData->m_infoList[i].m_gpsPvtItow = totalOffset;
612  m_fixedData->m_infoList[i].m_gpsPvtLatitude = totalOffset + 4;
613  m_fixedData->m_infoList[i].m_gpsPvtLongitude = totalOffset + 8;
614  m_fixedData->m_infoList[i].m_gpsPvtHeight = totalOffset + 12;
615  m_fixedData->m_infoList[i].m_gpsPvtVeln = totalOffset + 16;
616  m_fixedData->m_infoList[i].m_gpsPvtVele = totalOffset + 20;
617  m_fixedData->m_infoList[i].m_gpsPvtVeld = totalOffset + 24;
618  m_fixedData->m_infoList[i].m_gpsPvtHacc = totalOffset + 28;
619  m_fixedData->m_infoList[i].m_gpsPvtVacc = totalOffset + 32;
620  m_fixedData->m_infoList[i].m_gpsPvtSacc = totalOffset + 36;
621  m_fixedData->m_infoList[i].m_gpsPvtGpsAge = totalOffset + 40;
622  m_fixedData->m_infoList[i].m_size += 41;
623  totalOffset += 41;
624  }
625  else
626  {
627  m_fixedData->m_infoList[i].m_gpsPvtGpsData =
628  XS_DATA_ITEM_NOT_AVAILABLE;
629  m_fixedData->m_infoList[i].m_gpsPvtItow =
630  XS_DATA_ITEM_NOT_AVAILABLE;
631  m_fixedData->m_infoList[i].m_gpsPvtLatitude =
632  XS_DATA_ITEM_NOT_AVAILABLE;
633  m_fixedData->m_infoList[i].m_gpsPvtLongitude =
634  XS_DATA_ITEM_NOT_AVAILABLE;
635  m_fixedData->m_infoList[i].m_gpsPvtHeight =
636  XS_DATA_ITEM_NOT_AVAILABLE;
637  m_fixedData->m_infoList[i].m_gpsPvtVeln =
638  XS_DATA_ITEM_NOT_AVAILABLE;
639  m_fixedData->m_infoList[i].m_gpsPvtVele =
640  XS_DATA_ITEM_NOT_AVAILABLE;
641  m_fixedData->m_infoList[i].m_gpsPvtVeld =
642  XS_DATA_ITEM_NOT_AVAILABLE;
643  m_fixedData->m_infoList[i].m_gpsPvtHacc =
644  XS_DATA_ITEM_NOT_AVAILABLE;
645  m_fixedData->m_infoList[i].m_gpsPvtVacc =
646  XS_DATA_ITEM_NOT_AVAILABLE;
647  m_fixedData->m_infoList[i].m_gpsPvtSacc =
648  XS_DATA_ITEM_NOT_AVAILABLE;
649  m_fixedData->m_infoList[i].m_gpsPvtGpsAge =
650  XS_DATA_ITEM_NOT_AVAILABLE;
651  }
652  }
653  else
654  {
655  m_fixedData->m_infoList[i].m_gpsPvtData =
656  XS_DATA_ITEM_NOT_AVAILABLE;
657  m_fixedData->m_infoList[i].m_gpsPvtPressure =
658  XS_DATA_ITEM_NOT_AVAILABLE;
659  m_fixedData->m_infoList[i].m_gpsPvtPressureAge =
660  XS_DATA_ITEM_NOT_AVAILABLE;
661 
662  m_fixedData->m_infoList[i].m_gpsPvtGpsData =
663  XS_DATA_ITEM_NOT_AVAILABLE;
664  m_fixedData->m_infoList[i].m_gpsPvtItow =
665  XS_DATA_ITEM_NOT_AVAILABLE;
666  m_fixedData->m_infoList[i].m_gpsPvtLatitude =
667  XS_DATA_ITEM_NOT_AVAILABLE;
668  m_fixedData->m_infoList[i].m_gpsPvtLongitude =
669  XS_DATA_ITEM_NOT_AVAILABLE;
670  m_fixedData->m_infoList[i].m_gpsPvtHeight =
671  XS_DATA_ITEM_NOT_AVAILABLE;
672  m_fixedData->m_infoList[i].m_gpsPvtVeln =
673  XS_DATA_ITEM_NOT_AVAILABLE;
674  m_fixedData->m_infoList[i].m_gpsPvtVele =
675  XS_DATA_ITEM_NOT_AVAILABLE;
676  m_fixedData->m_infoList[i].m_gpsPvtVeld =
677  XS_DATA_ITEM_NOT_AVAILABLE;
678  m_fixedData->m_infoList[i].m_gpsPvtHacc =
679  XS_DATA_ITEM_NOT_AVAILABLE;
680  m_fixedData->m_infoList[i].m_gpsPvtVacc =
681  XS_DATA_ITEM_NOT_AVAILABLE;
682  m_fixedData->m_infoList[i].m_gpsPvtSacc =
683  XS_DATA_ITEM_NOT_AVAILABLE;
684  m_fixedData->m_infoList[i].m_gpsPvtGpsAge =
685  XS_DATA_ITEM_NOT_AVAILABLE;
686  }
687 
688  for (int j = 0; j < XS_MAX_TEMPERATURE_CHANNELS; j++)
689  m_fixedData->m_infoList[i].m_temp[j] = XS_DATA_ITEM_NOT_AVAILABLE;
690 
691  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Temperature)
692  {
693  int tCount = (m_fixedData->m_formatList[i].m_outputSettings &
694  XOS_ExtendedTemperature_Mask)
695  ? XS_MAX_TEMPERATURE_CHANNELS
696  : 1;
697  for (int j = 0; j < tCount; j++)
698  {
699  m_fixedData->m_infoList[i].m_temp[j] = totalOffset;
700  m_fixedData->m_infoList[i].m_size += ds;
701  totalOffset += ds;
702  }
703  }
704 
705  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Calibrated)
706  {
707  m_fixedData->m_infoList[i].m_calData = totalOffset;
708  if ((m_fixedData->m_formatList[i].m_outputSettings &
709  XOS_CalibratedMode_Acc_Mask) == 0)
710  {
711  m_fixedData->m_infoList[i].m_calAcc = totalOffset;
712  m_fixedData->m_infoList[i].m_size += 3 * ds;
713  totalOffset += 3 * ds;
714  }
715  else
716  m_fixedData->m_infoList[i].m_calAcc =
717  XS_DATA_ITEM_NOT_AVAILABLE;
718 
719  if ((m_fixedData->m_formatList[i].m_outputSettings &
720  XOS_CalibratedMode_Gyr_Mask) == 0)
721  {
722  m_fixedData->m_infoList[i].m_calGyr = totalOffset;
723  m_fixedData->m_infoList[i].m_size += 3 * ds;
724  totalOffset += 3 * ds;
725  }
726  else
727  m_fixedData->m_infoList[i].m_calGyr =
728  XS_DATA_ITEM_NOT_AVAILABLE;
729 
730  if ((m_fixedData->m_formatList[i].m_outputSettings &
731  XOS_CalibratedMode_Mag_Mask) == 0)
732  {
733  m_fixedData->m_infoList[i].m_calMag = totalOffset;
734  m_fixedData->m_infoList[i].m_size += 3 * ds;
735  totalOffset += 3 * ds;
736  }
737  else
738  m_fixedData->m_infoList[i].m_calMag =
739  XS_DATA_ITEM_NOT_AVAILABLE;
740 
741  if ((m_fixedData->m_formatList[i].m_outputSettings &
742  XOS_CalibratedMode_AccGyrMag_Mask) ==
743  XOS_CalibratedMode_AccGyrMag_Mask)
744  m_fixedData->m_infoList[i].m_calData =
745  XS_DATA_ITEM_NOT_AVAILABLE;
746  }
747  else
748  {
749  m_fixedData->m_infoList[i].m_calData = XS_DATA_ITEM_NOT_AVAILABLE;
750  m_fixedData->m_infoList[i].m_calAcc = XS_DATA_ITEM_NOT_AVAILABLE;
751  m_fixedData->m_infoList[i].m_calGyr = XS_DATA_ITEM_NOT_AVAILABLE;
752  m_fixedData->m_infoList[i].m_calMag = XS_DATA_ITEM_NOT_AVAILABLE;
753  }
754 
755  m_fixedData->m_infoList[i].m_oriEul = XS_DATA_ITEM_NOT_AVAILABLE;
756  m_fixedData->m_infoList[i].m_oriQuat = XS_DATA_ITEM_NOT_AVAILABLE;
757  m_fixedData->m_infoList[i].m_oriMat = XS_DATA_ITEM_NOT_AVAILABLE;
758 
759  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Orientation)
760  {
761  switch (m_fixedData->m_formatList[i].m_outputSettings &
762  XOS_OrientationMode_Mask)
763  {
764  case XOS_OrientationMode_Euler:
765  m_fixedData->m_infoList[i].m_oriEul = totalOffset;
766  m_fixedData->m_infoList[i].m_size += 3 * ds;
767  totalOffset += 3 * ds;
768  break;
769  case XOS_OrientationMode_Quaternion:
770  m_fixedData->m_infoList[i].m_oriQuat = totalOffset;
771  m_fixedData->m_infoList[i].m_size += 4 * ds;
772  totalOffset += 4 * ds;
773  break;
774  case XOS_OrientationMode_Matrix:
775  m_fixedData->m_infoList[i].m_oriMat = totalOffset;
776  m_fixedData->m_infoList[i].m_size += 9 * ds;
777  totalOffset += 9 * ds;
778  break;
779  default:
780  break;
781  }
782  }
783 
784  m_fixedData->m_infoList[i].m_analogIn1 = XS_DATA_ITEM_NOT_AVAILABLE;
785  m_fixedData->m_infoList[i].m_analogIn2 = XS_DATA_ITEM_NOT_AVAILABLE;
786  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Auxiliary)
787  {
788  if ((m_fixedData->m_formatList[i].m_outputSettings &
789  XOS_AuxiliaryMode_Ain1_Mask) == 0)
790  {
791  m_fixedData->m_infoList[i].m_analogIn1 = totalOffset;
792  m_fixedData->m_infoList[i].m_size += 2;
793  totalOffset += 2;
794  }
795  else
796  m_fixedData->m_infoList[i].m_analogIn1 =
797  XS_DATA_ITEM_NOT_AVAILABLE;
798 
799  if ((m_fixedData->m_formatList[i].m_outputSettings &
800  XOS_AuxiliaryMode_Ain2_Mask) == 0)
801  {
802  m_fixedData->m_infoList[i].m_analogIn2 = totalOffset;
803  m_fixedData->m_infoList[i].m_size += 2;
804  totalOffset += 2;
805  }
806  else
807  m_fixedData->m_infoList[i].m_analogIn2 =
808  XS_DATA_ITEM_NOT_AVAILABLE;
809  }
810 
811  m_fixedData->m_infoList[i].m_posLLA = XS_DATA_ITEM_NOT_AVAILABLE;
812  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Position)
813  {
814  if ((m_fixedData->m_formatList[i].m_outputSettings &
815  XOS_PositionMode_Mask) == XOS_PositionMode_Lla_Wgs84)
816  {
817  m_fixedData->m_infoList[i].m_posLLA = totalOffset;
818  m_fixedData->m_infoList[i].m_size += 3 * ds;
819  totalOffset += 3 * ds;
820  }
821  }
822 
823  m_fixedData->m_infoList[i].m_velNEDorNWU = XS_DATA_ITEM_NOT_AVAILABLE;
824  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Velocity)
825  {
826  if ((m_fixedData->m_formatList[i].m_outputSettings &
827  XOS_VelocityMode_Mask) == XOS_VelocityMode_Ms_Xyz)
828  {
829  m_fixedData->m_infoList[i].m_velNEDorNWU = totalOffset;
830  m_fixedData->m_infoList[i].m_size += 3 * ds;
831  totalOffset += 3 * ds;
832  }
833  }
834 
835  if (m_fixedData->m_formatList[i].m_outputMode & XOM_Status)
836  {
837  m_fixedData->m_infoList[i].m_status = totalOffset;
838 
839  if (m_fixedData->m_formatList[i].m_outputSettings &
840  XOS_Status_Detailed)
841  {
842  m_fixedData->m_infoList[i].m_detailedStatus = totalOffset;
843  m_fixedData->m_infoList[i].m_size += 4;
844  totalOffset += 4;
845  }
846  else
847  {
848  m_fixedData->m_infoList[i].m_detailedStatus =
849  XS_DATA_ITEM_NOT_AVAILABLE;
850  m_fixedData->m_infoList[i].m_size += 1;
851  totalOffset += 1;
852  }
853  }
854  else
855  {
856  m_fixedData->m_infoList[i].m_status = XS_DATA_ITEM_NOT_AVAILABLE;
857  m_fixedData->m_infoList[i].m_detailedStatus =
858  XS_DATA_ITEM_NOT_AVAILABLE;
859  }
860 
861  m_fixedData->m_infoList[i].m_sc = XS_DATA_ITEM_NOT_AVAILABLE;
862  if (m_fixedData->m_xm) m_fixedData->m_infoList[i].m_sc = 0;
863  if (m_fixedData->m_formatList[i].m_outputSettings &
864  XOS_Timestamp_PacketCounter)
865  {
866  if (!m_fixedData->m_xm)
867  m_fixedData->m_infoList[i].m_sc = totalOffset;
868  m_fixedData->m_infoList[i].m_size += 2;
869  totalOffset += 2;
870  }
871 
872  if (m_fixedData->m_formatList[i].m_outputSettings &
873  XOS_Timestamp_SampleUtc)
874  {
875  m_fixedData->m_infoList[i].m_utcTime = totalOffset;
876  m_fixedData->m_infoList[i].m_utcNano = totalOffset;
877  m_fixedData->m_infoList[i].m_utcYear = totalOffset + 4;
878  m_fixedData->m_infoList[i].m_utcMonth = totalOffset + 6;
879  m_fixedData->m_infoList[i].m_utcDay = totalOffset + 7;
880  m_fixedData->m_infoList[i].m_utcHour = totalOffset + 8;
881  m_fixedData->m_infoList[i].m_utcMinute = totalOffset + 9;
882  m_fixedData->m_infoList[i].m_utcSecond = totalOffset + 10;
883  m_fixedData->m_infoList[i].m_utcValid = totalOffset + 11;
884  m_fixedData->m_infoList[i].m_size += 12;
885  totalOffset += 12;
886  }
887  else
888  {
889  m_fixedData->m_infoList[i].m_utcTime = XS_DATA_ITEM_NOT_AVAILABLE;
890  m_fixedData->m_infoList[i].m_utcNano = XS_DATA_ITEM_NOT_AVAILABLE;
891  m_fixedData->m_infoList[i].m_utcYear = XS_DATA_ITEM_NOT_AVAILABLE;
892  m_fixedData->m_infoList[i].m_utcMonth = XS_DATA_ITEM_NOT_AVAILABLE;
893  m_fixedData->m_infoList[i].m_utcDay = XS_DATA_ITEM_NOT_AVAILABLE;
894  m_fixedData->m_infoList[i].m_utcHour = XS_DATA_ITEM_NOT_AVAILABLE;
895  m_fixedData->m_infoList[i].m_utcMinute = XS_DATA_ITEM_NOT_AVAILABLE;
896  m_fixedData->m_infoList[i].m_utcSecond = XS_DATA_ITEM_NOT_AVAILABLE;
897  m_fixedData->m_infoList[i].m_utcValid = XS_DATA_ITEM_NOT_AVAILABLE;
898  }
899 
900  // post-processing data is never available at this point
901  m_fixedData->m_infoList[i].m_acc_g = XS_DATA_ITEM_NOT_AVAILABLE;
902  m_fixedData->m_infoList[i].m_doubleBoundary = totalOffset;
903 
904  if (m_fixedData->m_infoList[i].m_sc == XS_DATA_ITEM_NOT_AVAILABLE)
905  m_fixedData->m_infoList[i].m_sc =
906  m_fixedData->m_infoList[i].m_wLastFrameNumber;
907  }
908 }
909 
910 #if 0
911 /*! \brief Take packet fixed data out of this packet
912  \details The packet fixed data can usually be shared across several other packets. When
913  implementing caching, the fixed packet data can cause a huge overhead. By removing it from the
914  packet, the amount of memory usage is significantly reduced.
915 */
916 PacketFixedData LegacyDataPacket::takePacketFixedData()
917 {
918  if (m_fixedData == nullptr)
919  return PacketFixedData();
920 
921  PacketFixedData data(*m_fixedData);
922  CHKDELNUL(m_fixedData);
923  return data;
924 }
925 
926 
927 /*! \brief Put packet fixed data back into this packet
928  \see takePacketFixedData
929 */
930 void LegacyDataPacket::putPacketFixedData(const PacketFixedData& data)
931 {
932  CHKDELNUL(m_fixedData);
933  m_fixedData = new PacketFixedData(data);
934 }
935 #endif
936 
937 /*! \brief The Raw Accelerometer component of a data item.
938 
939  \param index The index of the item of which the data should be returned.
940 
941  \returns A XsUShortVector containing the x, y and z axis values in that
942  order
943 */
944 XsUShortVector LegacyDataPacket::rawAcceleration(int32_t index) const
945 {
946  XsUShortVector buffer;
947  if (containsRawAcceleration(index))
948  for (uint16_t i = 0; i < 3; ++i)
949  buffer[i] = m_msg.getDataShort(
950  m_fixedData->m_infoList[index].m_rawAcc + (2 * i));
951 
952  return buffer;
953 }
954 
955 /*! \brief Check if data item contains Raw Accelerometer data
956 
957  \param index The index of the item of which the data should be returned
958 
959  \returns true if this packet contains raw acceleration data
960 */
961 bool LegacyDataPacket::containsRawAcceleration(int32_t index) const
962 {
963  if (dataSize(index) == 0) return false;
964  if (m_fixedData->m_infoList[index].m_rawAcc == XS_DATA_ITEM_NOT_AVAILABLE)
965  return false;
966  return true;
967 }
968 
969 /*! \brief Add/update Raw Accelerometer data for the item */
970 bool LegacyDataPacket::setRawAcceleration(
971  const XsUShortVector& vec, int32_t index)
972 {
973  if (dataSize(index) == 0) return false;
974  if (m_fixedData->m_infoList[index].m_rawAcc == XS_DATA_ITEM_NOT_AVAILABLE)
975  {
976  // add
977  m_fixedData->m_infoList[index].m_rawAcc = (uint16_t)m_msg.getDataSize();
978  m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
979  m_fixedData->m_infoList[index].m_size += 3 * 2;
980  }
981  // update
982  for (uint16_t i = 0; i < 3; ++i)
983  m_msg.setDataShort(
984  vec[i], m_fixedData->m_infoList[index].m_rawAcc + (2 * i));
985  return true;
986 }
987 
988 //////////////////////////////////////////////////////////////////////////////////////////
989 // Return the Raw Gyroscope component of a data item.
990 XsUShortVector LegacyDataPacket::rawGyroscopeData(int32_t index) const
991 {
992  XsUShortVector buffer;
993  if (containsRawGyroscopeData(index))
994  for (uint16_t i = 0; i < 3; ++i)
995  buffer[i] = m_msg.getDataShort(
996  m_fixedData->m_infoList[index].m_rawGyr + (2 * i));
997 
998  return buffer;
999 }
1000 bool LegacyDataPacket::containsRawGyroscopeData(int32_t index) const
1001 {
1002  if (dataSize(index) == 0) return false;
1003  if (m_fixedData->m_infoList[index].m_rawGyr == XS_DATA_ITEM_NOT_AVAILABLE)
1004  return false;
1005  return true;
1006 }
1007 bool LegacyDataPacket::setRawGyroscopeData(
1008  const XsUShortVector& vec, int32_t index)
1009 {
1010  if (dataSize(index) == 0) return false;
1011  if (m_fixedData->m_infoList[index].m_rawGyr == XS_DATA_ITEM_NOT_AVAILABLE)
1012  {
1013  // add
1014  m_fixedData->m_infoList[index].m_rawGyr = (uint16_t)m_msg.getDataSize();
1015  m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
1016  m_fixedData->m_infoList[index].m_size += 3 * 2;
1017  }
1018  // update
1019  for (uint16_t i = 0; i < 3; ++i)
1020  m_msg.setDataShort(
1021  vec[i], m_fixedData->m_infoList[index].m_rawGyr + (2 * i));
1022  return true;
1023 }
1024 
1025 //////////////////////////////////////////////////////////////////////////////////////////
1026 // Return the Raw Magnetometer component of a data item.
1027 XsUShortVector LegacyDataPacket::rawMagneticField(int32_t index) const
1028 {
1029  XsUShortVector buffer;
1030  if (containsRawMagneticField(index))
1031  for (uint16_t i = 0; i < 3; ++i)
1032  buffer[i] = m_msg.getDataShort(
1033  m_fixedData->m_infoList[index].m_rawMag + (2 * i));
1034 
1035  return buffer;
1036 }
1037 bool LegacyDataPacket::containsRawMagneticField(int32_t index) const
1038 {
1039  if (dataSize(index) == 0) return false;
1040  if (m_fixedData->m_infoList[index].m_rawMag == XS_DATA_ITEM_NOT_AVAILABLE)
1041  return false;
1042  return true;
1043 }
1044 bool LegacyDataPacket::setRawMagneticField(
1045  const XsUShortVector& vec, int32_t index)
1046 {
1047  if (dataSize(index) == 0) return false;
1048  if (m_fixedData->m_infoList[index].m_rawMag == XS_DATA_ITEM_NOT_AVAILABLE)
1049  {
1050  // add
1051  m_fixedData->m_infoList[index].m_rawMag = (uint16_t)m_msg.getDataSize();
1052  m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
1053  m_fixedData->m_infoList[index].m_size += 3 * 2;
1054  }
1055  // update
1056  for (uint16_t i = 0; i < 3; ++i)
1057  m_msg.setDataShort(
1058  vec[i], m_fixedData->m_infoList[index].m_rawMag + (2 * i));
1059  return true;
1060 }
1061 
1062 //////////////////////////////////////////////////////////////////////////////////////////
1063 // Return the Raw Temperature component of a data item.
1064 uint16_t LegacyDataPacket::rawTemperature(int32_t index, int channel) const
1065 {
1066  if (!containsRawTemperature(index, channel)) return 0;
1067 
1068  return m_msg.getDataShort(
1069  m_fixedData->m_infoList[index].m_rawTemp[channel]);
1070 }
1071 bool LegacyDataPacket::containsRawTemperature(int32_t index, int channel) const
1072 {
1073  if (dataSize(index) == 0) return false;
1074  if (channel >= XS_MAX_TEMPERATURE_CHANNELS) return false;
1075  if (m_fixedData->m_infoList[index].m_rawTemp[channel] ==
1076  XS_DATA_ITEM_NOT_AVAILABLE)
1077  return false;
1078  return true;
1079 }
1080 bool LegacyDataPacket::setRawTemperature(
1081  const uint16_t temp, int32_t index, int channel)
1082 {
1083  if (dataSize(index) == 0) return false;
1084  if (channel >= XS_MAX_TEMPERATURE_CHANNELS) return false;
1085  if (m_fixedData->m_infoList[index].m_rawTemp[channel] ==
1086  XS_DATA_ITEM_NOT_AVAILABLE)
1087  {
1088  int c = rawTemperatureChannelCount(index);
1089  for (int i = c; i <= channel;
1090  i++) // Add (also add missing intermediate channels)
1091  {
1092  m_fixedData->m_infoList[index].m_rawTemp[i] =
1093  (uint16_t)m_msg.getDataSize();
1094  m_msg.resizeData(m_msg.getDataSize() + 2);
1095  m_fixedData->m_infoList[index].m_size += 2;
1096  }
1097  }
1098  // update
1099  m_msg.setDataShort(temp, m_fixedData->m_infoList[index].m_rawTemp[channel]);
1100  return true;
1101 }
1102 int LegacyDataPacket::rawTemperatureChannelCount(int32_t index) const
1103 {
1104  for (int i = 0; i < XS_MAX_TEMPERATURE_CHANNELS; i++)
1105  {
1106  if (m_fixedData->m_infoList[index].m_rawTemp[i] ==
1107  XS_DATA_ITEM_NOT_AVAILABLE)
1108  return i;
1109  }
1110  return XS_MAX_TEMPERATURE_CHANNELS;
1111 }
1112 
1113 //////////////////////////////////////////////////////////////////////////////////////////
1114 // Return the Raw Data component of a data item.
1115 XsScrData LegacyDataPacket::rawData(int32_t index) const
1116 {
1117  XsScrData buffer;
1118  if (containsRawData(index))
1119  {
1120  const uint8_t* tmp =
1121  m_msg.getDataBuffer(m_fixedData->m_infoList[index].m_rawData);
1122  const uint16_t* sh = (const uint16_t*)tmp;
1123  uint16_t* bare = (uint16_t*)&buffer;
1124 
1125  for (uint16_t i = 0; i < (9 + rawTemperatureChannelCount(index));
1126  ++i, ++sh, ++bare)
1127  *bare = swapEndian16(
1128  *sh); // m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawData
1129  // + (2*i));
1130  }
1131  return buffer;
1132 }
1133 bool LegacyDataPacket::containsRawData(int32_t index) const
1134 {
1135  if (dataSize(index) == 0) return false;
1136  return (
1137  m_fixedData->m_infoList[index].m_rawData != XS_DATA_ITEM_NOT_AVAILABLE);
1138 }
1139 bool LegacyDataPacket::setRawData(const XsScrData& data, int32_t index)
1140 {
1141  if (dataSize(index) == 0) return false;
1142  if (m_fixedData->m_infoList[index].m_rawData == XS_DATA_ITEM_NOT_AVAILABLE)
1143  {
1144  // add
1145  m_fixedData->m_infoList[index].m_rawData =
1146  (uint16_t)m_msg.getDataSize();
1147  m_msg.resizeData(m_msg.getDataSize() + 3 * 3 * 2 + 2);
1148  m_fixedData->m_infoList[index].m_rawAcc =
1149  m_fixedData->m_infoList[index].m_rawData;
1150  m_fixedData->m_infoList[index].m_rawGyr =
1151  m_fixedData->m_infoList[index].m_rawData + 3 * 2;
1152  m_fixedData->m_infoList[index].m_rawMag =
1153  m_fixedData->m_infoList[index].m_rawData + 6 * 2;
1154  for (int i = 0; i < XS_MAX_TEMPERATURE_CHANNELS; i++)
1155  {
1156  m_fixedData->m_infoList[index].m_rawTemp[i] =
1157  m_fixedData->m_infoList[index].m_rawData + 9 * 2 + (i * 2);
1158  }
1159  m_fixedData->m_infoList[index].m_size +=
1160  3 * 3 * 2 + (XS_MAX_TEMPERATURE_CHANNELS * 2);
1161  }
1162  // update
1163  int16_t* bare = (int16_t*)&data;
1164  for (uint16_t i = 0; i < (9 + rawTemperatureChannelCount(index)); ++i)
1165  m_msg.setDataShort(
1166  bare[i], m_fixedData->m_infoList[index].m_rawData + (2 * i));
1167  return true;
1168 }
1169 
1170 //////////////////////////////////////////////////////////////////////////////////////////
1171 // Return the Gps PVT Data component of a data item.
1172 XsGpsPvtData LegacyDataPacket::gpsPvtData(int32_t index) const
1173 {
1174  XsGpsPvtData buffer;
1175  if (containsGpsPvtData(index))
1176  {
1177  // const uint8_t* tmp =
1178  // m_msg.getDataBuffer(m_fixedData->m_infoList[index].m_gpsPvtData);
1179  // const uint16_t* sh = (const uint16_t*) tmp;
1180  // uint16_t* bare = (uint16_t*) &buffer;
1181 
1182  // pressure data
1183  buffer.m_pressure =
1184  m_msg.getDataShort(m_fixedData->m_infoList[index].m_gpsPvtPressure);
1185  // pressAge
1186  buffer.m_pressureAge = m_msg.getDataByte(
1187  m_fixedData->m_infoList[index].m_gpsPvtPressureAge);
1188 
1189  // lon,lat,height,hacc,vacc,veln,vele,veld
1190  // tmp =
1191  // m_msg.getDataBuffer(m_fixedData->m_infoList[index].m_gpsPvtGpsData);
1192  // const uint32_t* ln = (const uint32_t*) tmp;
1193  uint32_t* bareln = (uint32_t*)&buffer.m_itow;
1194  for (uint16_t i = 0; i < 10; ++i)
1195  {
1196  // lint --e{662, 661}
1197  if (m_fixedData->m_infoList[index].m_gpsPvtGpsData !=
1198  XS_DATA_ITEM_NOT_AVAILABLE)
1199  {
1200  bareln[i] = m_msg.getDataLong(
1201  m_fixedData->m_infoList[index].m_gpsPvtGpsData +
1202  (4 * i)); // lint !e661 !e662
1203  }
1204  else
1205  {
1206  bareln[i] = 0;
1207  }
1208  }
1209 
1210  // gpsAge
1211  if (m_fixedData->m_infoList[index].m_gpsPvtGpsAge !=
1212  XS_DATA_ITEM_NOT_AVAILABLE)
1213  {
1214  buffer.m_gpsAge = m_msg.getDataByte(
1215  m_fixedData->m_infoList[index].m_gpsPvtGpsAge);
1216  }
1217  else
1218  {
1219  buffer.m_gpsAge = 0;
1220  }
1221  }
1222  else
1223  buffer.clear();
1224  return buffer;
1225 }
1226 bool LegacyDataPacket::containsGpsPvtData(int32_t index) const
1227 {
1228  if (dataSize(index) == 0) return false;
1229  if (m_fixedData->m_infoList[index].m_gpsPvtData ==
1230  XS_DATA_ITEM_NOT_AVAILABLE)
1231  return false;
1232  return true;
1233 }
1234 bool LegacyDataPacket::setGpsPvtData(const XsGpsPvtData& data, int32_t index)
1235 {
1236  if (dataSize(index) == 0) return false;
1237  if (m_fixedData->m_infoList[index].m_gpsPvtData ==
1238  XS_DATA_ITEM_NOT_AVAILABLE)
1239  {
1240  // add
1241  m_fixedData->m_infoList[index].m_gpsPvtData =
1242  (uint16_t)m_msg.getDataSize();
1243  m_msg.resizeData(m_msg.getDataSize() + (2 + 1) + (40 + 1));
1244  m_fixedData->m_infoList[index].m_gpsPvtPressure =
1245  m_fixedData->m_infoList[index].m_gpsPvtData;
1246  m_fixedData->m_infoList[index].m_gpsPvtPressureAge =
1247  m_fixedData->m_infoList[index].m_gpsPvtData + 2;
1248 
1249  m_fixedData->m_infoList[index].m_gpsPvtGpsData =
1250  m_fixedData->m_infoList[index].m_gpsPvtData + 3;
1251  m_fixedData->m_infoList[index].m_gpsPvtItow =
1252  m_fixedData->m_infoList[index].m_gpsPvtData + 3;
1253  m_fixedData->m_infoList[index].m_gpsPvtLatitude =
1254  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 4;
1255  m_fixedData->m_infoList[index].m_gpsPvtLongitude =
1256  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 8;
1257  m_fixedData->m_infoList[index].m_gpsPvtHeight =
1258  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 12;
1259  m_fixedData->m_infoList[index].m_gpsPvtVeln =
1260  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 16;
1261  m_fixedData->m_infoList[index].m_gpsPvtVele =
1262  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 20;
1263  m_fixedData->m_infoList[index].m_gpsPvtVeld =
1264  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 24;
1265  m_fixedData->m_infoList[index].m_gpsPvtHacc =
1266  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 28;
1267  m_fixedData->m_infoList[index].m_gpsPvtVacc =
1268  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 32;
1269  m_fixedData->m_infoList[index].m_gpsPvtSacc =
1270  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 36;
1271  m_fixedData->m_infoList[index].m_gpsPvtGpsAge =
1272  m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 40;
1273 
1274  m_fixedData->m_infoList[index].m_size += (2 + 1) + (40 + 1);
1275  }
1276  // update
1277  m_msg.setDataShort(
1278  data.m_pressure, m_fixedData->m_infoList[index].m_gpsPvtPressure);
1279  m_msg.setDataByte(
1280  data.m_pressureAge, m_fixedData->m_infoList[index].m_gpsPvtPressureAge);
1281 
1282  // lon,lat,height,hacc,vacc,veln,vele,veld
1283  int32_t* bareln = (int32_t*)&data.m_itow;
1284  for (uint16_t i = 0; i < 10; ++i)
1285  m_msg.setDataLong(
1286  bareln[i], m_fixedData->m_infoList[index].m_gpsPvtGpsData +
1287  (4 * i)); // lint !e661 !e662
1288 
1289  // gpsAge
1290  m_msg.setDataByte(
1291  data.m_gpsAge, m_fixedData->m_infoList[index].m_gpsPvtGpsAge);
1292  return true;
1293 }
1294 
1295 /*! \brief Return the pressure data component of a data item.
1296  \param index The index of the item of which the data should be returned.
1297  \returns The pressure data component of a data item.
1298 */
1299 XsPressure LegacyDataPacket::pressure(int32_t index) const
1300 {
1301  XsPressure buffer;
1302  if (containsPressure(index))
1303  {
1304  // pressure data
1305  // MH; \todo need a conversion factor here to go from short to double?
1306  buffer.m_pressure = (double)m_msg.getDataShort(
1307  m_fixedData->m_infoList[index].m_gpsPvtPressure);
1308  // pressAge
1309  buffer.m_pressureAge = m_msg.getDataByte(
1310  m_fixedData->m_infoList[index].m_gpsPvtPressureAge);
1311  }
1312  else if (containsMtwSdiData(index))
1313  {
1314  // pressure data
1315  buffer.m_pressure = (double)m_msg.getDataShort(
1316  m_fixedData->m_infoList[index].m_wBaroMeter);
1317  // pressAge
1318  buffer.m_pressureAge =
1319  (m_msg.getDataByte(m_fixedData->m_infoList[index].m_wAidingData)
1320  ? 0
1321  : 255);
1322  }
1323  else
1324  buffer = XsPressure();
1325  return buffer;
1326 }
1327 
1328 /*! \brief Return true if the packet contains pressure data
1329 */
1330 bool LegacyDataPacket::containsPressure(int32_t index) const
1331 {
1332  if (dataSize(index) == 0) return false;
1333  if (m_fixedData->m_infoList[index].m_gpsPvtPressure ==
1334  XS_DATA_ITEM_NOT_AVAILABLE &&
1335  m_fixedData->m_infoList[index].m_wBaroMeter ==
1336  XS_DATA_ITEM_NOT_AVAILABLE)
1337  return false;
1338  return true;
1339 }
1340 
1341 /*! \brief Add/update pressure data for the item
1342 */
1343 bool LegacyDataPacket::setPressure(const XsPressure& data, int32_t index)
1344 {
1345  if (dataSize(index) == 0) return false;
1346  if (m_fixedData->m_infoList[index].m_gpsPvtPressure ==
1347  XS_DATA_ITEM_NOT_AVAILABLE)
1348  {
1349  // add
1350  m_fixedData->m_infoList[index].m_gpsPvtData =
1351  (uint16_t)m_msg.getDataSize();
1352  m_msg.resizeData(m_msg.getDataSize() + (2 + 1));
1353  m_fixedData->m_infoList[index].m_gpsPvtPressure =
1354  m_fixedData->m_infoList[index].m_gpsPvtData;
1355  m_fixedData->m_infoList[index].m_gpsPvtPressureAge =
1356  m_fixedData->m_infoList[index].m_gpsPvtData + 2;
1357 
1358  m_fixedData->m_infoList[index].m_size += (2 + 1);
1359  }
1360  // update
1361  m_msg.setDataShort(
1362  (uint16_t)data.m_pressure,
1363  m_fixedData->m_infoList[index].m_gpsPvtPressure);
1364  m_msg.setDataByte(
1365  data.m_pressureAge, m_fixedData->m_infoList[index].m_gpsPvtPressureAge);
1366 
1367  return true;
1368 }
1369 
1370 /*! \brief Return the strapdown integration (SDI) data component of a data item.
1371  \param index The index of the item of which the data should be returned.
1372  \returns The SDI data component of a data item.
1373 */
1374 MtwSdiData LegacyDataPacket::mtwSdiData(int32_t index) const
1375 {
1376  MtwSdiData buffer;
1377  if (containsMtwSdiData(index))
1378  {
1379  buffer.m_deviceId =
1380  m_msg.getDataLong(m_fixedData->m_infoList[index].m_wClientId);
1381  buffer.m_timeSync =
1382  m_msg.getDataByte(m_fixedData->m_infoList[index].m_wTimeSync);
1383  buffer.m_firstFrameNumber = m_msg.getDataShort(
1384  m_fixedData->m_infoList[index].m_wFirstFrameNumber);
1385  buffer.m_lastFrameNumber = m_msg.getDataShort(
1386  m_fixedData->m_infoList[index].m_wLastFrameNumber);
1387  m_msg.getDataFPValue(
1388  CHECKIFDOUBLE(m_wCurrentBias), &buffer.m_currentBias[0],
1389  m_fixedData->m_infoList[index].m_wCurrentBias, 3);
1390  buffer.m_aidingData =
1391  (m_msg.getDataByte(m_fixedData->m_infoList[index].m_wAidingData) ==
1392  1)
1393  ? true
1394  : false;
1395  buffer.m_barometer =
1396  m_msg.getDataShort(m_fixedData->m_infoList[index].m_wBaroMeter) /
1397  50.0;
1398  buffer.m_rssi =
1399  (int8_t)m_msg.getDataByte(m_fixedData->m_infoList[index].m_wRssi);
1400 
1401  m_msg.getDataFPValue(
1402  CHECKIFDOUBLE(m_wOrientationIncrement),
1403  &buffer.m_orientationIncrement[0],
1404  m_fixedData->m_infoList[index].m_wOrientationIncrement, 4);
1405  m_msg.getDataFPValue(
1406  CHECKIFDOUBLE(m_wVelocityIncrement), &buffer.m_velocityIncrement[0],
1407  m_fixedData->m_infoList[index].m_wVelocityIncrement, 3);
1408  m_msg.getDataFPValue(
1409  CHECKIFDOUBLE(m_wMagnetoMeter), &buffer.m_magnetoMeter[0],
1410  m_fixedData->m_infoList[index].m_wMagnetoMeter, 3);
1411  }
1412  else
1413  {
1414  buffer.m_deviceId = 0;
1415  buffer.m_timeSync = 0;
1416  buffer.m_firstFrameNumber = 0;
1417  buffer.m_lastFrameNumber = 0;
1418  buffer.m_currentBias.zero();
1419  buffer.m_aidingData = 0;
1420  buffer.m_barometer = 0;
1421  buffer.m_rssi = 0;
1422 
1423  buffer.m_orientationIncrement = XsQuaternion::identity();
1424  buffer.m_velocityIncrement.zero();
1425  buffer.m_magnetoMeter.zero();
1426  }
1427  return buffer;
1428 }
1429 
1430 /*! \brief Check if data item contains strapdown integration data
1431  \param index The index of the item of which the data should be returned.
1432  \returns true if the packet contains MTw SDI data
1433 */
1434 bool LegacyDataPacket::containsMtwSdiData(int32_t index) const
1435 {
1436  if (dataSize(index) == 0) return false;
1437  if (m_fixedData->m_infoList[index].m_mtwSdiData ==
1438  XS_DATA_ITEM_NOT_AVAILABLE)
1439  return false;
1440  return true;
1441 }
1442 
1443 /*! \brief Add/update strapdown integration data for the item
1444  \param data The updated data
1445  \param index The index of the item of which the data should be returned.
1446  \returns true if the data was successfully updated
1447 */
1448 bool LegacyDataPacket::setMtwSdiData(const MtwSdiData& data, int32_t index)
1449 {
1450  if (m_fixedData->m_infoList[index].m_mtwSdiData ==
1451  XS_DATA_ITEM_NOT_AVAILABLE)
1452  {
1453  // add
1454  m_fixedData->m_infoList[index].m_mtwSdiData =
1455  (uint16_t)m_msg.getDataSize();
1456  XsSize ds = 8; // add as doubles
1457  m_msg.resizeData(m_msg.getDataSize() + 13 + 13 * ds);
1458 
1459  m_fixedData->m_infoList[index].m_wClientId =
1460  m_fixedData->m_infoList[index].m_mtwSdiData;
1461  m_fixedData->m_infoList[index].m_wTimeSync =
1462  m_fixedData->m_infoList[index].m_mtwSdiData + 4;
1463  m_fixedData->m_infoList[index].m_wFirstFrameNumber =
1464  m_fixedData->m_infoList[index].m_mtwSdiData + 5;
1465  m_fixedData->m_infoList[index].m_wLastFrameNumber =
1466  m_fixedData->m_infoList[index].m_mtwSdiData + 7;
1467  m_fixedData->m_infoList[index].m_wCurrentBias =
1468  m_fixedData->m_infoList[index].m_mtwSdiData + 9;
1469  m_fixedData->m_infoList[index].m_wOrientationIncrement = (uint16_t)(
1470  m_fixedData->m_infoList[index].m_mtwSdiData + 9 + 3 * ds);
1471  m_fixedData->m_infoList[index].m_wVelocityIncrement = (uint16_t)(
1472  m_fixedData->m_infoList[index].m_mtwSdiData + 9 + 7 * ds);
1473  m_fixedData->m_infoList[index].m_wAidingData = (uint16_t)(
1474  m_fixedData->m_infoList[index].m_mtwSdiData + 9 + 10 * ds);
1475  m_fixedData->m_infoList[index].m_wBaroMeter = (uint16_t)(
1476  m_fixedData->m_infoList[index].m_mtwSdiData + 10 + 10 * ds);
1477  m_fixedData->m_infoList[index].m_wMagnetoMeter = (uint16_t)(
1478  m_fixedData->m_infoList[index].m_mtwSdiData + 12 + 10 * ds);
1479  m_fixedData->m_infoList[index].m_wRssi = (uint16_t)(
1480  m_fixedData->m_infoList[index].m_mtwSdiData + 12 + 13 * ds);
1481  m_fixedData->m_infoList[index].m_size += (uint16_t)(13 + 13 * ds);
1482  }
1483  // update
1484  m_msg.setDataLong(
1485  data.m_deviceId.toInt(), m_fixedData->m_infoList[index].m_wClientId);
1486  m_msg.setDataByte(
1487  data.m_timeSync, m_fixedData->m_infoList[index].m_wTimeSync);
1488  m_msg.setDataShort(
1489  data.m_firstFrameNumber,
1490  m_fixedData->m_infoList[index].m_wFirstFrameNumber);
1491  m_msg.setDataShort(
1492  data.m_lastFrameNumber,
1493  m_fixedData->m_infoList[index].m_wLastFrameNumber);
1494  m_msg.setDataFPValue(
1495  CHECKIFDOUBLE(m_wCurrentBias), data.m_currentBias.data(),
1496  m_fixedData->m_infoList[index].m_wCurrentBias, 3);
1497  m_msg.setDataFPValue(
1498  CHECKIFDOUBLE(m_wOrientationIncrement),
1499  data.m_orientationIncrement.data(),
1500  m_fixedData->m_infoList[index].m_wOrientationIncrement, 4);
1501  m_msg.setDataFPValue(
1502  CHECKIFDOUBLE(m_wVelocityIncrement), data.m_velocityIncrement.data(),
1503  m_fixedData->m_infoList[index].m_wVelocityIncrement, 3);
1504  m_msg.setDataByte(
1505  (data.m_aidingData == true) ? 1 : 0,
1506  m_fixedData->m_infoList[index].m_wAidingData);
1507  m_msg.setDataShort(
1508  (uint16_t)(XsMath_doubleToLong(data.m_barometer * 50)),
1509  m_fixedData->m_infoList[index].m_wBaroMeter);
1510  m_msg.setDataFPValue(
1511  CHECKIFDOUBLE(m_wMagnetoMeter), data.m_magnetoMeter.data(),
1512  m_fixedData->m_infoList[index].m_wMagnetoMeter, 3);
1513  m_msg.setDataByte(data.m_rssi, m_fixedData->m_infoList[index].m_wRssi);
1514 
1515  return true;
1516 }
1517 
1518 //////////////////////////////////////////////////////////////////////////////////////////
1519 // Return the Temperature component of a data item.
1520 double LegacyDataPacket::temperature(int32_t index, int channel) const
1521 {
1522  if (containsTemperature(index, channel))
1523  return m_msg.getDataFPValue(
1524  CHECKIFDOUBLE(m_temp[channel]),
1525  m_fixedData->m_infoList[index].m_temp[channel]);
1526 
1527  return 0.0;
1528 }
1529 bool LegacyDataPacket::containsTemperature(int32_t index, int channel) const
1530 {
1531  if (dataSize(index) == 0) return false;
1532  if (channel >= XS_MAX_TEMPERATURE_CHANNELS) return false;
1533  if (m_fixedData->m_infoList[index].m_temp[channel] ==
1534  XS_DATA_ITEM_NOT_AVAILABLE)
1535  return false;
1536  return true;
1537 }
1538 bool LegacyDataPacket::setTemperature(
1539  const double& temp, int32_t index, int channel)
1540 {
1541  if (dataSize(index) == 0) return false;
1542  if (channel >= XS_MAX_TEMPERATURE_CHANNELS) return false;
1543 
1544  uint16_t ds = getFPValueSize(index);
1545 
1546  if (m_fixedData->m_infoList[index].m_temp[channel] ==
1547  XS_DATA_ITEM_NOT_AVAILABLE ||
1548  !ISDOUBLE(m_temp[channel]))
1549  {
1550  // add
1551  ds = 8;
1552  // m_msg.m_autoUpdateChecksum = false;
1553  int c = temperatureChannelCount(index);
1554  for (int i = c; i <= channel;
1555  i++) // Add (also add missing intermediate channels)
1556  {
1557  m_fixedData->m_infoList[index].m_temp[i] =
1558  (uint16_t)m_msg.getDataSize();
1559  m_msg.resizeData(m_msg.getDataSize() + ds);
1560  m_fixedData->m_infoList[index].m_size += ds;
1561  }
1562  }
1563  // update
1564  m_msg.setDataFPValue(
1565  CHECKIFDOUBLE(m_temp[channel]), temp,
1566  m_fixedData->m_infoList[index].m_temp[channel]);
1567  return true;
1568 }
1569 int LegacyDataPacket::temperatureChannelCount(int32_t index) const
1570 {
1571  for (int i = 0; i < XS_MAX_TEMPERATURE_CHANNELS; i++)
1572  {
1573  if (m_fixedData->m_infoList[index].m_temp[i] ==
1574  XS_DATA_ITEM_NOT_AVAILABLE)
1575  return i;
1576  }
1577  return XS_MAX_TEMPERATURE_CHANNELS;
1578 }
1579 //////////////////////////////////////////////////////////////////////////////////////////
1580 // Return the Calibrated Accelerometer component of a data item.
1581 XsVector LegacyDataPacket::calibratedAcceleration(int32_t index) const
1582 {
1583  XsVector3 buffer;
1584  if (containsCalibratedAcceleration(index))
1585  m_msg.getDataFPValue(
1586  CHECKIFDOUBLE(m_calAcc), &buffer[0],
1587  m_fixedData->m_infoList[index].m_calAcc, 3);
1588  else
1589  buffer.zero();
1590  return buffer;
1591 }
1592 bool LegacyDataPacket::containsCalibratedAcceleration(int32_t index) const
1593 {
1594  if (dataSize(index) == 0) return false;
1595  if (m_fixedData->m_infoList[index].m_calAcc == XS_DATA_ITEM_NOT_AVAILABLE)
1596  return false;
1597  return true;
1598 }
1599 bool LegacyDataPacket::setCalibratedAcceleration(
1600  const XsVector& vec, int32_t index)
1601 {
1602  const uint16_t numValues = 3;
1603  if (dataSize(index) == 0) return false;
1604 
1605  uint16_t ds = getFPValueSize(index);
1606 
1607  if (m_fixedData->m_infoList[index].m_calAcc == XS_DATA_ITEM_NOT_AVAILABLE ||
1608  !ISDOUBLE(m_calAcc))
1609  {
1610  // add
1611  ds = 8;
1612  // m_msg.m_autoUpdateChecksum = false;
1613 
1614  m_fixedData->m_infoList[index].m_calAcc = (uint16_t)m_msg.getDataSize();
1615  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1616  m_fixedData->m_infoList[index].m_size += numValues * ds;
1617  }
1618  // update
1619  m_msg.setDataFPValue(
1620  CHECKIFDOUBLE(m_calAcc), vec.data(),
1621  m_fixedData->m_infoList[index].m_calAcc, numValues);
1622  return true;
1623 }
1624 
1625 //////////////////////////////////////////////////////////////////////////////////////////
1626 // Return the Calibrated Gyroscope component of a data item.
1627 XsVector LegacyDataPacket::calibratedGyroscopeData(int32_t index) const
1628 {
1629  XsVector3 buffer;
1630  if (containsCalibratedGyroscopeData(index))
1631  m_msg.getDataFPValue(
1632  CHECKIFDOUBLE(m_calGyr), &buffer[0],
1633  m_fixedData->m_infoList[index].m_calGyr, 3);
1634  else
1635  buffer.zero();
1636  return buffer;
1637 }
1638 bool LegacyDataPacket::containsCalibratedGyroscopeData(int32_t index) const
1639 {
1640  if (dataSize(index) == 0) return false;
1641  if (m_fixedData->m_infoList[index].m_calGyr == XS_DATA_ITEM_NOT_AVAILABLE)
1642  return false;
1643  return true;
1644 }
1645 bool LegacyDataPacket::setCalibratedGyroscopeData(
1646  const XsVector& vec, int32_t index)
1647 {
1648  const uint16_t numValues = 3;
1649  if (dataSize(index) == 0) return false;
1650 
1651  uint16_t ds = getFPValueSize(index);
1652 
1653  if (m_fixedData->m_infoList[index].m_calGyr == XS_DATA_ITEM_NOT_AVAILABLE ||
1654  !ISDOUBLE(m_calGyr))
1655  {
1656  // add
1657  ds = 8;
1658  // m_msg.m_autoUpdateChecksum = false;
1659 
1660  m_fixedData->m_infoList[index].m_calGyr = (uint16_t)m_msg.getDataSize();
1661  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1662  m_fixedData->m_infoList[index].m_size += numValues * ds;
1663  }
1664  // update
1665  m_msg.setDataFPValue(
1666  CHECKIFDOUBLE(m_calGyr), vec.data(),
1667  m_fixedData->m_infoList[index].m_calGyr, numValues);
1668  return true;
1669 }
1670 
1671 //////////////////////////////////////////////////////////////////////////////////////////
1672 // Return the Calibrated Magnetometer component of a data item.
1673 XsVector LegacyDataPacket::calibratedMagneticField(int32_t index) const
1674 {
1675  XsVector3 buffer;
1676  if (containsCalibratedMagneticField(index))
1677  m_msg.getDataFPValue(
1678  CHECKIFDOUBLE(m_calMag), &buffer[0],
1679  m_fixedData->m_infoList[index].m_calMag, 3);
1680  else
1681  buffer.zero();
1682  return buffer;
1683 }
1684 bool LegacyDataPacket::containsCalibratedMagneticField(int32_t index) const
1685 {
1686  if (dataSize(index) == 0) return false;
1687  if (m_fixedData->m_infoList[index].m_calMag == XS_DATA_ITEM_NOT_AVAILABLE)
1688  return false;
1689  return true;
1690 }
1691 bool LegacyDataPacket::setCalibratedMagneticField(
1692  const XsVector& vec, int32_t index)
1693 {
1694  const uint16_t numValues = 3;
1695  if (dataSize(index) == 0) return false;
1696 
1697  uint16_t ds = getFPValueSize(index);
1698 
1699  if (m_fixedData->m_infoList[index].m_calMag == XS_DATA_ITEM_NOT_AVAILABLE ||
1700  !ISDOUBLE(m_calMag))
1701  {
1702  // add
1703  ds = 8;
1704  // m_msg.m_autoUpdateChecksum = false;
1705 
1706  m_fixedData->m_infoList[index].m_calMag = (uint16_t)m_msg.getDataSize();
1707  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1708  m_fixedData->m_infoList[index].m_size += numValues * ds;
1709  }
1710  // update
1711  m_msg.setDataFPValue(
1712  CHECKIFDOUBLE(m_calMag), vec.data(),
1713  m_fixedData->m_infoList[index].m_calMag, numValues);
1714  return true;
1715 }
1716 
1717 /*! \brief Return the Calibrated Data component of a data item.
1718  \param index The index of the item of which the data should be returned.
1719  \returns The Calibrated Data component of a data item.
1720 */
1721 XsCalibratedData LegacyDataPacket::calibratedData(int32_t index) const
1722 {
1723  XsCalibratedData buffer;
1724  if (containsCalibratedData(index))
1725  {
1726  // lint --e{419}
1727  double* bare = &buffer.m_acc[0];
1728  if (m_fixedData->m_infoList[index].m_calAcc ==
1729  XS_DATA_ITEM_NOT_AVAILABLE)
1730  memset(bare, 0, 3 * sizeof(double));
1731  else
1732  m_msg.getDataFPValue(
1733  CHECKIFDOUBLE(m_calAcc), bare,
1734  m_fixedData->m_infoList[index].m_calAcc, 3);
1735 
1736  bare = &buffer.m_gyr[0];
1737  if (m_fixedData->m_infoList[index].m_calGyr ==
1738  XS_DATA_ITEM_NOT_AVAILABLE)
1739  memset(bare, 0, 3 * sizeof(double));
1740  else
1741  m_msg.getDataFPValue(
1742  CHECKIFDOUBLE(m_calGyr), bare,
1743  m_fixedData->m_infoList[index].m_calGyr, 3);
1744 
1745  bare = &buffer.m_mag[0];
1746  if (m_fixedData->m_infoList[index].m_calMag ==
1747  XS_DATA_ITEM_NOT_AVAILABLE)
1748  memset(bare, 0, 3 * sizeof(double));
1749  else
1750  m_msg.getDataFPValue(
1751  CHECKIFDOUBLE(m_calMag), bare,
1752  m_fixedData->m_infoList[index].m_calMag, 3);
1753  }
1754  else
1755  {
1756  buffer.m_acc.zero();
1757  buffer.m_gyr.zero();
1758  buffer.m_mag.zero();
1759  }
1760  return buffer;
1761 }
1762 
1763 /*! \brief Check if data item contains Calibrated Data
1764  \param index The index of the item of which the data should be returned.
1765  \returns true if the packet contains Calibrated Data
1766 */
1767 bool LegacyDataPacket::containsCalibratedData(int32_t index) const
1768 {
1769  if (dataSize(index) == 0) return false;
1770  return (
1771  m_fixedData->m_infoList[index].m_calData != XS_DATA_ITEM_NOT_AVAILABLE);
1772 }
1773 
1774 /*! \brief Add/update Calibrated Data for the item */
1775 bool LegacyDataPacket::setCalibratedData(
1776  const XsCalibratedData& data, int32_t index)
1777 {
1778  const uint16_t numValues = 9;
1779  if (dataSize(index) == 0) return false;
1780 
1781  uint16_t ds = getFPValueSize(index);
1782 
1783  if (m_fixedData->m_infoList[index].m_calData ==
1784  XS_DATA_ITEM_NOT_AVAILABLE ||
1785  !ISDOUBLE(m_calData))
1786  {
1787  // add
1788  ds = 8; // added values are always in double precision
1789  // m_msg.m_autoUpdateChecksum = false;
1790 
1791  m_fixedData->m_infoList[index].m_calData =
1792  (uint16_t)m_msg.getDataSize();
1793  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1794  m_fixedData->m_infoList[index].m_calAcc =
1795  m_fixedData->m_infoList[index].m_calData;
1796  m_fixedData->m_infoList[index].m_calGyr =
1797  m_fixedData->m_infoList[index].m_calData + 3 * ds;
1798  m_fixedData->m_infoList[index].m_calMag =
1799  m_fixedData->m_infoList[index].m_calData + 6 * ds;
1800  m_fixedData->m_infoList[index].m_size += numValues * ds;
1801  }
1802  // update
1803  double* bare = (double*)&data;
1804  if (m_fixedData->m_infoList[index].m_calAcc != XS_DATA_ITEM_NOT_AVAILABLE)
1805  m_msg.setDataFPValue(
1806  CHECKIFDOUBLE(m_calAcc), bare,
1807  m_fixedData->m_infoList[index].m_calAcc, 3);
1808  bare += 3;
1809  if (m_fixedData->m_infoList[index].m_calGyr != XS_DATA_ITEM_NOT_AVAILABLE)
1810  m_msg.setDataFPValue(
1811  CHECKIFDOUBLE(m_calGyr), bare,
1812  m_fixedData->m_infoList[index].m_calGyr, 3);
1813  bare += 3;
1814  if (m_fixedData->m_infoList[index].m_calMag != XS_DATA_ITEM_NOT_AVAILABLE)
1815  m_msg.setDataFPValue(
1816  CHECKIFDOUBLE(m_calMag), bare,
1817  m_fixedData->m_infoList[index].m_calMag, 3);
1818 
1819  return true;
1820 }
1821 
1822 //////////////////////////////////////////////////////////////////////////////////////////
1823 // Return the Orientation component of a data item as a Quaternion.
1824 XsQuaternion LegacyDataPacket::orientationQuaternion(int32_t index) const
1825 {
1826  XsQuaternion buffer;
1827  if (containsOrientationQuaternion(index))
1828  m_msg.getDataFPValue(
1829  CHECKIFDOUBLE(m_oriQuat), &buffer[0],
1830  m_fixedData->m_infoList[index].m_oriQuat, 4);
1831  else
1832  memset(&buffer, 0, sizeof(buffer));
1833  return buffer;
1834 }
1835 
1836 bool LegacyDataPacket::containsOrientationQuaternion(int32_t index) const
1837 {
1838  if (dataSize(index) == 0) return false;
1839  if (m_fixedData->m_infoList[index].m_oriQuat == XS_DATA_ITEM_NOT_AVAILABLE)
1840  return false;
1841  return true;
1842 }
1843 bool LegacyDataPacket::setOrientationQuaternion(
1844  const XsQuaternion& data, int32_t index)
1845 {
1846  const uint16_t numValues = 4;
1847  if (dataSize(index) == 0) return false;
1848 
1849  uint16_t ds = getFPValueSize(index);
1850 
1851  if (m_fixedData->m_infoList[index].m_oriQuat ==
1852  XS_DATA_ITEM_NOT_AVAILABLE ||
1853  !ISDOUBLE(m_oriQuat))
1854  {
1855  // add
1856  ds = 8; // added values are always in double precision
1857  // m_msg.m_autoUpdateChecksum = false;
1858 
1859  m_fixedData->m_infoList[index].m_oriQuat =
1860  (uint16_t)m_msg.getDataSize();
1861  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1862  m_fixedData->m_infoList[index].m_size += numValues * ds;
1863 
1864  double* bare = (double*)&data;
1865  m_msg.setDataFPValue(
1866  (m_fixedData->m_formatList[index].m_outputSettings &
1867  XOS_Dataformat_Mask) |
1868  XOS_Dataformat_Double,
1869  bare, m_fixedData->m_infoList[index].m_oriQuat, numValues);
1870  return true;
1871  }
1872  // update
1873  m_msg.setDataFPValue(
1874  CHECKIFDOUBLE(m_oriQuat), data.data(),
1875  m_fixedData->m_infoList[index].m_oriQuat, numValues);
1876  return true;
1877 }
1878 
1879 //////////////////////////////////////////////////////////////////////////////////////////
1880 // Return the Orientation component of a data item as XsEuler angles.
1881 XsEuler LegacyDataPacket::orientationEuler(int32_t index) const
1882 {
1883  XsEuler buffer;
1884  if (containsOrientationEuler(index))
1885  {
1886  m_msg.getDataFPValue(
1887  CHECKIFDOUBLE(m_oriEul), &buffer[0],
1888  m_fixedData->m_infoList[index].m_oriEul, 3);
1889  }
1890  else
1891  memset(&buffer, 0, sizeof(buffer));
1892  return buffer;
1893 }
1894 bool LegacyDataPacket::containsOrientationEuler(int32_t index) const
1895 {
1896  if (dataSize(index) == 0) return false;
1897  if (m_fixedData->m_infoList[index].m_oriEul == XS_DATA_ITEM_NOT_AVAILABLE)
1898  return false;
1899  return true;
1900 }
1901 bool LegacyDataPacket::setOrientationEuler(const XsEuler& data, int32_t index)
1902 {
1903  const uint16_t numValues = 3;
1904  if (dataSize(index) == 0) return false;
1905 
1906  uint16_t ds = getFPValueSize(index);
1907 
1908  if (m_fixedData->m_infoList[index].m_oriEul == XS_DATA_ITEM_NOT_AVAILABLE ||
1909  !ISDOUBLE(m_oriEul))
1910  {
1911  // add
1912  ds = 8; // added values are always in double precision
1913  // m_msg.m_autoUpdateChecksum = false;
1914 
1915  m_fixedData->m_infoList[index].m_oriEul = (uint16_t)m_msg.getDataSize();
1916  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1917  m_fixedData->m_infoList[index].m_size += numValues * ds;
1918  }
1919  // update
1920  m_msg.setDataFPValue(
1921  CHECKIFDOUBLE(m_oriEul), data.data(),
1922  m_fixedData->m_infoList[index].m_oriEul, numValues);
1923  return true;
1924 }
1925 
1926 //////////////////////////////////////////////////////////////////////////////////////////
1927 // Return the Orientation component of a data item as an Orientation Matrix.
1928 XsMatrix LegacyDataPacket::orientationMatrix(int32_t index) const
1929 {
1930  XsMatrix3x3 buffer;
1931  uint16_t k = 0;
1932  if (containsOrientationMatrix(index))
1933  {
1934  // remember to use column major order in the xbus message!
1935  uint16_t ds = getFPValueSize(index);
1936  for (int32_t i = 0; i < 3; ++i)
1937  for (int32_t j = 0; j < 3; ++j, k += ds)
1938  buffer.setValue(
1939  j, i, m_msg.getDataFPValue(
1940  CHECKIFDOUBLE(m_oriMat),
1941  m_fixedData->m_infoList[index].m_oriMat + k));
1942  }
1943  else
1944  buffer.zero();
1945 
1946  return buffer;
1947 }
1948 bool LegacyDataPacket::containsOrientationMatrix(int32_t index) const
1949 {
1950  if (dataSize(index) == 0) return false;
1951  if (m_fixedData->m_infoList[index].m_oriMat == XS_DATA_ITEM_NOT_AVAILABLE)
1952  return false;
1953  return true;
1954 }
1955 bool LegacyDataPacket::setOrientationMatrix(const XsMatrix& data, int32_t index)
1956 {
1957  const uint16_t numValues = 9;
1958  if (dataSize(index) == 0) return false;
1959 
1960  uint16_t ds = getFPValueSize(index);
1961 
1962  // remember to use column major order in the xbus message!
1963  if (m_fixedData->m_infoList[index].m_oriMat == XS_DATA_ITEM_NOT_AVAILABLE ||
1964  !ISDOUBLE(m_oriMat))
1965  {
1966  // add
1967  ds = 8; // added values are always in double precision
1968  // m_msg.m_autoUpdateChecksum = false;
1969 
1970  m_fixedData->m_infoList[index].m_oriMat = (uint16_t)m_msg.getDataSize();
1971  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
1972  m_fixedData->m_infoList[index].m_size += numValues * ds;
1973  }
1974  // update
1975  uint16_t k = 0;
1976  for (int32_t i = 0; i < 3; ++i)
1977  for (int32_t j = 0; j < 3; ++j, k += ds)
1978  m_msg.setDataFPValue(
1979  CHECKIFDOUBLE(m_oriMat), data.value(j, i),
1980  m_fixedData->m_infoList[index].m_oriMat + k);
1981  return true;
1982 }
1983 
1984 bool LegacyDataPacket::containsOrientation(int32_t index) const
1985 {
1986  if (dataSize(index) == 0) return false;
1987  if (m_fixedData->m_infoList[index].m_oriEul == XS_DATA_ITEM_NOT_AVAILABLE &&
1988  m_fixedData->m_infoList[index].m_oriMat == XS_DATA_ITEM_NOT_AVAILABLE &&
1989  m_fixedData->m_infoList[index].m_oriQuat == XS_DATA_ITEM_NOT_AVAILABLE)
1990  return false;
1991  return true;
1992 }
1993 
1994 //////////////////////////////////////////////////////////////////////////////////////////
1995 // Return the AnalogIn 1 component of a data item.
1996 XsAnalogInData LegacyDataPacket::analogIn1Data(int32_t index) const
1997 {
1998  XsAnalogInData buffer;
1999  if (containsAnalogIn1Data(index))
2000  buffer.m_data =
2001  m_msg.getDataShort(m_fixedData->m_infoList[index].m_analogIn1);
2002 
2003  return buffer;
2004 }
2005 bool LegacyDataPacket::containsAnalogIn1Data(int32_t index) const
2006 {
2007  if (dataSize(index) == 0) return false;
2008  if (m_fixedData->m_infoList[index].m_analogIn1 ==
2009  XS_DATA_ITEM_NOT_AVAILABLE)
2010  return false;
2011  return true;
2012 }
2013 bool LegacyDataPacket::setAnalogIn1Data(
2014  const XsAnalogInData& data, int32_t index)
2015 {
2016  if (dataSize(index) == 0) return false;
2017  if (m_fixedData->m_infoList[index].m_analogIn1 ==
2018  XS_DATA_ITEM_NOT_AVAILABLE)
2019  {
2020  // add
2021  m_fixedData->m_infoList[index].m_analogIn1 =
2022  (uint16_t)m_msg.getDataSize();
2023  m_msg.resizeData(m_msg.getDataSize() + 2);
2024  m_fixedData->m_infoList[index].m_size += 2;
2025  }
2026  // update
2027  m_msg.setDataShort(data.m_data, m_fixedData->m_infoList[index].m_analogIn1);
2028  return true;
2029 }
2030 
2031 //////////////////////////////////////////////////////////////////////////////////////////
2032 // Return the AnalogIn 2 component of a data item.
2033 XsAnalogInData LegacyDataPacket::analogIn2Data(int32_t index) const
2034 {
2035  XsAnalogInData buffer;
2036  if (containsAnalogIn2Data(index))
2037  buffer.m_data =
2038  m_msg.getDataShort(m_fixedData->m_infoList[index].m_analogIn2);
2039 
2040  return buffer;
2041 }
2042 bool LegacyDataPacket::containsAnalogIn2Data(int32_t index) const
2043 {
2044  if (dataSize(index) == 0) return false;
2045  if (m_fixedData->m_infoList[index].m_analogIn2 ==
2046  XS_DATA_ITEM_NOT_AVAILABLE)
2047  return false;
2048  return true;
2049 }
2050 bool LegacyDataPacket::setAnalogIn2Data(
2051  const XsAnalogInData& data, int32_t index)
2052 {
2053  if (dataSize(index) == 0) return false;
2054  if (m_fixedData->m_infoList[index].m_analogIn2 ==
2055  XS_DATA_ITEM_NOT_AVAILABLE)
2056  {
2057  // add
2058  m_fixedData->m_infoList[index].m_analogIn2 =
2059  (uint16_t)m_msg.getDataSize();
2060  m_msg.resizeData(m_msg.getDataSize() + 2);
2061  m_fixedData->m_infoList[index].m_size += 2;
2062  }
2063  // update
2064  m_msg.setDataShort(data.m_data, m_fixedData->m_infoList[index].m_analogIn2);
2065  return true;
2066 }
2067 
2068 //////////////////////////////////////////////////////////////////////////////////////////
2069 // Return the Position LLA component of a data item.
2070 XsVector LegacyDataPacket::positionLLA(int32_t index) const
2071 {
2072  XsVector3 buffer;
2073  if (containsPositionLLA(index))
2074  m_msg.getDataFPValue(
2075  CHECKIFDOUBLE(m_posLLA), &buffer[0],
2076  m_fixedData->m_infoList[index].m_posLLA, 3);
2077  else
2078  buffer.zero();
2079  return buffer;
2080 }
2081 bool LegacyDataPacket::containsPositionLLA(int32_t index) const
2082 {
2083  if (dataSize(index) == 0) return false;
2084  if (m_fixedData->m_infoList[index].m_posLLA == XS_DATA_ITEM_NOT_AVAILABLE)
2085  return false;
2086  return true;
2087 }
2088 bool LegacyDataPacket::setPositionLLA(const XsVector& data, int32_t index)
2089 {
2090  const uint16_t numValues = 3;
2091  if (dataSize(index) == 0) return false;
2092 
2093  uint16_t ds = getFPValueSize(index);
2094 
2095  if (m_fixedData->m_infoList[index].m_posLLA == XS_DATA_ITEM_NOT_AVAILABLE ||
2096  !ISDOUBLE(m_posLLA))
2097  {
2098  // add
2099  ds = 8; // added values are always in double precision
2100  // m_msg.m_autoUpdateChecksum = false;
2101 
2102  m_fixedData->m_infoList[index].m_posLLA = (uint16_t)m_msg.getDataSize();
2103  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
2104  m_fixedData->m_infoList[index].m_size += numValues * ds;
2105  }
2106  // update
2107  m_msg.setDataFPValue(
2108  CHECKIFDOUBLE(m_posLLA), data.data(),
2109  m_fixedData->m_infoList[index].m_posLLA, numValues);
2110  return true;
2111 }
2112 
2113 //////////////////////////////////////////////////////////////////////////////////////////
2114 // Return the Velocity NED component of a data item.
2115 XsVector LegacyDataPacket::velocity(int32_t index) const
2116 {
2117  XsVector3 buffer;
2118  if (containsVelocity(index))
2119  m_msg.getDataFPValue(
2120  CHECKIFDOUBLE(m_velNEDorNWU), &buffer[0],
2121  m_fixedData->m_infoList[index].m_velNEDorNWU, 3);
2122  else
2123  buffer.zero();
2124  return buffer;
2125 }
2126 bool LegacyDataPacket::containsVelocity(int32_t index) const
2127 {
2128  if (dataSize(index) == 0) return false;
2129  if (m_fixedData->m_infoList[index].m_velNEDorNWU ==
2130  XS_DATA_ITEM_NOT_AVAILABLE)
2131  return false;
2132  return true;
2133 }
2134 bool LegacyDataPacket::setVelocity(const XsVector& data, int32_t index)
2135 {
2136  const uint16_t numValues = 3;
2137  if (dataSize(index) == 0) return false;
2138 
2139  uint16_t ds = getFPValueSize(index);
2140 
2141  if (m_fixedData->m_infoList[index].m_velNEDorNWU ==
2142  XS_DATA_ITEM_NOT_AVAILABLE ||
2143  !ISDOUBLE(m_posLLA))
2144  {
2145  // add
2146  ds = 8; // added values are always in double precision
2147  // m_msg.m_autoUpdateChecksum = false;
2148 
2149  m_fixedData->m_infoList[index].m_velNEDorNWU =
2150  (uint16_t)m_msg.getDataSize();
2151  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
2152  m_fixedData->m_infoList[index].m_size += numValues * ds;
2153  }
2154  // update
2155  m_msg.setDataFPValue(
2156  CHECKIFDOUBLE(m_velNEDorNWU), data.data(),
2157  m_fixedData->m_infoList[index].m_velNEDorNWU, numValues);
2158  return true;
2159 }
2160 
2161 //////////////////////////////////////////////////////////////////////////////////////////
2162 // Return the Status component of a data item.
2163 uint32_t LegacyDataPacket::status(int32_t index, bool* outIsDetailed) const
2164 {
2165  assert(outIsDetailed != 0);
2166  if (containsStatus(index))
2167  {
2168  if (m_fixedData->m_infoList[index].m_detailedStatus ==
2169  XS_DATA_ITEM_NOT_AVAILABLE)
2170  {
2171  *outIsDetailed = false;
2172  return m_msg.getDataByte(m_fixedData->m_infoList[index].m_status);
2173  }
2174  else
2175  {
2176  *outIsDetailed = true;
2177  uint32_t tmp = m_msg.getDataLong(
2178  m_fixedData->m_infoList[index].m_detailedStatus);
2179  return tmp;
2180  }
2181  }
2182  else
2183  {
2184  return 0;
2185  }
2186 }
2187 bool LegacyDataPacket::containsStatus(int32_t index) const
2188 {
2189  if (dataSize(index) == 0) return false;
2190  if (m_fixedData->m_infoList[index].m_status == XS_DATA_ITEM_NOT_AVAILABLE)
2191  return false;
2192  return true;
2193 }
2194 bool LegacyDataPacket::containsDetailedStatus(int32_t index) const
2195 {
2196  if (dataSize(index) == 0) return false;
2197  if (m_fixedData->m_infoList[index].m_detailedStatus ==
2198  XS_DATA_ITEM_NOT_AVAILABLE)
2199  return false;
2200  return true;
2201 }
2202 bool LegacyDataPacket::setStatus(const uint32_t data, int32_t index)
2203 {
2204  if (dataSize(index) == 0) return false;
2205 
2206  if (m_fixedData->m_infoList[index].m_status == XS_DATA_ITEM_NOT_AVAILABLE)
2207  {
2208  // add
2209  m_fixedData->m_infoList[index].m_status = (uint16_t)m_msg.getDataSize();
2210  m_fixedData->m_infoList[index].m_detailedStatus =
2211  (uint16_t)m_msg.getDataSize();
2212  m_msg.resizeData(m_msg.getDataSize() + 4);
2213  m_fixedData->m_infoList[index].m_size += 4;
2214  }
2215  // update
2216  if (m_fixedData->m_infoList[index].m_detailedStatus ==
2217  XS_DATA_ITEM_NOT_AVAILABLE)
2218  m_msg.setDataByte(data & 0xFF, m_fixedData->m_infoList[index].m_status);
2219  else
2220  m_msg.setDataLong(
2221  data, m_fixedData->m_infoList[index].m_detailedStatus);
2222 
2223  return true;
2224 }
2225 
2226 //////////////////////////////////////////////////////////////////////////////////////////
2227 // Return the Sample Counter component of the packet.
2228 uint16_t LegacyDataPacket::packetCounter(int32_t index) const
2229 {
2230  if (!containsPacketCounter(index)) return 0;
2231  return m_msg.getDataShort(m_fixedData->m_infoList[index].m_sc);
2232 }
2233 bool LegacyDataPacket::containsPacketCounter(int32_t index) const
2234 {
2235  if (dataSize(index) == 0) return false;
2236  if (m_fixedData->m_infoList[index].m_sc == XS_DATA_ITEM_NOT_AVAILABLE)
2237  return false;
2238  return true;
2239 }
2240 bool LegacyDataPacket::setPacketCounter(const uint16_t counter, int32_t index)
2241 {
2242  if (dataSize(index) == 0) return false;
2243  if (m_fixedData->m_infoList[index].m_sc == XS_DATA_ITEM_NOT_AVAILABLE)
2244  {
2245  // add
2246  m_fixedData->m_infoList[index].m_sc = (uint16_t)m_msg.getDataSize();
2247  m_msg.resizeData(m_msg.getDataSize() + 2);
2248  m_fixedData->m_infoList[index].m_size += 2;
2249  }
2250  // update
2251  m_msg.setDataShort(counter, m_fixedData->m_infoList[index].m_sc);
2252  return true;
2253 }
2254 
2255 //////////////////////////////////////////////////////////////////////////////////////////
2256 // Return the UTC Time component of the packet.
2257 XsUtcTime LegacyDataPacket::utcTime(int32_t index) const
2258 {
2259  XsUtcTime buffer;
2260  if (containsUtcTime(index))
2261  {
2262  buffer.m_nano =
2263  m_msg.getDataLong(m_fixedData->m_infoList[index].m_utcNano);
2264  buffer.m_year =
2265  m_msg.getDataShort(m_fixedData->m_infoList[index].m_utcYear);
2266 
2267  // month, day, hour, minute, second and valid
2268  uint8_t* bareByte = (uint8_t*)&buffer.m_month;
2269  for (uint16_t i = 0; i < 6; ++i)
2270  bareByte[i] = m_msg.getDataByte(
2271  m_fixedData->m_infoList[index].m_utcMonth +
2272  i); // lint !e661 !e662
2273  }
2274  else
2275  buffer = XsUtcTime();
2276  return buffer;
2277 }
2278 bool LegacyDataPacket::containsUtcTime(int32_t index) const
2279 {
2280  if (dataSize(index) == 0) return false;
2281  if (m_fixedData->m_infoList[index].m_utcTime == XS_DATA_ITEM_NOT_AVAILABLE)
2282  return false;
2283  return true;
2284 }
2285 bool LegacyDataPacket::setUtcTime(const XsUtcTime& data, int32_t index)
2286 {
2287  if (dataSize(index) == 0) return false;
2288  if (m_fixedData->m_infoList[index].m_utcTime == XS_DATA_ITEM_NOT_AVAILABLE)
2289  {
2290  // add
2291  m_fixedData->m_infoList[index].m_utcTime =
2292  (uint16_t)m_msg.getDataSize();
2293  m_msg.resizeData(m_msg.getDataSize() + 12);
2294  m_fixedData->m_infoList[index].m_utcNano =
2295  m_fixedData->m_infoList[index].m_utcTime;
2296  m_fixedData->m_infoList[index].m_utcYear =
2297  m_fixedData->m_infoList[index].m_utcTime + 4;
2298  m_fixedData->m_infoList[index].m_utcMonth =
2299  m_fixedData->m_infoList[index].m_utcTime + 6;
2300  m_fixedData->m_infoList[index].m_utcDay =
2301  m_fixedData->m_infoList[index].m_utcTime + 7;
2302  m_fixedData->m_infoList[index].m_utcHour =
2303  m_fixedData->m_infoList[index].m_utcTime + 8;
2304  m_fixedData->m_infoList[index].m_utcMinute =
2305  m_fixedData->m_infoList[index].m_utcTime + 9;
2306  m_fixedData->m_infoList[index].m_utcSecond =
2307  m_fixedData->m_infoList[index].m_utcTime + 10;
2308  m_fixedData->m_infoList[index].m_utcValid =
2309  m_fixedData->m_infoList[index].m_utcTime + 11;
2310 
2311  m_fixedData->m_infoList[index].m_size += 12;
2312  }
2313  // update
2314  m_msg.setDataLong(data.m_nano, m_fixedData->m_infoList[index].m_utcNano);
2315  m_msg.setDataShort(data.m_year, m_fixedData->m_infoList[index].m_utcYear);
2316 
2317  // month, day, hour, minute, second and valid
2318  int8_t* bareByte = (int8_t*)&data.m_month;
2319  for (uint16_t i = 0; i < 6; ++i)
2320  m_msg.setDataByte(
2321  bareByte[i],
2322  m_fixedData->m_infoList[index].m_utcMonth + i); // lint !e661 !e662
2323 
2324  return true;
2325 }
2326 
2327 //////////////////////////////////////////////////////////////////////////////////////////
2328 // Return the XKF-3 Acc G component of the packet.
2329 XsVector LegacyDataPacket::freeAcceleration(int32_t index) const
2330 {
2331  XsVector3 buffer;
2332  if (containsFreeAcceleration(index))
2333  m_msg.getDataFPValue(
2334  CHECKIFDOUBLE(m_acc_g), &buffer[0],
2335  m_fixedData->m_infoList[index].m_acc_g, 3);
2336  else
2337  buffer.zero();
2338  return buffer;
2339 }
2340 bool LegacyDataPacket::containsFreeAcceleration(int32_t index) const
2341 {
2342  if (dataSize(index) == 0) return false;
2343  if (m_fixedData->m_infoList[index].m_acc_g == XS_DATA_ITEM_NOT_AVAILABLE)
2344  return false;
2345  return true;
2346 }
2347 bool LegacyDataPacket::setFreeAcceleration(const XsVector& g, int32_t index)
2348 {
2349  const uint16_t numValues = 3;
2350  if (dataSize(index) == 0) return false;
2351 
2352  uint16_t ds = getFPValueSize(index);
2353 
2354  if (m_fixedData->m_infoList[index].m_acc_g == XS_DATA_ITEM_NOT_AVAILABLE ||
2355  !ISDOUBLE(m_acc_g))
2356  {
2357  // add
2358  ds = 8; // added values are always in double precision
2359  // m_msg.m_autoUpdateChecksum = false;
2360 
2361  m_fixedData->m_infoList[index].m_acc_g = (uint16_t)m_msg.getDataSize();
2362  m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
2363  m_fixedData->m_infoList[index].m_size += numValues * ds;
2364  }
2365  // update
2366  m_msg.setDataFPValue(
2367  CHECKIFDOUBLE(m_acc_g), g.data(),
2368  m_fixedData->m_infoList[index].m_acc_g, numValues);
2369  return true;
2370 }
2371 
2372 XsTimeStamp LegacyDataPacket::triggerIndication(int channelID) const
2373 {
2374  switch (channelID)
2375  {
2376  case 1:
2377  return m_triggerIn1;
2378  case 2:
2379  return m_triggerIn2;
2380 
2381  default:
2382  return XsTimeStamp();
2383  }
2384 }
2385 
2386 bool LegacyDataPacket::containsTriggerIndication(int channelID /*= 0*/) const
2387 {
2388  switch (channelID)
2389  {
2390  case 0:
2391  return (m_triggerIn1.msTime() == 0 && m_triggerIn2.msTime() == 0)
2392  ? false
2393  : true;
2394  case 1:
2395  return (m_triggerIn1.msTime() == 0) ? false : true;
2396  case 2:
2397  return (m_triggerIn2.msTime() == 0) ? false : true;
2398 
2399  default:
2400  return false;
2401  }
2402 }
2403 
2404 bool LegacyDataPacket::setTriggerIndication(int channelID, const XsTimeStamp& t)
2405 {
2406  switch (channelID)
2407  {
2408  case 1:
2409  m_triggerIn1 = t;
2410  return true;
2411  case 2:
2412  m_triggerIn2 = t;
2413  return true;
2414  }
2415 
2416  return false;
2417 }
2418 // lint +esym(613, LegacyDataPacket::m_fixedData)
XsOutputMode
XsOutputMode
Bit values for legacy output mode.
Definition: xsoutputmode.h:16
PacketInfo::m_rawData
uint16_t m_rawData
Offset of raw data.
Definition: packetfixeddata.h:35
XOM_Velocity
Definition: xsoutputmode.h:24
LegacyDataPacket::containsMtwSdiData
bool containsMtwSdiData(int32_t index=0) const
Check if data item contains strapdown integration data.
Definition: legacydatapacket.cpp:1434
XS_EXTRA_TEMPERATURE_CHANNELS
#define XS_EXTRA_TEMPERATURE_CHANNELS
Definition: xsscrdata.h:26
LegacyDataPacket::setMtwSdiData
bool setMtwSdiData(const MtwSdiData &data, int32_t index=0)
Add/update strapdown integration data for the item.
Definition: legacydatapacket.cpp:1448
PacketInfo::m_offset
uint16_t m_offset
Offset of first data byte (whatever it is)
Definition: packetfixeddata.h:33
LegacyDataPacket::itemCount
uint16_t itemCount(void) const
Returns the number of devices whose data is contained in the object.
Definition: legacydatapacket.cpp:153
LegacyDataPacket::dataSize
XsSize dataSize(int32_t index=0) const
Returns the size of the data.
Definition: legacydatapacket.cpp:475
LegacyDataPacket::mtwSdiData
MtwSdiData mtwSdiData(int32_t index=0) const
Return the strapdown integration (SDI) data component of a data item.
Definition: legacydatapacket.cpp:1374
count
GLuint GLuint GLsizei count
Definition: glext.h:3528
LegacyDataPacket::containsOrientation
bool containsOrientation(int32_t index=0) const
Check if data item contains Orientation Data of any kind.
Definition: legacydatapacket.cpp:1984
PacketFixedData::m_formatList
XsDataFormat * m_formatList
A list of the formats of the data items.
Definition: packetfixeddata.h:176
LegacyDataPacket::freeAcceleration
XsVector freeAcceleration(int32_t index=0) const
Return the Acc-G component of the packet.
Definition: legacydatapacket.cpp:2329
LegacyDataPacket::rawData
XsScrData rawData(int32_t index=0) const
Return the Raw Data component of a data item.
Definition: legacydatapacket.cpp:1115
LegacyDataPacket::rtc
XsTimeStamp rtc(void) const
Returns the Real Time Clock value as stored in the object.
Definition: legacydatapacket.cpp:180
LegacyDataPacket::rawTemperature
uint16_t rawTemperature(int32_t index=0, int channel=0) const
Return the Raw Temperature component of a data item.
Definition: legacydatapacket.cpp:1064
PacketInfo::m_gpsPvtLatitude
uint16_t m_gpsPvtLatitude
Offset of raw GPS latitude data.
Definition: packetfixeddata.h:107
LegacyDataPacket::m_fixedData
PacketFixedData * m_fixedData
Fixed packet data.
Definition: legacydatapacket.h:334
LegacyDataPacket::containsOrientationMatrix
bool containsOrientationMatrix(int32_t index=0) const
Check if data item contains Matrix Orientation data.
Definition: legacydatapacket.cpp:1948
t
GLdouble GLdouble t
Definition: glext.h:3689
XOM_Status
Definition: xsoutputmode.h:26
PacketInfo::m_gpsPvtGpsData
uint16_t m_gpsPvtGpsData
Offset of raw GPS data.
Definition: packetfixeddata.h:103
uint16_t
unsigned __int16 uint16_t
Definition: rptypes.h:44
XS_MAX_TEMPERATURE_CHANNELS
#define XS_MAX_TEMPERATURE_CHANNELS
Definition: xsscrdata.h:27
XsPressure
struct XsPressure XsPressure
Definition: xspressure.h:24
PacketInfo::m_wClientId
uint16_t m_wClientId
Offset of MTw SDI client ID data.
Definition: packetfixeddata.h:129
XOS_PositionMode_Lla_Wgs84
Definition: xsoutputsettings.h:52
XOS_OrientationMode_Mask
Definition: xsoutputsettings.h:22
PacketInfo::m_gpsPvtVeld
uint16_t m_gpsPvtVeld
Offset of raw GPS dowanward velocity data.
Definition: packetfixeddata.h:117
XOM_Sdi
Definition: xsoutputmode.h:25
XOM_Temperature
Definition: xsoutputmode.h:19
PacketInfo::m_utcMonth
uint16_t m_utcMonth
Offset of month part of UTC time.
Definition: packetfixeddata.h:83
LegacyDataPacket::setMessage
void setMessage(const XsMessage &message)
Set the source message to msg.
Definition: legacydatapacket.cpp:232
LegacyDataPacket::setRawMagneticField
bool setRawMagneticField(const XsUShortVector &vec, int32_t index=0)
Add/update Raw Magnetometer data for the item.
Definition: legacydatapacket.cpp:1044
PacketInfo::m_gpsPvtLongitude
uint16_t m_gpsPvtLongitude
Offset of raw GPS longitude data.
Definition: packetfixeddata.h:109
LegacyDataPacket::utcTime
XsUtcTime utcTime(int32_t index=0) const
Return the UTC Time component of the packet.
Definition: legacydatapacket.cpp:2257
PacketInfo::m_wAidingData
uint16_t m_wAidingData
Offset of MTw SDI aiding data.
Definition: packetfixeddata.h:143
LegacyDataPacket::setTriggerIndication
bool setTriggerIndication(int channelID, const XsTimeStamp &t)
Add/update trigger indication time for the item to the given line.
Definition: legacydatapacket.cpp:2404
PacketInfo::m_wTimeSync
uint16_t m_wTimeSync
Offset of MTw SDI time sync data.
Definition: packetfixeddata.h:131
LegacyDataPacket::temperatureChannelCount
int temperatureChannelCount(int32_t index=0) const
Returns the number of temperature channels.
Definition: legacydatapacket.cpp:1569
LegacyDataPacket::m_msg
XsMessage m_msg
The message.
Definition: legacydatapacket.h:340
PacketInfo::m_rawAcc
uint16_t m_rawAcc
Offset of raw acceleration data.
Definition: packetfixeddata.h:37
LegacyDataPacket::containsOrientationEuler
bool containsOrientationEuler(int32_t index=0) const
Check if data item contains Euler Orientation data.
Definition: legacydatapacket.cpp:1894
LegacyDataPacket::containsRawData
bool containsRawData(int32_t index=0) const
Check if data item contains Raw Data.
Definition: legacydatapacket.cpp:1133
buffer
GLuint buffer
Definition: glext.h:3917
PacketInfo::m_wFirstFrameNumber
uint16_t m_wFirstFrameNumber
Offset of MTw SDI first frame number in interval data.
Definition: packetfixeddata.h:133
LegacyDataPacket::setRawTemperature
bool setRawTemperature(uint16_t temp, int32_t index=0, int channel=0)
Add/update Raw Temperature data for the item.
Definition: legacydatapacket.cpp:1080
XsUShortVector
A vector containing 3 short values.
Definition: xsushortvector.h:24
PacketInfo::m_velNEDorNWU
uint16_t m_velNEDorNWU
Offset of velocity data.
Definition: packetfixeddata.h:69
PacketInfo::m_gpsPvtPressure
uint16_t m_gpsPvtPressure
Offset of pressure data.
Definition: packetfixeddata.h:99
PacketInfo::m_gpsPvtHeight
uint16_t m_gpsPvtHeight
Offset of raw GPS height data.
Definition: packetfixeddata.h:111
PacketInfo::m_utcTime
uint16_t m_utcTime
Offset of UTC time data.
Definition: packetfixeddata.h:77
MtwSdiData
Class to store strapdown integration data.
Definition: mtwsdidata.h:19
PacketInfo::m_gpsPvtVacc
uint16_t m_gpsPvtVacc
Offset of raw GPS vertical accuracy estimate data.
Definition: packetfixeddata.h:121
XOM_Position
Definition: xsoutputmode.h:23
LegacyDataPacket::setDataFormat
bool setDataFormat(const XsDataFormat &format, int32_t index=0)
Sets the data format of the device with the given index to format.
Definition: legacydatapacket.cpp:340
PacketInfo::m_rawTemp
uint16_t m_rawTemp[XS_MAX_TEMPERATURE_CHANNELS]
Offset of raw temperature data.
Definition: packetfixeddata.h:44
PacketInfo::m_calMag
uint16_t m_calMag
Offset of calibrated magnetometer data.
Definition: packetfixeddata.h:55
LegacyDataPacket::setGpsPvtData
bool setGpsPvtData(const XsGpsPvtData &data, int32_t index=0)
Add/update Gps PVT Data for the item.
Definition: legacydatapacket.cpp:1234
PacketInfo::m_calAcc
uint16_t m_calAcc
Offset of calibrated acceleration data.
Definition: packetfixeddata.h:51
LegacyDataPacket::findDeviceId
int32_t findDeviceId(XsDeviceId dev) const
Returns the index of the fixed data with id dev.
Definition: legacydatapacket.cpp:276
XOM_Raw
Definition: xsoutputmode.h:29
XOS_CalibratedMode_Acc_Mask
Definition: xsoutputsettings.h:29
XOM_Auxiliary
Definition: xsoutputmode.h:22
LegacyDataPacket::setOrientationQuaternion
bool setOrientationQuaternion(const XsQuaternion &data, int32_t index=0)
Add/update Quaternion Orientation data for the item.
Definition: legacydatapacket.cpp:1843
XOS_Dataformat_Mask
Definition: xsoutputsettings.h:40
LegacyDataPacket::rawTemperatureChannelCount
int rawTemperatureChannelCount(int32_t index=0) const
Returns the number of available Raw Temperature channels.
Definition: legacydatapacket.cpp:1102
PacketInfo::m_utcValid
uint16_t m_utcValid
Offset of validity part of UTC time.
Definition: packetfixeddata.h:93
XsSize
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:19
int8_t
signed char int8_t
Definition: rptypes.h:40
PacketInfo::m_oriQuat
uint16_t m_oriQuat
Offset of orientation in quaternion format.
Definition: packetfixeddata.h:57
LegacyDataPacket::rawGyroscopeData
XsUShortVector rawGyroscopeData(int32_t index=0) const
Return the Raw Gyroscope component of a data item.
Definition: legacydatapacket.cpp:990
PacketInfo::m_mtwSdiData
uint16_t m_mtwSdiData
Offset of MTw SDI data.
Definition: packetfixeddata.h:127
LegacyDataPacket::setAnalogIn2Data
bool setAnalogIn2Data(const XsAnalogInData &data, int32_t index=0)
Add/update AnalogIn 2 for the item.
Definition: legacydatapacket.cpp:2050
LegacyDataPacket::orientationEuler
XsEuler orientationEuler(int32_t index=0) const
Return the Orientation component of a data item as Euler angles.
Definition: legacydatapacket.cpp:1881
PacketInfo::m_status
uint16_t m_status
Offset of status data.
Definition: packetfixeddata.h:71
LegacyDataPacket::setTemperature
bool setTemperature(const double &temp, int32_t index=0, int channel=0)
Add/update Calibrated Accelerometer data for the item.
Definition: legacydatapacket.cpp:1538
XOS_Dataformat_Fp1632
Definition: xsoutputsettings.h:43
XOS_CalibratedMode_Gyr_Mask
Definition: xsoutputsettings.h:33
PacketInfo::m_utcSecond
uint16_t m_utcSecond
Offset of second part of UTC time.
Definition: packetfixeddata.h:91
XOS_CalibratedMode_AccGyrMag_Mask
Definition: xsoutputsettings.h:27
PacketInfo::m_gpsPvtPressureAge
uint16_t m_gpsPvtPressureAge
Offset of pressure age.
Definition: packetfixeddata.h:101
PacketInfo::m_oriEul
uint16_t m_oriEul
Offset of orientation in euler format.
Definition: packetfixeddata.h:59
XOS_PositionMode_Mask
Definition: xsoutputsettings.h:51
XOS_Dataformat_Float
Definition: xsoutputsettings.h:41
counter
int counter
Definition: visit_each_unittest.cpp:15
PacketInfo::m_rawGyr
uint16_t m_rawGyr
Offset of raw gyroscope data.
Definition: packetfixeddata.h:39
PacketInfo::m_wCurrentBias
uint16_t m_wCurrentBias
Offset of MTw SDI gyroscope bias data.
Definition: packetfixeddata.h:137
PacketFixedData::m_itemCount
uint16_t m_itemCount
The number of data items in the message.
Definition: packetfixeddata.h:182
LegacyDataPacket::containsFreeAcceleration
bool containsFreeAcceleration(int32_t index=0) const
Check if data item contains XKF-3 Acc-G data.
Definition: legacydatapacket.cpp:2340
LegacyDataPacket::temperature
double temperature(int32_t index=0, int channel=0) const
Return the Temperature component of a data item.
Definition: legacydatapacket.cpp:1520
PacketInfo::m_gpsPvtItow
uint16_t m_gpsPvtItow
Offset of raw GPS ITOW (Integer Time Of Week) data.
Definition: packetfixeddata.h:105
XsUtcTime
struct XsUtcTime XsUtcTime
Definition: xsutctime.h:36
PacketInfo::m_doubleBoundary
uint16_t m_doubleBoundary
Boundary where the original data format is ignored and values are stored in double precision...
Definition: packetfixeddata.h:154
LegacyDataPacket::rawAcceleration
XsUShortVector rawAcceleration(int32_t index=0) const
The Raw Accelerometer component of a data item.
Definition: legacydatapacket.cpp:944
PacketInfo::m_gpsPvtGpsAge
uint16_t m_gpsPvtGpsAge
Offset of raw GPS age.
Definition: packetfixeddata.h:125
LegacyDataPacket::positionLLA
XsVector positionLLA(int32_t index=0) const
Return the Position Lat Lon Alt component of a data item.
Definition: legacydatapacket.cpp:2070
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
XOS_Status_Detailed
Definition: xsoutputsettings.h:39
LegacyDataPacket::LegacyDataPacket
LegacyDataPacket()
Construct a new LegacyDataPacket without data. The LegacyDataPacket will be invalid.
Definition: legacydatapacket.cpp:51
LegacyDataPacket::dataFormat
XsDataFormat dataFormat(int32_t index=0) const
Returns the data format of the device with the given index.
Definition: legacydatapacket.cpp:323
XOS_Dataformat_F1220
Definition: xsoutputsettings.h:42
PacketInfo::m_gpsPvtData
uint16_t m_gpsPvtData
Offset of GPS & pressure data.
Definition: packetfixeddata.h:97
PacketInfo::m_calGyr
uint16_t m_calGyr
Offset of calibrated gyroscope data.
Definition: packetfixeddata.h:53
LegacyDataPacket::setRawGyroscopeData
bool setRawGyroscopeData(const XsUShortVector &vec, int32_t index=0)
Add/update Raw Gyroscope data for the item.
Definition: legacydatapacket.cpp:1007
LegacyDataPacket::containsTriggerIndication
bool containsTriggerIndication(int channelID=0) const
Check if data item contains indication time on the given line.
Definition: legacydatapacket.cpp:2386
LegacyDataPacket::analogIn2Data
XsAnalogInData analogIn2Data(int32_t index=0) const
Return the AnalogIn 2 component of a data item.
Definition: legacydatapacket.cpp:2033
LegacyDataPacket::containsCalibratedData
bool containsCalibratedData(int32_t index=0) const
Check if data item contains Calibrated Data.
Definition: legacydatapacket.cpp:1767
LegacyDataPacket::containsCalibratedAcceleration
bool containsCalibratedAcceleration(int32_t index=0) const
Check if data item contains Calibrated Accelerometer data.
Definition: legacydatapacket.cpp:1592
PacketInfo::m_utcHour
uint16_t m_utcHour
Offset of hour part of UTC time.
Definition: packetfixeddata.h:87
XOS_Timestamp_PacketCounter
Definition: xsoutputsettings.h:20
XOS_VelocityMode_Mask
Definition: xsoutputsettings.h:53
PacketInfo
Contains offset information about data in the packet.
Definition: packetfixeddata.h:30
int16_t
__int16 int16_t
Definition: rptypes.h:43
LegacyDataPacket::calibratedGyroscopeData
XsVector calibratedGyroscopeData(int32_t index=0) const
Return the Calibrated Gyroscope component of a data item.
Definition: legacydatapacket.cpp:1627
LegacyDataPacket::largePacketCounter
int64_t largePacketCounter(void) const
Return the 64-bit sample counter associated with this packet.
Definition: legacydatapacket.cpp:193
PacketInfo::m_analogIn1
uint16_t m_analogIn1
Offset of analog in channel 1 data.
Definition: packetfixeddata.h:63
XsMessage
Structure for storing a single message.
Definition: xsmessage.h:198
int64_t
__int64 int64_t
Definition: rptypes.h:49
LegacyDataPacket::gpsPvtData
XsGpsPvtData gpsPvtData(int32_t index=0) const
Return the Gps PVT data component of a data item.
Definition: legacydatapacket.cpp:1172
index
GLuint index
Definition: glext.h:4054
XOS_OrientationMode_Quaternion
Definition: xsoutputsettings.h:23
c
const GLubyte * c
Definition: glext.h:6313
LegacyDataPacket::setAnalogIn1Data
bool setAnalogIn1Data(const XsAnalogInData &data, int32_t index=0)
Add/update AnalogIn 1 for the item.
Definition: legacydatapacket.cpp:2013
PACKETLOG
#define PACKETLOG(...)
Definition: legacydatapacket.cpp:26
PacketInfo::m_wVelocityIncrement
uint16_t m_wVelocityIncrement
Offset of MTw SDI velocity increment data.
Definition: packetfixeddata.h:141
LegacyDataPacket::containsCalibratedMagneticField
bool containsCalibratedMagneticField(int32_t index=0) const
Check if data item contains Calibrated Magnetometer data.
Definition: legacydatapacket.cpp:1684
PacketInfo::m_oriMat
uint16_t m_oriMat
Offset of orientation in matrix format.
Definition: packetfixeddata.h:61
PacketInfo::m_gpsPvtSacc
uint16_t m_gpsPvtSacc
Offset of raw GPS speed accuracy estimate data.
Definition: packetfixeddata.h:123
LegacyDataPacket::setFreeAcceleration
bool setFreeAcceleration(const XsVector &g, int32_t index=0)
Add/update XKF-3 Acc-G data for the item.
Definition: legacydatapacket.cpp:2347
LegacyDataPacket::setPacketCounter
bool setPacketCounter(const uint16_t counter, int32_t index=0)
Add/update Sample Counter for all items.
Definition: legacydatapacket.cpp:2240
PacketFixedData::m_infoList
PacketInfo * m_infoList
Contains information about data in the packet and the format of that data.
Definition: packetfixeddata.h:174
PacketInfo::m_wRssi
uint16_t m_wRssi
Offset of MTw SDI RSSI data.
Definition: packetfixeddata.h:149
LegacyDataPacket::setDeviceId
void setDeviceId(XsDeviceId deviceId, int32_t index)
Sets the device ID of the device with the given index to deviceid.
Definition: legacydatapacket.cpp:311
LegacyDataPacket::setUtcTime
bool setUtcTime(const XsUtcTime &data, int32_t index=0)
Add/update UTC Time for all items.
Definition: legacydatapacket.cpp:2285
PacketInfo::m_rawMag
uint16_t m_rawMag
Offset of raw magnetometer data.
Definition: packetfixeddata.h:41
g
GLubyte g
Definition: glext.h:6279
LegacyDataPacket::containsUtcTime
bool containsUtcTime(int32_t index=0) const
Check if data item contains UTC Time.
Definition: legacydatapacket.cpp:2278
LegacyDataPacket::deviceId
XsDeviceId deviceId(int32_t index) const
Returns the device ID of the device with the given index.
Definition: legacydatapacket.cpp:298
LegacyDataPacket::setVelocity
bool setVelocity(const XsVector &data, int32_t index=0)
Add/update Velocity for the item.
Definition: legacydatapacket.cpp:2134
XsTimeStamp
struct XsTimeStamp XsTimeStamp
Definition: xstimestamp.h:253
PacketInfo::m_acc_g
uint16_t m_acc_g
Offset of acceleration in global frame.
Definition: packetfixeddata.h:95
LegacyDataPacket::setPositionLLA
bool setPositionLLA(const XsVector &data, int32_t index=0)
Add/update Position Lat Lon Alt for the item.
Definition: legacydatapacket.cpp:2088
XsTimeStamp
Class for managing timestamps in a unified way.
Definition: xstimestamp.h:53
XOM_Calibrated
Definition: xsoutputmode.h:20
XOS_AuxiliaryMode_Ain2_Mask
Definition: xsoutputsettings.h:48
XOS_AuxiliaryMode_Ain1_Mask
Definition: xsoutputsettings.h:47
LegacyDataPacket::m_lastFoundIndex
uint16_t m_lastFoundIndex
Index of last found deviceId, speeds up searches.
Definition: legacydatapacket.h:338
XOM_GpsPvt_Pressure
Definition: xsoutputmode.h:27
LegacyDataPacket::triggerIndication
XsTimeStamp triggerIndication(int channelID) const
Return the synhcronization time recorded by the station.
Definition: legacydatapacket.cpp:2372
LegacyDataPacket::setItemCount
void setItemCount(uint16_t count)
Set the number of devices whose data is contained in this object to count.
Definition: legacydatapacket.cpp:162
PacketInfo::m_temp
uint16_t m_temp[XS_MAX_TEMPERATURE_CHANNELS]
Offset of calibrated temperature data.
Definition: packetfixeddata.h:47
LegacyDataPacket::containsCalibratedGyroscopeData
bool containsCalibratedGyroscopeData(int32_t index=0) const
Check if data item contains Calibrated Gyroscope data.
Definition: legacydatapacket.cpp:1638
LegacyDataPacket::setPressure
bool setPressure(const XsPressure &data, int32_t index=0)
Add/update pressure data for the item.
Definition: legacydatapacket.cpp:1343
XS_DATA_ITEM_NOT_AVAILABLE
#define XS_DATA_ITEM_NOT_AVAILABLE
Indicates that a data item is not available in the packet.
Definition: legacydatapacket.h:20
LegacyDataPacket::orientationQuaternion
XsQuaternion orientationQuaternion(int32_t index=0) const
Return the Orientation component of a data item as a Quaternion.
Definition: legacydatapacket.cpp:1824
LegacyDataPacket::originalMessage
XsMessage originalMessage(void) const
Returns the original message as it was received, without computed and added data (except for SDI inte...
Definition: legacydatapacket.cpp:211
XOS_VelocityMode_Ms_Xyz
Definition: xsoutputsettings.h:54
LegacyDataPacket::operator=
const LegacyDataPacket & operator=(const LegacyDataPacket &pack)
Assignment operator, copies the contents of pack into this.
Definition: legacydatapacket.cpp:107
LegacyDataPacket::containsRawTemperature
bool containsRawTemperature(int32_t index=0, int channel=0) const
Check if data item contains Raw Temperature data.
Definition: legacydatapacket.cpp:1071
PacketInfo::m_utcNano
uint16_t m_utcNano
Offset of nanosecond part of UTC time.
Definition: packetfixeddata.h:79
PacketInfo::m_wLastFrameNumber
uint16_t m_wLastFrameNumber
Offset of MTw SDI last frame number in interval data.
Definition: packetfixeddata.h:135
XsAnalogInData
Data from analog inputs from sensors.
Definition: xsanalogindata.h:15
PacketInfo::m_utcYear
uint16_t m_utcYear
Offset of year part of UTC time.
Definition: packetfixeddata.h:81
CHECKIFDOUBLE
#define CHECKIFDOUBLE(a)
Definition: legacydatapacket.cpp:40
LegacyDataPacket::setStatus
bool setStatus(const uint32_t data, int32_t index=0)
Add/update Status information for the item.
Definition: legacydatapacket.cpp:2202
int32_t
__int32 int32_t
Definition: rptypes.h:46
LegacyDataPacket::containsPacketCounter
bool containsPacketCounter(int32_t index=0) const
Check if data item contains Sample Counter.
Definition: legacydatapacket.cpp:2233
LegacyDataPacket
Contains an MTData XsMessage and supports functions for extracting contained data.
Definition: legacydatapacket.h:41
XsDataFormat
A structure for storing data formats.
Definition: xsdataformat.h:17
LegacyDataPacket::setXbusSystem
void setXbusSystem(bool xbus, bool convert=false)
Sets the xbus flag.
Definition: legacydatapacket.cpp:400
XsOutputSettings
XsOutputSettings
Bit values for output settings.
Definition: xsoutputsettings.h:16
LegacyDataPacket::containsRawMagneticField
bool containsRawMagneticField(int32_t index=0) const
Check if data item contains Raw Magnetometer data.
Definition: legacydatapacket.cpp:1037
LegacyDataPacket::message
XsMessage message(void) const
Returns a copy of the XsMessage contained by the object, including computed and added data...
Definition: legacydatapacket.cpp:207
LegacyDataPacket::analogIn1Data
XsAnalogInData analogIn1Data(int32_t index=0) const
Return the AnalogIn 1 component of a data item.
Definition: legacydatapacket.cpp:1996
XsDataFormat::m_outputMode
XsOutputMode m_outputMode
The stored output mode.
Definition: xsdataformat.h:20
PacketInfo::m_gpsPvtVele
uint16_t m_gpsPvtVele
Offset of raw GPS eastward velocity data.
Definition: packetfixeddata.h:115
PacketInfo::m_wBaroMeter
uint16_t m_wBaroMeter
Offset of MTw SDI barometer data.
Definition: packetfixeddata.h:145
XsDataFormat::m_outputSettings
XsOutputSettings m_outputSettings
The stored output settings.
Definition: xsdataformat.h:22
PacketInfo::m_detailedStatus
uint16_t m_detailedStatus
Offset of detailed status data.
Definition: packetfixeddata.h:73
LegacyDataPacket::setRawData
bool setRawData(const XsScrData &data, int32_t index=0)
Add/update Raw Data for the item.
Definition: legacydatapacket.cpp:1139
XsUtcTime
A structure for storing UTC Time values.
Definition: xsutctime.h:15
LegacyDataPacket::packetInfo
PacketInfo packetInfo(int32_t index) const
Returns the packet info for the index&#39;th device in the packet.
Definition: legacydatapacket.cpp:238
PacketInfo::m_size
uint16_t m_size
Total size of the data.
Definition: packetfixeddata.h:151
PacketFixedData
A structure containing fixed packet data, which should not change during a measurement for the same d...
Definition: packetfixeddata.h:163
LegacyDataPacket::containsPositionLLA
bool containsPositionLLA(int32_t index=0) const
Check if data item contains Position Lat Lon Alt.
Definition: legacydatapacket.cpp:2081
PacketInfo::m_gpsPvtVeln
uint16_t m_gpsPvtVeln
Offset of raw GPS northward velocity data.
Definition: packetfixeddata.h:113
LegacyDataPacket::containsPressure
bool containsPressure(int32_t index=0) const
Return true if the packet contains pressure data.
Definition: legacydatapacket.cpp:1330
LegacyDataPacket::m_triggerIn1
XsTimeStamp m_triggerIn1
Trigger indication on line 1.
Definition: legacydatapacket.h:348
XOM_Orientation
Definition: xsoutputmode.h:21
LegacyDataPacket::packetCounter
uint16_t packetCounter(int32_t index=0) const
Return the Sample Counter component of the packet.
Definition: legacydatapacket.cpp:2228
XsMath_doubleToLong
XSTYPES_DLL_API int32_t XsMath_doubleToLong(double d)
LegacyDataPacket::status
uint32_t status(int32_t index, bool *outIsDetailed) const
Return the Status component of a data item.
Definition: legacydatapacket.cpp:2163
LegacyDataPacket::updateInfoList
void updateInfoList()
Update the internal info list by analyzing the known XsDataFormat for each device.
Definition: legacydatapacket.cpp:485
LegacyDataPacket::setLargePacketCounter
void setLargePacketCounter(int64_t sc)
Set the 64-bit sample counter associated with this packet.
Definition: legacydatapacket.cpp:200
LegacyDataPacket::containsOrientationQuaternion
bool containsOrientationQuaternion(int32_t index=0) const
Check if data item contains Quaternion Orientation data.
Definition: legacydatapacket.cpp:1836
LegacyDataPacket::containsAnalogIn1Data
bool containsAnalogIn1Data(int32_t index=0) const
Check if data item contains AnalogIn 1.
Definition: legacydatapacket.cpp:2005
LegacyDataPacket::frameCounter
uint16_t frameCounter() const
Return the frame counter (previously: sample counter) of the packet.
Definition: legacydatapacket.cpp:254
PacketInfo::m_gpsPvtHacc
uint16_t m_gpsPvtHacc
Offset of raw GPS horizontal accuracy estimate data.
Definition: packetfixeddata.h:119
LegacyDataPacket::m_packetId
XsTimeStamp m_packetId
64 bit sample counter
Definition: legacydatapacket.h:346
LegacyDataPacket::rawMagneticField
XsUShortVector rawMagneticField(int32_t index=0) const
Return the Raw Magnetometer component of a data item.
Definition: legacydatapacket.cpp:1027
LegacyDataPacket::setTimeOfArrival
void setTimeOfArrival(XsTimeStamp timeOfArrival)
Set the Time Of Arrival value to timeofarrival.
Definition: legacydatapacket.cpp:173
LegacyDataPacket::setRawAcceleration
bool setRawAcceleration(const XsUShortVector &vec, int32_t index=0)
Add/update Raw Accelerometer data for the item.
Definition: legacydatapacket.cpp:970
PacketInfo::m_analogIn2
uint16_t m_analogIn2
Offset of analog in channel 2 data.
Definition: packetfixeddata.h:65
LegacyDataPacket::calibratedAcceleration
XsVector calibratedAcceleration(int32_t index=0) const
Return the Calibrated Accelerometer component of a data item.
Definition: legacydatapacket.cpp:1581
XsMtTimeStamp
uint16_t XsMtTimeStamp
An MT timestamp (sample count)
Definition: legacydatapacket.h:34
XOS_Timestamp_SampleUtc
Definition: xsoutputsettings.h:21
XOS_Dataformat_Double
Definition: xsoutputsettings.h:44
format
GLenum GLsizei GLenum format
Definition: glext.h:3531
PacketInfo::m_utcMinute
uint16_t m_utcMinute
Offset of minute part of UTC time.
Definition: packetfixeddata.h:89
LegacyDataPacket::m_lastFoundId
XsDeviceId m_lastFoundId
Last found deviceId, speeds up searches.
Definition: legacydatapacket.h:336
LegacyDataPacket::setOrientationMatrix
bool setOrientationMatrix(const XsMatrix &data, int32_t index=0)
Add/update Matrix Orientation data for the item.
Definition: legacydatapacket.cpp:1955
XsScrData
Container for raw sensor measurement data.
Definition: xsscrdata.h:35
PacketInfo::m_calData
uint16_t m_calData
Offset of calibrated data.
Definition: packetfixeddata.h:49
ISDOUBLE
#define ISDOUBLE(a)
Definition: legacydatapacket.cpp:37
LegacyDataPacket::calibratedData
XsCalibratedData calibratedData(int32_t index=0) const
Return the Calibrated Data component of a data item.
Definition: legacydatapacket.cpp:1721
LegacyDataPacket::getFPValueSize
uint16_t getFPValueSize(int32_t index) const
Returns the floating/fixed point value size in bytes.
Definition: legacydatapacket.cpp:443
PacketInfo::m_wOrientationIncrement
uint16_t m_wOrientationIncrement
Offset of MTw SDI orientation increment data.
Definition: packetfixeddata.h:139
LegacyDataPacket::containsAnalogIn2Data
bool containsAnalogIn2Data(int32_t index=0) const
Check if data item contains AnalogIn 2.
Definition: legacydatapacket.cpp:2042
LegacyDataPacket::pressure
XsPressure pressure(int32_t index=0) const
Return the pressure data component of a data item.
Definition: legacydatapacket.cpp:1299
LegacyDataPacket::setRtc
void setRtc(XsTimeStamp rtc)
Set the Real Time Clock value to realtimeclock.
Definition: legacydatapacket.cpp:183
LegacyDataPacket::setCalibratedMagneticField
bool setCalibratedMagneticField(const XsVector &vec, int32_t index=0)
Add/update Calibrated Magnetometer data for the item.
Definition: legacydatapacket.cpp:1691
LegacyDataPacket::containsVelocity
bool containsVelocity(int32_t index=0) const
Check if data item contains Velocity.
Definition: legacydatapacket.cpp:2126
LegacyDataPacket::containsStatus
bool containsStatus(int32_t index=0) const
Check if data item contains Status.
Definition: legacydatapacket.cpp:2187
LegacyDataPacket::orientationMatrix
XsMatrix orientationMatrix(int32_t index=0) const
Return the Orientation component of a data item as an Orientation Matrix.
Definition: legacydatapacket.cpp:1928
XOS_OrientationMode_Euler
Definition: xsoutputsettings.h:24
PacketInfo::m_wMagnetoMeter
uint16_t m_wMagnetoMeter
Offset of MTw SDI magnetometer data.
Definition: packetfixeddata.h:147
LegacyDataPacket::containsDetailedStatus
bool containsDetailedStatus(int32_t index=0) const
Check if data item contains detailed Status information.
Definition: legacydatapacket.cpp:2194
PacketInfo::m_posLLA
uint16_t m_posLLA
Offset of Latitude-Longitude-Altitude position data.
Definition: packetfixeddata.h:67
LegacyDataPacket::setCalibratedData
bool setCalibratedData(const XsCalibratedData &data, int32_t index=0)
Add/update Calibrated Data for the item.
Definition: legacydatapacket.cpp:1775
PacketInfo::m_sc
uint16_t m_sc
Offset of sample counter.
Definition: packetfixeddata.h:75
LegacyDataPacket::m_rtc
XsTimeStamp m_rtc
Sample time in ms, based on the sample counter.
Definition: legacydatapacket.h:342
XOS_NoGpsInGpsPvt
Definition: xsoutputsettings.h:56
LegacyDataPacket::containsRawGyroscopeData
bool containsRawGyroscopeData(int32_t index=0) const
Check if data item contains Raw Gyroscope data.
Definition: legacydatapacket.cpp:1000
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
XOS_CalibratedMode_Mag_Mask
Definition: xsoutputsettings.h:36
LegacyDataPacket::~LegacyDataPacket
~LegacyDataPacket()
Destructor.
Definition: legacydatapacket.cpp:135
LegacyDataPacket::calibratedMagneticField
XsVector calibratedMagneticField(int32_t index=0) const
Return the Calibrated Magnetometer component of a data item.
Definition: legacydatapacket.cpp:1673
swapEndian16
#define swapEndian16(src)
Definition: cmtmessage.h:76
data
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
LegacyDataPacket::containsTemperature
bool containsTemperature(int32_t index=0, int channel=0) const
Check if data item contains Temperature data.
Definition: legacydatapacket.cpp:1529
LegacyDataPacket::m_triggerIn2
XsTimeStamp m_triggerIn2
Trigger indication on line 2.
Definition: legacydatapacket.h:350
XOS_ExtendedTemperature_Mask
Definition: xsoutputsettings.h:57
LegacyDataPacket::setCalibratedAcceleration
bool setCalibratedAcceleration(const XsVector &vec, int32_t index=0)
Add/update Calibrated Accelerometer data for the item.
Definition: legacydatapacket.cpp:1599
LegacyDataPacket::containsGpsPvtData
bool containsGpsPvtData(int32_t index=0) const
Check if data item contains Gps PVT Data.
Definition: legacydatapacket.cpp:1226
LegacyDataPacket::velocity
XsVector velocity(int32_t index=0) const
Return the Velocity component of a data item.
Definition: legacydatapacket.cpp:2115
LegacyDataPacket::isXbusSystem
bool isXbusSystem(void) const
Returns whether the xbus flag is set or not.
Definition: legacydatapacket.cpp:384
XOS_OrientationMode_Matrix
Definition: xsoutputsettings.h:25
LegacyDataPacket::setCalibratedGyroscopeData
bool setCalibratedGyroscopeData(const XsVector &vec, int32_t index=0)
Add/update Calibrated Gyroscope data for the item.
Definition: legacydatapacket.cpp:1645
LegacyDataPacket::setOrientationEuler
bool setOrientationEuler(const XsEuler &data, int32_t index=0)
Add/update Euler Orientation data for the item.
Definition: legacydatapacket.cpp:1901
LegacyDataPacket::m_toa
XsTimeStamp m_toa
Time of arrival.
Definition: legacydatapacket.h:344
PacketFixedData::m_xm
bool m_xm
Indicates that xbus-formatting is used.
Definition: packetfixeddata.h:180
LegacyDataPacket::timeOfArrival
XsTimeStamp timeOfArrival(void) const
Returns the Time Of Arrival value as stored in the object.
Definition: legacydatapacket.cpp:170
LegacyDataPacket::containsRawAcceleration
bool containsRawAcceleration(int32_t index=0) const
Check if data item contains Raw Accelerometer data.
Definition: legacydatapacket.cpp:961
XsGpsPvtData
Data from the GPS unit of a legacy MTi-G.
Definition: xsgpspvtdata.h:34
XsPressure
Pressure data.
Definition: xspressure.h:17
PacketInfo::m_utcDay
uint16_t m_utcDay
Offset of day part of UTC time.
Definition: packetfixeddata.h:85
PacketFixedData::m_idList
XsDeviceId * m_idList
A list of the device-ids in this packet.
Definition: packetfixeddata.h:178


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020