Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #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 //lint --e(1511, 1509, 1536)
25 public:
26  //! \brief \copybrief XsDataPacket::m_msg
27  inline XsMessage& msg()
28  {
29  return m_msg;
30  }
31 
32  //! \brief \copybrief XsDataPacket::m_legacyMsg
33  inline XsMessage& legacyMsg()
34  {
35  return m_legacyMsg;
36  }
37 
38  //! \brief \copybrief XsDataPacket::m_deviceId
39  inline XsDeviceId& deviceId()
40  {
41  return m_deviceId;
42  }
43 
44  //! \brief \copybrief XsDataPacket::m_lastFoundId
46  {
47  return m_lastFoundId;
48  }
49 
50  //! \brief \copybrief XsDataPacket::m_lastFoundOffset
51  inline int& lastFoundOffset()
52  {
53  return m_lastFoundOffset;
54  }
55 
56  //! \brief \copybrief XsDataPacket::m_itemCount
57  inline uint16_t& itemCount()
58  {
59  return m_itemCount;
60  }
61 
62  //! \brief \copybrief XsDataPacket::m_originalMessageLength
64  {
66  }
67 
68  //! \brief \copybrief XsDataPacket::m_toa
69  inline XsTimeStamp& toa()
70  {
71  return m_toa;
72  }
73 
74  //! \brief \copybrief XsDataPacket::m_packetId
76  {
77  return m_packetId;
78  }
79 };
80 
81 /*! \class XsDataPacket
82  \brief Contains data received from a device or read from a file
83 
84  This class is used by XDA for easy access to data contained in a message. It analyzes its internal
85  XsMessage upon contruction to give access to the individual contained pieces of data.
86  New data can also be added to the %XsDataPacket or updated if it already existed.
87 */
88 extern "C" {
89 
90 void validatePacket(XsDataPacket* thisPtr);
91 
92 void XsDataPacket_assignFromXsLegacyDataPacket(struct XsDataPacket* thisPtr, struct LegacyDataPacket const* pack, int index)
93 {
94  assert(pack);
95 
96  InternalDataPacket* hacket = (InternalDataPacket*) thisPtr;
97 
98  hacket->legacyMsg() = pack->message();
99 
100  if (!pack->isXbusSystem())
101  index = 0;
102 
103  hacket->message().clear();
104  if (pack->isXbusSystem())
105  hacket->message().setBusId(index + 1);
106  else
107  hacket->message().setBusId(XS_BID_MASTER);
108 
109  hacket->message().setMessageId(XMID_MtData2);
110  //hacket->m_legacyMsg = pack->message();
111  hacket->deviceId() = pack->deviceId(index);
112  hacket->itemCount() = 1;
113  hacket->toa() = pack->timeOfArrival();
114  hacket->originalMessageLength() = (uint16_t)pack->originalMessage().getDataSize();
115  hacket->packetId() = pack->largePacketCounter();
116  PacketInfo info = pack->packetInfo(index);
118  XsDataIdentifier di;
119  switch (format.m_outputSettings & XOS_Dataformat_Mask) {
121  di = XDI_SubFormatFloat;
122  break;
124  di = XDI_SubFormatDouble;
125  break;
127  di = XDI_SubFormatFp1632;
128  break;
130  di = XDI_SubFormatFp1220;
131  break;
132  default:
133  di = XDI_None;
134  break;
135  }
136  if (pack->containsRawData(index))
137  hacket->setRawData(pack->rawData(index));
138  if (pack->rawTemperatureChannelCount(index) == 4)
139  {
140  XsUShortVector rawGyroTemperatures;
141  rawGyroTemperatures[0] = pack->rawTemperature(index, 1);
142  rawGyroTemperatures[1] = pack->rawTemperature(index, 2);
143  rawGyroTemperatures[2] = pack->rawTemperature(index, 3);
144  hacket->setRawGyroscopeTemperatureData(rawGyroTemperatures);
145  }
147  hacket->setCalibratedAcceleration(pack->calibratedAcceleration(index));
149  {
150  // Special copy to preserve watermarking
151  hacket->message().setDataShort(XDI_RateOfTurn | di, hacket->message().getDataSize());
152  hacket->message().setDataByte(3*hacket->getFPValueSize(di), hacket->message().getDataSize());
153  hacket->itemCount()++;
154  hacket->message().setDataBuffer(hacket->legacyMsg().getDataBuffer(info.m_calGyr), 3*hacket->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_RateOfTurn | di));
155  }
157  hacket->setCalibratedMagneticField(pack->calibratedMagneticField(index));
159  hacket->setOrientationQuaternion(pack->orientationQuaternion(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
160  if (pack->containsOrientationEuler(index))
161  hacket->setOrientationEuler(pack->orientationEuler(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
162  if (pack->containsOrientationMatrix(index))
163  hacket->setOrientationMatrix(pack->orientationMatrix(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
164  if (pack->containsPositionLLA(index))
165  hacket->setPositionLLA(pack->positionLLA(index));
166  if (pack->containsVelocity(index))
167  hacket->setVelocity(pack->velocity(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
168  if (pack->containsStatus(index))
169  {
170  bool isDetailed = true;
171  uint32_t status = pack->status(index, &isDetailed);
172 
173  // For MTw's only, the status needs to be swapEndian32-ed
174  if (hacket->deviceId().isMtw())
175  {
177  }
178 
179  if (isDetailed)
180  {
181  hacket->setStatus(status);
182  }
183  else
184  {
185  hacket->setStatusByte((uint8_t) status);
186  }
187  }
188  if (pack->containsGpsPvtData())
189  {
190  XsPressure tmp;
191  tmp.m_pressure = pack->gpsPvtData(index).m_pressure * 2.0; // Convert to Pascal
193  hacket->setPressure(tmp);
194  XsGpsPvtData pvt;
195  pvt = pack->gpsPvtData(index);
196  hacket->setGpsPvtData(pvt);
197  }
198  if (pack->containsUtcTime(index))
199  {
200  XsUtcTime time = pack->utcTime(index);
201  hacket->setUtcTime(time);
202  }
203  if (pack->containsMtwSdiData(index))
204  {
205  // not yet converted:
206  //uint8_t m_timeSync;
207  //XsVector3 m_currentBias;
208 
209  MtwSdiData mtwsdi = pack->mtwSdiData(index);
210  hacket->deviceId() = mtwsdi.m_deviceId;
211  {
212  // Special copy to preserve watermarking
213  hacket->message().setDataShort(XDI_DeltaQ | di, hacket->message().getDataSize());
214  hacket->message().setDataByte(4*hacket->getFPValueSize(di), hacket->message().getDataSize());
215  hacket->itemCount()++;
216  hacket->message().setDataBuffer(hacket->legacyMsg().getDataBuffer(info.m_wOrientationIncrement), 4*hacket->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaQ | di));
217 
218  hacket->message().setDataShort(XDI_DeltaV | di, hacket->message().getDataSize());
219  hacket->message().setDataByte(3*hacket->getFPValueSize(di), hacket->message().getDataSize());
220  hacket->itemCount()++;
221  hacket->message().setDataBuffer(hacket->legacyMsg().getDataBuffer(info.m_wVelocityIncrement), 3*hacket->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaV | di));
222  }
223 
224  //if (mtwsdi.m_aidingData)
225  hacket->setCalibratedMagneticField(mtwsdi.m_magnetoMeter);
226  if (mtwsdi.m_barometer)
227  {
228  XsPressure tmp;
229  tmp.m_pressure = mtwsdi.m_barometer * 100.0; // convert from millibar to pascal
230  tmp.m_pressureAge = mtwsdi.m_barometer?0:255;
231  hacket->setPressure(tmp);
232  }
233  hacket->setFrameRange(XsRange((int) mtwsdi.m_firstFrameNumber, (int) mtwsdi.m_lastFrameNumber));
234  hacket->setRssi(mtwsdi.m_rssi);
235  }
236  if (pack->containsTemperature(index))
237  hacket->setTemperature(pack->temperature(index));
238  if (pack->containsPacketCounter(index))
239  hacket->setPacketCounter(pack->packetCounter(index));
240 
241  // enable this when you experience weird crashes in XsByteArray_destruct or XsDataPacket_destruct
242  //validatePacket(thisPtr);
243 }
244 
245 } // 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:46
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. When it decreases relative to the previous packet...
Definition: xsgpspvtdata.h:34
Class to store strapdown integration data.
Definition: mtwsdidata.h:19
#define swapEndian32(src)
Definition: cmtmessage.h:72
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:19
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:43
XsDeviceId m_deviceId
The ID of the device that generated the data.
Definition: mtwsdidata.h:21
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:18
void validatePacket(XsDataPacket *thisPtr)
uint16_t m_firstFrameNumber
The first frame number of the SDI interval. The time of the interval is [first, last) ...
Definition: mtwsdidata.h:23
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:138
uint16_t m_itemCount
The number of data items in the message.
double m_barometer
The barometer value during the interval.
Definition: mtwsdidata.h:29
XsGpsPvtData gpsPvtData(int32_t index=0) const
Return the Gps PVT data component of a data item.
GLuint index
Definition: glext.h:3891
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:31
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:43
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:30
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. The time of the interval is [first, last) ...
Definition: mtwsdidata.h:24
uint16_t m_pressure
The pressure measurement in units of 2 Pascal, only valid if m_pressureAge is not 255...
Definition: xsgpspvtdata.h:33
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.
backing_store_ptr info
Definition: jmemsys.h:170
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:109
_u8 status
Definition: rplidar_cmd.h:21
uint16_t & originalMessageLength()
Length of the original message payload.
Contains data received from a device or read from a file.
Definition: xsdatapacket.h:180
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:3513
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:49
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:32
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.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019