SoftwarePLL.cpp
Go to the documentation of this file.
1 /*
2 ====================================================================================================
3 File: SoftwarePLL.cpp
4 Note: SoftwarePLL intentionally copied from https://github.com/michael1309/SoftwarePLL
5 See https://github.com/michael1309/SoftwarePLL/blob/master/README.md for details.
6 ====================================================================================================
7 */
9 
10 // #include "softwarePLL.h"
11 #include <iostream>
12 // #include <chrono>
13 // #include <thread>
14 #include <math.h>
15 #include <iterator>
16 #include <iostream>
17 #include <fstream>
18 #include <sstream>
19 #include <vector>
20 #include <string>
21 
22 
23 std::map<std::string,SoftwarePLL*> SoftwarePLL::_instances; // list of SoftwarePLL instances, mapped by id
24 
25 const double SoftwarePLL::MaxAllowedTimeDeviation_ = 0.1;
26 const uint32_t SoftwarePLL::MaxExtrapolationCounter_ = 20;
27 
28 SoftwarePLL& SoftwarePLL::Instance(const std::string & id, int fifo_length)
29 {
30  SoftwarePLL* pll = _instances[id];
31  if(!pll)
32  {
33  pll = new SoftwarePLL(fifo_length);
34  _instances[id] = pll;
35  }
36  return *pll;
37 }
38 
40 {
41  // Remove this from the list of SoftwarePLL instances
42  for(std::map<std::string,SoftwarePLL*>::iterator iter = _instances.begin(); iter != _instances.end(); )
43  {
44  if(iter->second == this)
45  iter = _instances.erase(iter);
46  else
47  iter++;
48  }
49 }
50 
51 bool SoftwarePLL::PushIntoFifo(double curTimeStamp, uint32_t curtick)
52 // update tick fifo and update clock (timestamp) fifo
53 {
54  for (int i = 0; i < FifoSize_ - 1; i++)
55  {
56  TickFifo_[i] = TickFifo_[i + 1];
57  ClockFifo_[i] = ClockFifo_[i + 1];
58  }
59  TickFifo_[FifoSize_ - 1] = curtick; // push most recent tick and timestamp into fifo
60  ClockFifo_[FifoSize_ - 1] = curTimeStamp;
61 
63  {
64  NumberValInFifo_++; // remember the number of valid number in fifo
65  }
66  FirstTick(TickFifo_[0]);
68 
69  return(true);
70 }
71 
73 {
74  int32_t tempTick =0;
75  tempTick = tick-(uint32_t)(0xFFFFFFFF & FirstTick());
76  double timeDiff = tempTick * this->InterpolationSlope();
77  return(timeDiff);
78 
79 }
80 
81 bool SoftwarePLL::UpdatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
82 {
83  if(curtick!=this->Lastcurtick_)
84  {
85  this->Lastcurtick_ = curtick;
86  double start = sec + nanoSec * 1E-9;
87  // bool bRet = true;
88 
89  if (false == IsInitialized())
90  {
91  PushIntoFifo(start, curtick);
92  bool bCheck = this->UpdateInterpolationSlope();
93  if (bCheck)
94  {
95  IsInitialized(true);
96  }
97  }
98 
99  if (IsInitialized() == false)
100  {
101  return (false);
102  }
103 
104  double relTimeStamp = ExtraPolateRelativeTimeStamp(curtick);
105  double cmpTimeStamp = start - this->FirstTimeStamp();
106 
107  bool timeStampVerified = false;
108  if (NearSameTimeStamp(relTimeStamp, cmpTimeStamp) == true)// if timestamp matches prediction update FIFO
109  {
110  timeStampVerified = true;
111  PushIntoFifo(start, curtick);
114  }
115 
116  if (timeStampVerified == false)
117  {
118  // BEGIN HANDLING Extrapolation divergence
119  uint32_t tmp = ExtrapolationDivergenceCounter();
120  tmp++;
123  {
124  IsInitialized(false); // reset FIFO - maybe happened due to abrupt change of time base
125  }
126  // END HANDLING Extrapolation divergence
127  }
128  return(true);
129  }
130  else
131  {
132  return(false);
133  //this curtick has been updated allready
134  }
135 
136 }
137 
138 bool SoftwarePLL::GetCorrectedTimeStamp(uint32_t& sec, uint32_t& nanoSec, uint32_t curtick)
139 {
140  if (IsInitialized() == false)
141  {
142  return(false);
143  }
144 
145  double relTimeStamp = ExtraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen
146  double corrTime = relTimeStamp + this->FirstTimeStamp();
147  sec = (uint32_t)corrTime;
148  double frac = corrTime - sec;
149  nanoSec = (uint32_t)(1E9 * frac);
150  return(true);
151 }
152 
153 bool SoftwarePLL::NearSameTimeStamp(double relTimeStamp1, double relTimeStamp2)
154 {
155  double dTAbs = fabs(relTimeStamp1 - relTimeStamp2);
156  if (dTAbs < AllowedTimeDeviation())
157  {
158  return(true);
159  }
160  else
161  {
162  return(false);
163  }
164 }
165 
166 bool SoftwarePLL::UpdateInterpolationSlope() // fifo already updated
167 {
168 
170  {
171  return(false);
172  }
173  std::vector<uint64_t> tickFifoUnwrap;
174  std::vector<double> clockFifoUnwrap;
175  clockFifoUnwrap.resize(FifoSize_);
176  tickFifoUnwrap.resize(FifoSize_);
177  uint64_t tickOffset = 0;
178  clockFifoUnwrap[0] = 0.00;
179  tickFifoUnwrap[0] = 0;
180  FirstTimeStamp(this->ClockFifo_[0]);
181  FirstTick(this->TickFifo_[0]);
182 
183  uint64_t tickDivisor = 0x100000000;
184 
185 
186 
187  for (int i = 1; i < FifoSize_; i++) // typical 643 for 20ms -> round about 32150 --> near to 32768 standard clock in many watches
188  {
189  if (TickFifo_[i] < TickFifo_[i - 1]) // Overflow
190  {
191  tickOffset += tickDivisor;
192  }
193  tickFifoUnwrap[i] = tickOffset + TickFifo_[i] - FirstTick();
194  clockFifoUnwrap[i] = (this->ClockFifo_[i] - FirstTimeStamp());
195  }
196 
197  double sum_xy = 0.0;
198  double sum_x = 0.0;
199  double sum_y = 0.0;
200  double sum_xx = 0.0;
201  for (int i = 0; i < FifoSize_; i++)
202  {
203  sum_xy += tickFifoUnwrap[i] * clockFifoUnwrap[i];
204  sum_x += tickFifoUnwrap[i];
205  sum_y += clockFifoUnwrap[i];
206  sum_xx += tickFifoUnwrap[i] * tickFifoUnwrap[i];
207  }
208 
209  // calculate slope of regression line, interception is 0 by construction
210  double m = (FifoSize_ * sum_xy - sum_x * sum_y) / (FifoSize_ * sum_xx - sum_x*sum_x);
211 
212  int matchCnt = 0;
213  for (int i = 0; i < FifoSize_; i++)
214  {
215  double yesti = m * tickFifoUnwrap[i];
216  if (this->NearSameTimeStamp(yesti, clockFifoUnwrap[i]))
217  {
218  matchCnt++;
219  }
220  }
221 
222  bool retVal = false;
223  if (matchCnt == FifoSize_)
224  {
226  retVal = true;
227  }
228 
229  return(retVal);
230 }
231 
232 /*
233 Example CMakeLists.txt to generate test-binary-file for testing this class
234 --- CUT ---
235 #
236 #
237 # softwarePLL
238 #
239 #
240 cmake_minimum_required(VERSION 2.8)
241 cmake_policy(SET CMP0015 NEW)
242 project( softwarePLL )
243 #
244 #
245 add_definitions(-D${PROJECT_NAME}_MAINTEST)
246 
247 MESSAGE( STATUS "CMKAKE for " ${PROJECT_NAME} )
248 
249 include_directories( inc)
250 file( GLOB LIB_SOURCES src/ *.cpp )
251 
252 if(WIN32)
253 else()
254 set(CMAKE_CXX_STANDARD 11)
255 endif()
256 
257 add_executable( ${PROJECT_NAME} ${LIB_SOURCES} inc/${PROJECT_NAME}.h)
258 target_link_libraries( ${PROJECT_NAME})
259 --- CUT ---
260 
261 */
SoftwarePLL
class SoftwarePLL implements synchronisation between ticks and timestamp. See https://github....
Definition: softwarePLL.h:20
SoftwarePLL::AllowedTimeDeviation
double AllowedTimeDeviation() const
Definition: softwarePLL.h:66
SoftwarePLL::SoftwarePLL
SoftwarePLL()
Definition: softwarePLL.h:111
SoftwarePLL::~SoftwarePLL
~SoftwarePLL()
Definition: softwarePLL.h:29
SoftwarePLL::Lastcurtick_
uint32_t Lastcurtick_
Definition: SoftwarePLL.h:181
SoftwarePLL::NearSameTimeStamp
bool NearSameTimeStamp(double relTimeStamp1, double relTimeStamp2)
Definition: SoftwarePLL.cpp:153
SoftwarePLL::FirstTick
uint64_t FirstTick() const
Definition: softwarePLL.h:48
SoftwarePLL::ExtraPolateRelativeTimeStamp
double ExtraPolateRelativeTimeStamp(uint32_t tick)
Extrapolates and returns the measurement timestamp in seconds relative to FirstTimeStamp....
Definition: SoftwarePLL.cpp:72
SoftwarePLL::GetCorrectedTimeStamp
bool GetCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick)
Computes the timestamp of a measurement from sensor ticks.
Definition: SoftwarePLL.cpp:138
SoftwarePLL::FifoSize_
const int FifoSize_
Definition: SoftwarePLL.h:172
SoftwarePLL::PushIntoFifo
bool PushIntoFifo(double curTimeStamp, uint32_t curtick)
Pushes measurement timestamp and sensor ticks to the fifo, updates tick fifo and clock (timestamp) fi...
Definition: SoftwarePLL.cpp:51
SoftwarePLL.h
SoftwarePLL::NumberValInFifo_
int NumberValInFifo_
Definition: SoftwarePLL.h:171
SoftwarePLL::ExtrapolationDivergenceCounter
uint32_t ExtrapolationDivergenceCounter() const
Definition: softwarePLL.h:72
SoftwarePLL::TickFifo_
std::vector< uint32_t > TickFifo_
Definition: SoftwarePLL.h:175
SoftwarePLL::UpdateInterpolationSlope
bool UpdateInterpolationSlope()
Definition: SoftwarePLL.cpp:166
SoftwarePLL::UpdatePLL
bool UpdatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
Updates PLL internale State should be called only with network send timestamps.
Definition: SoftwarePLL.cpp:81
start
ROSCPP_DECL void start()
SoftwarePLL::FirstTimeStamp
double FirstTimeStamp() const
Definition: softwarePLL.h:54
SoftwarePLL::MaxExtrapolationCounter_
static const uint32_t MaxExtrapolationCounter_
Definition: SoftwarePLL.h:174
SoftwarePLL::IsInitialized
bool IsInitialized() const
Definition: softwarePLL.h:42
SoftwarePLL::_instances
static std::map< std::string, SoftwarePLL * > _instances
Definition: SoftwarePLL.h:197
SoftwarePLL::ClockFifo_
std::vector< double > ClockFifo_
Definition: SoftwarePLL.h:176
SoftwarePLL::InterpolationSlope
double InterpolationSlope() const
Definition: softwarePLL.h:60
SoftwarePLL::Instance
static SoftwarePLL & Instance(const std::string &id="", int fifo_length=7)
Creates an instance of SoftwarePLL or returns an existing one, given its id.
Definition: SoftwarePLL.cpp:28
SoftwarePLL::MaxAllowedTimeDeviation_
static const double MaxAllowedTimeDeviation_
Definition: SoftwarePLL.h:173


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19