softwarePLL.cpp
Go to the documentation of this file.
1 /*
2 ====================================================================================================
3 File: softwarePLL.cpp
4 ====================================================================================================
5 */
6 #include "softwarePLL.h"
7 #include <iostream>
8 // #include <chrono>
9 // #include <thread>
10 #include <math.h>
11 #include <iterator>
12 #include <iostream>
13 #include <fstream>
14 #include <sstream>
15 #include <vector>
16 #include <string>
17 
18 
19 const double SoftwarePLL::MaxAllowedTimeDeviation = 0.1;
20 const uint32_t SoftwarePLL::MaxExtrapolationCounter = 20;
21 
22 // Helper class for reading csv file with test data
23 
24 class CSVRow
25 {
26 public:
27  std::string const &operator[](std::size_t index) const
28  {
29  return m_data[index];
30  }
31 
32  std::size_t size() const
33  {
34  return m_data.size();
35  }
36 
37  void readNextRow(std::istream &str)
38  {
39  std::string line;
40  std::getline(str, line);
41 
42  std::stringstream lineStream(line);
43  std::string cell;
44 
45  m_data.clear();
46  while (std::getline(lineStream, cell, ';'))
47  {
48  m_data.push_back(cell);
49  }
50  // This checks for a trailing comma with no data after it.
51  if (!lineStream && cell.empty())
52  {
53  // If there was a trailing comma then add an empty element.
54  m_data.push_back("");
55  }
56  }
57 
58 private:
59  std::vector<std::string> m_data;
60 };
61 
62 std::istream &operator>>(std::istream &str, CSVRow &data)
63 {
64  data.readNextRow(str);
65  return str;
66 }
67 
68 
69 bool SoftwarePLL::pushIntoFifo(double curTimeStamp, uint32_t curtick)
70 // update tick fifo and update clock (timestamp) fifo
71 {
72  for (int i = 0; i < fifoSize - 1; i++)
73  {
74  tickFifo[i] = tickFifo[i + 1];
75  clockFifo[i] = clockFifo[i + 1];
76  }
77  tickFifo[fifoSize - 1] = curtick; // push most recent tick and timestamp into fifo
78  clockFifo[fifoSize - 1] = curTimeStamp;
79 
81  {
82  numberValInFifo++; // remember the number of valid number in fifo
83  }
84  FirstTick(tickFifo[0]);
86 
87  return (true);
88 }
89 
91 {
92  int32_t tempTick = 0;
93  tempTick = tick - (uint32_t) (0xFFFFFFFF & FirstTick());
94  double timeDiff = tempTick * this->InterpolationSlope();
95  return (timeDiff);
96 
97 }
98 
99 int SoftwarePLL::findDiffInFifo(double diff, double tol)
100 {
101  int numFnd = 0;
102  double minAllowedDiff = (1.0 - tol) * diff;
103  double maxAllowedDiff = (1.0 + tol) * diff;
104 
105  for (int i = 0; i < numberValInFifo - 1; i++)
106  {
107  double diffTime = this->clockFifo[i + 1] - clockFifo[i];
108  if ((diffTime >= minAllowedDiff) && (diffTime <= maxAllowedDiff))
109  {
110  numFnd++;
111  }
112  }
113 
114  return (numFnd);
115 }
116 
125 bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
126 {
127  if (curtick != this->lastcurtick)
128  {
129  this->lastcurtick = curtick;
130  double start = sec + nanoSec * 1E-9;
131  bool bRet = true;
132 
133  if (false == IsInitialized())
134  {
135  pushIntoFifo(start, curtick);
136  bool bCheck = this->updateInterpolationSlope();
137  if (bCheck)
138  {
139  IsInitialized(true);
140  }
141  }
142 
143  if (IsInitialized() == false)
144  {
145  return (false);
146  }
147 
148  double relTimeStamp = extraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen
149  double cmpTimeStamp = start - this->FirstTimeStamp();
150 
151  bool timeStampVerified = false;
152  if (nearSameTimeStamp(relTimeStamp, cmpTimeStamp) == true)// if timestamp matches prediction update FIFO
153  {
154  timeStampVerified = true;
155  pushIntoFifo(start, curtick);
158  }
159 
160  if (timeStampVerified == false)
161  {
162  // BEGIN HANDLING Extrapolation divergence
163  uint32_t tmp = ExtrapolationDivergenceCounter();
164  tmp++;
167  {
168  IsInitialized(false); // reset FIFO - maybe happened due to abrupt change of time base
169  }
170  // END HANDLING Extrapolation divergence
171  }
172  return (true);
173  }
174  else
175  {
176  return (false);
177  //this curtick has been updated allready
178  }
179 
180 }
181 
182 //TODO Kommentare
183 bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t curtick)
184 {
185  if (IsInitialized() == false)
186  {
187  return (false);
188  }
189 
190  double relTimeStamp = extraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen
191  double corrTime = relTimeStamp + this->FirstTimeStamp();
192  sec = (uint32_t) corrTime;
193  double frac = corrTime - sec;
194  nanoSec = (uint32_t) (1E9 * frac);
195  return (true);
196 }
197 
198 bool SoftwarePLL::nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2)
199 {
200  double dTAbs = fabs(relTimeStamp1 - relTimeStamp2);
201  if (dTAbs < AllowedTimeDeviation())
202  {
203  return (true);
204  }
205  else
206  {
207  return (false);
208  }
209 }
210 
211 bool SoftwarePLL::updateInterpolationSlope() // fifo already updated
212 {
213 
215  {
216  return (false);
217  }
218  std::vector<uint64_t> tickFifoUnwrap;
219  std::vector<double> clockFifoUnwrap;
220  clockFifoUnwrap.resize(fifoSize);
221  tickFifoUnwrap.resize(fifoSize);
222  uint64_t tickOffset = 0;
223  clockFifoUnwrap[0] = 0.00;
224  tickFifoUnwrap[0] = 0;
225  FirstTimeStamp(this->clockFifo[0]);
226  FirstTick(this->tickFifo[0]);
227 
228  uint64_t tickDivisor = 0x100000000;
229 
230 
231  for (int i = 1;
232  i < fifoSize; i++) // typical 643 for 20ms -> round about 32150 --> near to 32768 standard clock in many watches
233  {
234  if (tickFifo[i] < tickFifo[i - 1]) // Overflow
235  {
236  tickOffset += tickDivisor;
237  }
238  tickFifoUnwrap[i] = tickOffset + tickFifo[i] - FirstTick();
239  clockFifoUnwrap[i] = (this->clockFifo[i] - FirstTimeStamp());
240  }
241 
242  double sum_xy = 0.0;
243  double sum_x = 0.0;
244  double sum_y = 0.0;
245  double sum_xx = 0.0;
246  for (int i = 0; i < fifoSize; i++)
247  {
248  sum_xy += tickFifoUnwrap[i] * clockFifoUnwrap[i];
249  sum_x += tickFifoUnwrap[i];
250  sum_y += clockFifoUnwrap[i];
251  sum_xx += tickFifoUnwrap[i] * tickFifoUnwrap[i];
252  }
253 
254  // calculate slope of regression line, interception is 0 by construction
255  double m = (fifoSize * sum_xy - sum_x * sum_y) / (fifoSize * sum_xx - sum_x * sum_x);
256 
257  int matchCnt = 0;
258  for (int i = 0; i < fifoSize; i++)
259  {
260  double yesti = m * tickFifoUnwrap[i];
261  if (this->nearSameTimeStamp(yesti, clockFifoUnwrap[i]))
262  {
263  matchCnt++;
264  }
265  }
266 
267  bool retVal = false;
268  if (matchCnt == fifoSize)
269  {
271  retVal = true;
272  }
273 
274  return (retVal);
275 }
276 
277 #if 0
278 bool SoftwarePLL::getDemoFileData(std::string fileName, std::vector<uint32_t>& tickVec,std::vector<uint32_t>& secVec, std::vector<uint32_t>& nanoSecVec )
279 {
280  std::ifstream file(fileName);
281 
282  CSVRow row;
283  tickVec.clear();
284  secVec.clear();
285  nanoSecVec.clear();
286  int lineCnt = 0;
287  while (file >> row) {
288  if (lineCnt > 0)
289  {
290  uint32_t tickVal = (uint32_t)std::stoi(row[0]);
291  uint32_t secVal = (uint32_t)std::stoi(row[1]);
292  uint32_t nanoSecVal = (uint32_t)std::stoi(row[2]);
293  tickVec.push_back(tickVal);
294  secVec.push_back(secVal);
295  nanoSecVec.push_back(nanoSecVal);
296  }
297  lineCnt++;
298  }
299  if (lineCnt <= 1)
300  return false;
301  else
302  return true;
303 }
304 #endif
305 
306 //TODO update testbed
308 {
309  std::cout << "Running testbed for SofwarePLL" << std::endl;
310  uint32_t curtick = 0;
311  int cnt = 0;
312 
313  SoftwarePLL testPll;
314  uint32_t sec = 9999;
315  uint32_t nanoSec = 0;
316  double tickPerSec = 1E6;
317  uint32_t tickInc = 1000;
318  int maxLoop = 20;
319 
320  std::vector<uint32_t> tickVec;
321  std::vector<uint32_t> secVec;
322  std::vector<uint32_t> nanoSecVec;
323  bool bRet = false;
324 
325  bool testWithDataFile = true;
326  if (testWithDataFile)
327  {
328  // commented for trusty bRet = testPll.getDemoFileData("/home/rosuser/dumpimu3.csv", tickVec, secVec, nanoSecVec);
329  maxLoop = tickVec.size();
330  }
331 
332  for (int i = 0; i < maxLoop; i++)
333  {
334  if (testWithDataFile)
335  {
336  curtick = tickVec[i];
337  sec = secVec[i];
338  nanoSec = nanoSecVec[i];
339  }
340  else
341  {
342  cnt++;
343  curtick += tickInc; // increment tick counter
344  double deltaT = tickInc / tickPerSec;
345  nanoSec += (int) (deltaT * 1E9);
346  if (nanoSec >= 1E9)
347  {
348  nanoSec = 0;
349  sec++;
350  }
351  if (cnt >= 8)
352  {
353  sec++;
354  }
355  }
356  printf("Before correction: %3d.%09d\n", sec, nanoSec);
357  uint32_t org_sec = sec;
358  uint32_t org_nanoSec = nanoSec;
359 
360  bool bRet = testPll.getCorrectedTimeStamp(sec, nanoSec, curtick);
361 
362  bool corrected = false;
363  if ((nanoSec != org_nanoSec) || (sec != org_sec))
364  {
365  corrected = true;
366  }
367  printf("After correction : %3d.%09d %s %s\n", sec, nanoSec, bRet ? "OK " : "DISMISS",
368  corrected ? "MODI." : "OK ");
369  }
370 
371  return;
372 }
373 
374 
375 #ifdef softwarePLL_MAINTEST
376 int main(int argc, char **argv)
377 {
378  printf("Test for softwarePLL-Class\n");
379  printf("\n");
381 }
382 #endif
383 
384 
385 
386 /*
387 Example CMakeLists.txt to generate test-binary-file for testing this class
388 --- CUT ---
389 #
390 #
391 # softwarePLL
392 #
393 #
394 cmake_minimum_required(VERSION 2.8)
395 cmake_policy(SET CMP0015 NEW)
396 project( softwarePLL )
397 #
398 #
399 add_definitions(-D${PROJECT_NAME}_MAINTEST)
400 
401 MESSAGE( STATUS "CMKAKE for " ${PROJECT_NAME} )
402 
403 include_directories( inc)
404 file( GLOB LIB_SOURCES src/ *.cpp )
405 
406 if(WIN32)
407 else()
408 set(CMAKE_CXX_STANDARD 11)
409 endif()
410 
411 add_executable( ${PROJECT_NAME} ${LIB_SOURCES} inc/${PROJECT_NAME}.h)
412 target_link_libraries( ${PROJECT_NAME})
413 --- CUT ---
414 
415 */
SoftwarePLL
class SoftwarePLL implements synchronisation between ticks and timestamp. See https://github....
Definition: softwarePLL.h:20
CSVRow::m_data
std::vector< std::string > m_data
Definition: softwarePLL.cpp:59
SoftwarePLL::AllowedTimeDeviation
double AllowedTimeDeviation() const
Definition: softwarePLL.h:66
SoftwarePLL::getDemoFileData
bool getDemoFileData(std::string fileName, std::vector< uint32_t > &tickVec, std::vector< uint32_t > &secVec, std::vector< uint32_t > &nanoSecVec)
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:125
SoftwarePLL::extraPolateRelativeTimeStamp
double extraPolateRelativeTimeStamp(uint32_t tick)
Definition: softwarePLL.cpp:90
SoftwarePLL::FirstTick
uint64_t FirstTick() const
Definition: softwarePLL.h:48
SoftwarePLL::getCorrectedTimeStamp
bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick)
Definition: softwarePLL.cpp:183
data
data
CSVRow::readNextRow
void readNextRow(std::istream &str)
Definition: softwarePLL.cpp:37
SoftwarePLL::lastcurtick
uint32_t lastcurtick
Definition: softwarePLL.h:99
SoftwarePLL::pushIntoFifo
bool pushIntoFifo(double curTimeStamp, uint32_t curtick)
Definition: softwarePLL.cpp:69
softwarePLL.h
operator>>
std::istream & operator>>(std::istream &str, CSVRow &data)
Definition: softwarePLL.cpp:62
SoftwarePLL::numberValInFifo
int numberValInFifo
Definition: softwarePLL.h:86
CSVRow
Definition: softwarePLL.cpp:24
SoftwarePLL::MaxExtrapolationCounter
static const uint32_t MaxExtrapolationCounter
Definition: softwarePLL.h:88
SoftwarePLL::ExtrapolationDivergenceCounter
uint32_t ExtrapolationDivergenceCounter() const
Definition: softwarePLL.h:72
SoftwarePLL::tickFifo
uint32_t tickFifo[fifoSize]
Definition: softwarePLL.h:89
SoftwarePLL::testbed
static void testbed()
Definition: softwarePLL.cpp:307
SoftwarePLL::clockFifo
double clockFifo[fifoSize]
Definition: softwarePLL.h:90
start
ROSCPP_DECL void start()
SoftwarePLL::FirstTimeStamp
double FirstTimeStamp() const
Definition: softwarePLL.h:54
SoftwarePLL::MaxAllowedTimeDeviation
static const double MaxAllowedTimeDeviation
Definition: softwarePLL.h:87
SoftwarePLL::nearSameTimeStamp
bool nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2)
Definition: softwarePLL.cpp:198
SoftwarePLL::IsInitialized
bool IsInitialized() const
Definition: softwarePLL.h:42
CSVRow::size
std::size_t size() const
Definition: softwarePLL.cpp:32
SoftwarePLL::updateInterpolationSlope
bool updateInterpolationSlope()
Definition: softwarePLL.cpp:211
main
int main(int argc, char **argv)
Startup routine - if called with no argmuments we assume debug session. Set scanner name variable by ...
Definition: sick_generic_caller.cpp:141
SoftwarePLL::findDiffInFifo
int findDiffInFifo(double diff, double tol)
Definition: softwarePLL.cpp:99
SoftwarePLL::fifoSize
static const int fifoSize
Definition: softwarePLL.h:82
SoftwarePLL::InterpolationSlope
double InterpolationSlope() const
Definition: softwarePLL.h:60
CSVRow::operator[]
const std::string & operator[](std::size_t index) const
Definition: softwarePLL.cpp:27


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