Main MRPT website > C++ reference for MRPT 1.5.7
packetstamper.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/xsdatapacket.h>
10 #include "packetstamper.h"
11 
12 /*! \class PacketStamper
13  \brief Supplies functionality for timestamping data packets.
14  \details This class can analyze a data packet and create a proper packet id for it.
15 */
16 
17 //! \brief 16 bit MT Sample Counter boundary
18 const int64_t PacketStamper::MTSCBOUNDARY = 0x00010000LL;
19 //! \brief 16 bit MT Sample Counter boundary inclusive mask
20 const int64_t PacketStamper::MTSCBOUNDARY_LOWMASK = (MTSCBOUNDARY-1);
21 //! \brief 16 bit MT Sample Counter boundary exclusive mask
22 const int64_t PacketStamper::MTSCBOUNDARY_HIGHMASK = (~MTSCBOUNDARY_LOWMASK);
23 //! \brief 16 bit MT Sample Counter boundary/2, used for determining SC wrapping
24 const int64_t PacketStamper::MTSCBOUNDARY_HALF = (MTSCBOUNDARY/2);
25 
26 //! \brief 8 bit Sample Counter boundary
27 const int64_t PacketStamper::SC8BOUNDARY = 0x00000100LL;
28 //! \brief 8 bit Sample Counter boundary inclusive mask
29 const int64_t PacketStamper::SC8BOUNDARY_LOWMASK = (SC8BOUNDARY-1);
30 //! \brief 8 bit Sample Counter boundary exclusive mask
31 const int64_t PacketStamper::SC8BOUNDARY_HIGHMASK = (~SC8BOUNDARY_LOWMASK);
32 //! \brief 8 bit Sample Counter boundary/2, used for determining SC wrapping
33 const int64_t PacketStamper::SC8BOUNDARY_HALF = (SC8BOUNDARY/2);
34 
35 /*! \brief Create 64 bit counter for a packet.
36  \details Wrap when new XsDataPacket is too far away from the previous XsDataPacket in time.
37  Use half cache size as reasonable time difference
38  When infinite cache, simply wrap when new is lower than old
39  \param pack The XsDataPacket that needs its 64-bit sample counter updated
40  \param highestPacket The highest packet available for the current device, it will be updated if
41  the new counter is higher than the stored value.
42  \returns The computed counter for the packet.
43 */
45 {
46 // int did = 0;
47 // if (pack.containsMtwSdiData())
48 // {
49 // MtwSdiData sdi = pack.mtwSdiData();
50 // did = sdi.m_deviceId;
51 // JLDEBUG(gJournal, "XsensDeviceAPI", "%s [%08x] SDI interval (%d-%d)\n", __FUNCTION__, sdi.m_deviceId, sdi.m_firstFrameNumber, sdi.m_lastFrameNumber);
52 // }
53 
54  //! \todo This could be a (couple of) milliseconds too late, this should be set as soon as the source message arrives: mantis 7157
55  pack.setTimeOfArrival(XsTimeStamp::now());
56  int64_t newCounter, lastCounter = -1;
57 
58  if (!highestPacket.empty())
59  lastCounter = highestPacket.packetId().msTime();
60 
61  if (pack.containsPacketCounter())
62  newCounter = calculateLargePacketCounter(pack.packetCounter(), lastCounter);
63  else if (pack.containsSampleTimeFine())
64  {
65  if (pack.containsSampleTimeCoarse())
66  newCounter = (int64_t) pack.sampleTime64();
67  else
68  newCounter = calculateLargeSampleTime((int32_t) pack.sampleTimeFine(), lastCounter);
69  }
70  else if (pack.containsPacketCounter8())
71  newCounter = calculateLargePacketCounter8(pack.packetCounter8(), lastCounter);
72  else
73  newCounter = lastCounter + 1;
74 
75 // JLDEBUG(gJournal, "XsensDeviceAPI", "%s [%08x] old = %I64d new = %I64d diff = %I64d\n", __FUNCTION__, did, lastCounter, newCounter, (newCounter-lastCounter));
76 
77  pack.setPacketId(newCounter);
78  if (newCounter > lastCounter)
79  highestPacket = pack;
80 
81  return newCounter;
82 }
83 
84 /*! \brief Calculate the new large packet counter value based on \a frameCounter and the \a lastCounter
85  \details Wraparound is at the 8-bit boundary
86  \param[in] frameCounter The frame counter
87  \param[in] lastCounter The last counter
88  \returns The computed packet counter value
89  \note If lastCounter < 0, returns frameCounter
90 */
92 {
93  if (lastCounter < 0) {
94  return frameCounter;
95  }
96 
97  int64_t low = lastCounter & SC8BOUNDARY_LOWMASK;
98  int64_t dt = frameCounter - low;
99  if (dt < -SC8BOUNDARY_HALF)
100  return lastCounter + dt + SC8BOUNDARY; // positive wraparound
101  if (dt < SC8BOUNDARY_HALF)
102  return lastCounter + dt; // normal increment
103 
104  return lastCounter + dt - SC8BOUNDARY; // negative wraparound
105 }
106 
107 /*! \brief Calculate the new large sample counter value based on \a frameCounter and the \a lastCounter
108  \details Wraparound is at the 16-bit boundary
109  \param[in] frameCounter The frame counter
110  \param[in] lastCounter The last counter
111  \returns The computed packet counter value
112  \note If lastCounter < 0, returns frameCounter
113 */
115 {
116  if (lastCounter < 0) {
117  return frameCounter;
118  }
119 
120  int64_t low = lastCounter & MTSCBOUNDARY_LOWMASK;
121  int64_t dt = frameCounter - low;
122  if (dt < -MTSCBOUNDARY_HALF)
123  return lastCounter + dt + MTSCBOUNDARY; // positive wraparound
124  if (dt < MTSCBOUNDARY_HALF)
125  return lastCounter + dt; // normal increment
126 
127  return lastCounter + dt - MTSCBOUNDARY; // negative wraparound
128 }
129 
130 /*! \brief Calculate the new large sample time value based on \a frameTime and the \a lastTime
131  \details Wraparound is at 864000000 (1 day @ 10kHz)
132  \param[in] frameTime The frame time
133  \param[in] lastTime The last time
134  \returns The computed packet counter value
135  \note If lastTime < 0, returns frameTime
136 */
138 {
139  if (lastTime < 0) {
140  return frameTime;
141  }
142  int64_t low = lastTime % 864000000;
143  int64_t dt = frameTime - low;
144  if (dt < (-864000000/2))
145  return lastTime + dt + 864000000; // positive wraparound
146  if (dt < (864000000/2))
147  return lastTime + dt; // normal increment
148 
149  return lastTime + dt - 864000000; // negative wraparound
150 }
static int64_t stampPacket(XsDataPacket &pack, XsDataPacket &highest)
Create 64 bit counter for a packet.
static const int64_t SC8BOUNDARY_HIGHMASK
8 bit Sample Counter boundary exclusive mask
Definition: packetstamper.h:26
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
static const int64_t SC8BOUNDARY
8 bit Sample Counter boundary
Definition: packetstamper.h:24
static const int64_t SC8BOUNDARY_HALF
8 bit Sample Counter boundary/2, used for determining SC wrapping
Definition: packetstamper.h:27
static int64_t calculateLargeSampleTime(int64_t frameTime, int64_t lastTime)
Calculate the new large sample time value based on frameTime and the lastTime.
__int64 int64_t
Definition: rptypes.h:51
static const int64_t SC8BOUNDARY_LOWMASK
8 bit Sample Counter boundary inclusive mask
Definition: packetstamper.h:25
static int64_t calculateLargePacketCounter(int64_t frameCounter, int64_t lastCounter)
Calculate the new large sample counter value based on frameCounter and the lastCounter.
__int32 int32_t
Definition: rptypes.h:48
static const int64_t MTSCBOUNDARY_LOWMASK
16 bit MT Sample Counter boundary inclusive mask
Definition: packetstamper.h:20
static int64_t calculateLargePacketCounter8(int64_t frameCounter, int64_t lastCounter)
Calculate the new large packet counter value based on frameCounter and the lastCounter.
static const int64_t MTSCBOUNDARY_HALF
16 bit MT Sample Counter boundary/2, used for determining SC wrapping
Definition: packetstamper.h:22
static const int64_t MTSCBOUNDARY
16 bit MT Sample Counter boundary
Definition: packetstamper.h:19
Contains data received from a device or read from a file.
Definition: xsdatapacket.h:180
static const int64_t MTSCBOUNDARY_HIGHMASK
16 bit MT Sample Counter boundary exclusive mask
Definition: packetstamper.h:21



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019