MRPT  1.9.9
int_xsdatapacket.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 "int_xsdatapacket.h"
11 #include "legacydatapacket.h"
12 #include "packetfixeddata.h"
13 #include "mtwsdidata.h"
14 #include <xsens/xsbusid.h>
15 #include <xsens/xsdatapacket.h>
16 #include <xsens/xsmessage.h>
17 #include <xsens/xsgpspvtdata.h>
18 
19 // lint -esym(1512, XsDataPacket)
20 /*! \internal
21  \brief Hack class to access internals of XsDataPacket in C++
22 */
24 {
25  // lint --e(1511, 1509, 1536)
26  public:
27  //! \brief \copybrief XsDataPacket::m_msg
28  inline XsMessage& msg() { return m_msg; }
29  //! \brief \copybrief XsDataPacket::m_legacyMsg
30  inline XsMessage& legacyMsg() { return m_legacyMsg; }
31  //! \brief \copybrief XsDataPacket::m_deviceId
32  inline XsDeviceId& deviceId() { return m_deviceId; }
33  //! \brief \copybrief XsDataPacket::m_lastFoundId
35  //! \brief \copybrief XsDataPacket::m_lastFoundOffset
36  inline int& lastFoundOffset() { return m_lastFoundOffset; }
37  //! \brief \copybrief XsDataPacket::m_itemCount
38  inline uint16_t& itemCount() { return m_itemCount; }
39  //! \brief \copybrief XsDataPacket::m_originalMessageLength
41  //! \brief \copybrief XsDataPacket::m_toa
42  inline XsTimeStamp& toa() { return m_toa; }
43  //! \brief \copybrief XsDataPacket::m_packetId
44  inline XsTimeStamp& packetId() { return m_packetId; }
45 };
46 
47 /*! \class XsDataPacket
48  \brief Contains data received from a device or read from a file
49 
50  This class is used by XDA for easy access to data contained in a message. It
51  analyzes its internal
52  XsMessage upon contruction to give access to the individual contained pieces
53  of data.
54  New data can also be added to the %XsDataPacket or updated if it already
55  existed.
56 */
57 extern "C" {
58 
59 void validatePacket(XsDataPacket* thisPtr);
60 
62  struct XsDataPacket* thisPtr, struct LegacyDataPacket const* pack,
63  int index)
64 {
65  assert(pack);
66 
67  InternalDataPacket* hacket = (InternalDataPacket*)thisPtr;
68 
69  hacket->legacyMsg() = pack->message();
70 
71  if (!pack->isXbusSystem()) index = 0;
72 
73  hacket->message().clear();
74  if (pack->isXbusSystem())
75  hacket->message().setBusId(index + 1);
76  else
77  hacket->message().setBusId(XS_BID_MASTER);
78 
79  hacket->message().setMessageId(XMID_MtData2);
80  // hacket->m_legacyMsg = pack->message();
81  hacket->deviceId() = pack->deviceId(index);
82  hacket->itemCount() = 1;
83  hacket->toa() = pack->timeOfArrival();
84  hacket->originalMessageLength() =
85  (uint16_t)pack->originalMessage().getDataSize();
86  hacket->packetId() = pack->largePacketCounter();
87  PacketInfo info = pack->packetInfo(index);
90  switch (format.m_outputSettings & XOS_Dataformat_Mask)
91  {
93  di = XDI_SubFormatFloat;
94  break;
97  break;
100  break;
102  di = XDI_SubFormatFp1220;
103  break;
104  default:
105  di = XDI_None;
106  break;
107  }
108  if (pack->containsRawData(index)) hacket->setRawData(pack->rawData(index));
109  if (pack->rawTemperatureChannelCount(index) == 4)
110  {
111  XsUShortVector rawGyroTemperatures;
112  rawGyroTemperatures[0] = pack->rawTemperature(index, 1);
113  rawGyroTemperatures[1] = pack->rawTemperature(index, 2);
114  rawGyroTemperatures[2] = pack->rawTemperature(index, 3);
115  hacket->setRawGyroscopeTemperatureData(rawGyroTemperatures);
116  }
118  hacket->setCalibratedAcceleration(pack->calibratedAcceleration(index));
120  {
121  // Special copy to preserve watermarking
122  hacket->message().setDataShort(
123  XDI_RateOfTurn | di, hacket->message().getDataSize());
124  hacket->message().setDataByte(
125  3 * hacket->getFPValueSize(di), hacket->message().getDataSize());
126  hacket->itemCount()++;
127  hacket->message().setDataBuffer(
128  hacket->legacyMsg().getDataBuffer(info.m_calGyr),
129  3 * hacket->getFPValueSize(di),
131  }
133  hacket->setCalibratedMagneticField(
136  hacket->setOrientationQuaternion(
138  (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
139  : XDI_CoordSysNwu);
140  if (pack->containsOrientationEuler(index))
141  hacket->setOrientationEuler(
142  pack->orientationEuler(index),
143  (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
144  : XDI_CoordSysNwu);
145  if (pack->containsOrientationMatrix(index))
146  hacket->setOrientationMatrix(
147  pack->orientationMatrix(index),
148  (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
149  : XDI_CoordSysNwu);
150  if (pack->containsPositionLLA(index))
151  hacket->setPositionLLA(pack->positionLLA(index));
152  if (pack->containsVelocity(index))
153  hacket->setVelocity(
154  pack->velocity(index),
155  (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
156  : XDI_CoordSysNwu);
157  if (pack->containsStatus(index))
158  {
159  bool isDetailed = true;
160  uint32_t status = pack->status(index, &isDetailed);
161 
162  // For MTw's only, the status needs to be swapEndian32-ed
163  if (hacket->deviceId().isMtw())
164  {
166  }
167 
168  if (isDetailed)
169  {
170  hacket->setStatus(status);
171  }
172  else
173  {
174  hacket->setStatusByte((uint8_t)status);
175  }
176  }
177  if (pack->containsGpsPvtData())
178  {
179  XsPressure tmp;
180  tmp.m_pressure =
181  pack->gpsPvtData(index).m_pressure * 2.0; // Convert to Pascal
183  hacket->setPressure(tmp);
184  XsGpsPvtData pvt;
185  pvt = pack->gpsPvtData(index);
186  hacket->setGpsPvtData(pvt);
187  }
188  if (pack->containsUtcTime(index))
189  {
190  XsUtcTime time = pack->utcTime(index);
191  hacket->setUtcTime(time);
192  }
193  if (pack->containsMtwSdiData(index))
194  {
195  // not yet converted:
196  // uint8_t m_timeSync;
197  // XsVector3 m_currentBias;
198 
199  MtwSdiData mtwsdi = pack->mtwSdiData(index);
200  hacket->deviceId() = mtwsdi.m_deviceId;
201  {
202  // Special copy to preserve watermarking
203  hacket->message().setDataShort(
204  XDI_DeltaQ | di, hacket->message().getDataSize());
205  hacket->message().setDataByte(
206  4 * hacket->getFPValueSize(di),
207  hacket->message().getDataSize());
208  hacket->itemCount()++;
209  hacket->message().setDataBuffer(
210  hacket->legacyMsg().getDataBuffer(info.m_wOrientationIncrement),
211  4 * hacket->getFPValueSize(di),
213 
214  hacket->message().setDataShort(
215  XDI_DeltaV | di, hacket->message().getDataSize());
216  hacket->message().setDataByte(
217  3 * hacket->getFPValueSize(di),
218  hacket->message().getDataSize());
219  hacket->itemCount()++;
220  hacket->message().setDataBuffer(
221  hacket->legacyMsg().getDataBuffer(info.m_wVelocityIncrement),
222  3 * hacket->getFPValueSize(di),
224  }
225 
226  // if (mtwsdi.m_aidingData)
227  hacket->setCalibratedMagneticField(mtwsdi.m_magnetoMeter);
228  if (mtwsdi.m_barometer)
229  {
230  XsPressure tmp;
231  tmp.m_pressure =
232  mtwsdi.m_barometer * 100.0; // convert from millibar to pascal
233  tmp.m_pressureAge = mtwsdi.m_barometer ? 0 : 255;
234  hacket->setPressure(tmp);
235  }
236  hacket->setFrameRange(
237  XsRange(
238  (int)mtwsdi.m_firstFrameNumber, (int)mtwsdi.m_lastFrameNumber));
239  hacket->setRssi(mtwsdi.m_rssi);
240  }
241  if (pack->containsTemperature(index))
242  hacket->setTemperature(pack->temperature(index));
243  if (pack->containsPacketCounter(index))
244  hacket->setPacketCounter(pack->packetCounter(index));
245 
246  // enable this when you experience weird crashes in XsByteArray_destruct or
247  // XsDataPacket_destruct
248  // validatePacket(thisPtr);
249 }
250 
251 } // extern "C"
bool containsMtwSdiData(int32_t index=0) const
Check if data item contains strapdown integration data.
XsMessage & msg()
The message that contains the data.
MtwSdiData mtwSdiData(int32_t index=0) const
Return the strapdown integration (SDI) data component of a data item.
XsScrData rawData(int32_t index=0) const
Return the Raw Data component of a data item.
uint16_t rawTemperature(int32_t index=0, int channel=0) const
Return the Raw Temperature component of a data item.
bool containsOrientationMatrix(int32_t index=0) const
Check if data item contains Matrix Orientation data.
unsigned __int16 uint16_t
Definition: rptypes.h:44
XsDataIdentifier & lastFoundId()
Last found data identifer, speeds up searches.
XsDeviceId & deviceId()
The device Id to which the message belongs.
XsUtcTime utcTime(int32_t index=0) const
Return the UTC Time component of the packet.
bool containsOrientationEuler(int32_t index=0) const
Check if data item contains Euler Orientation data.
bool containsRawData(int32_t index=0) const
Check if data item contains Raw Data.
XsTimeStamp & toa()
Time of arrival (live packets only)
A vector containing 3 short values.
uint16_t m_originalMessageLength
Length of the original message payload.
uint8_t m_pressureAge
The age of the pressure measurement in packets.
Definition: xsgpspvtdata.h:42
Class to store strapdown integration data.
Definition: mtwsdidata.h:19
#define swapEndian32(src)
Definition: cmtmessage.h:77
int rawTemperatureChannelCount(int32_t index=0) const
Returns the number of available Raw Temperature channels.
uint16_t & itemCount()
The number of data items in the message.
XsEuler orientationEuler(int32_t index=0) const
Return the Orientation component of a data item as Euler angles.
XsDataIdentifier
Defines the data identifiers.
XsTimeStamp m_toa
Time of arrival (live packets only)
double temperature(int32_t index=0, int channel=0) const
Return the Temperature component of a data item.
uint8_t m_pressureAge
Age of pressure data in samples.
Definition: xspressure.h:22
XsVector positionLLA(int32_t index=0) const
Return the Position Lat Lon Alt component of a data item.
unsigned char uint8_t
Definition: rptypes.h:41
XsDeviceId m_deviceId
The ID of the device that generated the data.
Definition: mtwsdidata.h:22
XsDataFormat dataFormat(int32_t index=0) const
Returns the data format of the device with the given index.
double m_pressure
Pressure in Pascal.
Definition: xspressure.h:20
void validatePacket(XsDataPacket *thisPtr)
uint16_t m_firstFrameNumber
The first frame number of the SDI interval.
Definition: mtwsdidata.h:27
uint16_t m_calGyr
Offset of calibrated gyroscope data.
bool containsCalibratedAcceleration(int32_t index=0) const
Check if data item contains Calibrated Accelerometer data.
Contains offset information about data in the packet.
int64_t largePacketCounter(void) const
Return the 64-bit sample counter associated with this packet.
Structure for storing a single message.
Definition: xsmessage.h:198
uint16_t m_itemCount
The number of data items in the message.
double m_barometer
The barometer value during the interval.
Definition: mtwsdidata.h:40
XsGpsPvtData gpsPvtData(int32_t index=0) const
Return the Gps PVT data component of a data item.
GLuint index
Definition: glext.h:4054
uint16_t m_wVelocityIncrement
Offset of MTw SDI velocity increment data.
bool containsCalibratedMagneticField(int32_t index=0) const
Check if data item contains Calibrated Magnetometer data.
int8_t m_rssi
The Received Signal Strength Indication (RSSI) of the message.
Definition: mtwsdidata.h:44
XsDataIdentifier m_lastFoundId
Last found data identifer, speeds up searches.
bool containsUtcTime(int32_t index=0) const
Check if data item contains UTC Time.
XsDeviceId deviceId(int32_t index) const
Returns the device ID of the device with the given index.
Class for managing timestamps in a unified way.
Definition: xstimestamp.h:53
bool containsCalibratedGyroscopeData(int32_t index=0) const
Check if data item contains Calibrated Gyroscope data.
XsVector3 m_magnetoMeter
The magnetometer values during the interval.
Definition: mtwsdidata.h:42
int m_lastFoundOffset
Offset of last found data identifier, speeds up searches.
int & lastFoundOffset()
Offset of last found data identifier, speeds up searches.
XsQuaternion orientationQuaternion(int32_t index=0) const
Return the Orientation component of a data item as a Quaternion.
XsMessage originalMessage(void) const
Returns the original message as it was received, without computed and added data (except for SDI inte...
XsDeviceId m_deviceId
The device Id to which the message belongs.
uint16_t m_lastFrameNumber
The last frame number of the SDI interval.
Definition: mtwsdidata.h:30
uint16_t m_pressure
The pressure measurement in units of 2 Pascal, only valid if m_pressureAge is not 255...
Definition: xsgpspvtdata.h:38
bool containsPacketCounter(int32_t index=0) const
Check if data item contains Sample Counter.
Contains an MTData XsMessage and supports functions for extracting contained data.
A structure for storing data formats.
Definition: xsdataformat.h:17
XsMessage message(void) const
Returns a copy of the XsMessage contained by the object, including computed and added data...
A structure for storing UTC Time values.
Definition: xsutctime.h:15
PacketInfo packetInfo(int32_t index) const
Returns the packet info for the index&#39;th device in the packet.
bool containsPositionLLA(int32_t index=0) const
Check if data item contains Position Lat Lon Alt.
uint16_t packetCounter(int32_t index=0) const
Return the Sample Counter component of the packet.
uint32_t status(int32_t index, bool *outIsDetailed) const
Return the Status component of a data item.
XsMessage & legacyMsg()
Optional legacy MtData message as received (for logging the received data only)
XsMessage m_msg
The message that contains the data.
bool containsOrientationQuaternion(int32_t index=0) const
Check if data item contains Quaternion Orientation data.
struct XsRange XsRange
Definition: xsrange.h:89
_u8 status
Definition: rplidar_cmd.h:19
uint16_t & originalMessageLength()
Length of the original message payload.
Contains data received from a device or read from a file.
Definition: xsdatapacket.h:302
XsMessage m_legacyMsg
Optional legacy MtData message as received (for logging the received data only)
XsVector calibratedAcceleration(int32_t index=0) const
Return the Calibrated Accelerometer component of a data item.
GLenum GLsizei GLenum format
Definition: glext.h:3531
uint16_t m_wOrientationIncrement
Offset of MTw SDI orientation increment data.
bool containsVelocity(int32_t index=0) const
Check if data item contains Velocity.
bool containsStatus(int32_t index=0) const
Check if data item contains Status.
XsMatrix orientationMatrix(int32_t index=0) const
Return the Orientation component of a data item as an Orientation Matrix.
unsigned __int32 uint32_t
Definition: rptypes.h:47
XsVector calibratedMagneticField(int32_t index=0) const
Return the Calibrated Magnetometer component of a data item.
bool containsTemperature(int32_t index=0, int channel=0) const
Check if data item contains Temperature data.
XsTimeStamp m_packetId
64 bit packet id, based on, depending on availability: (1) packet counter (2) sample time (3) arrival...
bool containsGpsPvtData(int32_t index=0) const
Check if data item contains Gps PVT Data.
XsVector velocity(int32_t index=0) const
Return the Velocity component of a data item.
bool isXbusSystem(void) const
Returns whether the xbus flag is set or not.
void XsDataPacket_assignFromXsLegacyDataPacket(struct XsDataPacket *thisPtr, struct LegacyDataPacket const *pack, int index)
XsTimeStamp timeOfArrival(void) const
Returns the Time Of Arrival value as stored in the object.
Data from the GPS unit of a legacy MTi-G.
Definition: xsgpspvtdata.h:34
Pressure data.
Definition: xspressure.h:17
#define XS_BID_MASTER
The bus identifier of the master device.
Definition: xsbusid.h:17
XsTimeStamp & packetId()
64 bit packet id, based on, depending on availability: (1) packet counter (2) sample time (3) arrival...
XSTYPES_DLL_API int XsDataPacket_itemOffsetExact(const XsDataPacket *thisPtr, XsDataIdentifier id)



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