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