39 std::getline(str, line);
41 std::stringstream lineStream(line);
45 while(std::getline(lineStream, cell,
';'))
50 if (!lineStream && cell.empty())
71 for (
int i = 0; i < fifoSize - 1; i++)
73 tickFifo[i] = tickFifo[i + 1];
74 clockFifo[i] = clockFifo[i + 1];
76 tickFifo[fifoSize - 1] = curtick;
77 clockFifo[fifoSize - 1] = curTimeStamp;
79 if (numberValInFifo < fifoSize)
83 FirstTick(tickFifo[0]);
84 FirstTimeStamp(clockFifo[0]);
91 uint32_t tempTick = tick;
92 tempTick -= (uint32_t)(0xFFFFFFFF & FirstTick());
93 double timeDiff = tempTick * this->InterpolationSlope();
101 double minAllowedDiff = (1.0 - tol) * diff;
102 double maxAllowedDiff = (1.0 + tol) * diff;
104 for (
int i = 0; i < numberValInFifo - 1; i++)
106 double diffTime = this->clockFifo[i+1] - clockFifo[i];
107 if ((diffTime >= minAllowedDiff) && (diffTime <= maxAllowedDiff))
126 double start = sec + nanoSec * 1E-9;
129 if (
false == IsInitialized())
131 pushIntoFifo(start, curtick);
132 bool bCheck = this->updateInterpolationSlope();
139 if (IsInitialized() ==
false)
144 double relTimeStamp = extraPolateRelativeTimeStamp(curtick);
145 double cmpTimeStamp = start - this->FirstTimeStamp();
147 bool timeStampVerified =
false;
148 if (nearSameTimeStamp(relTimeStamp, cmpTimeStamp) ==
true)
150 timeStampVerified =
true;
151 pushIntoFifo(start, curtick);
152 updateInterpolationSlope();
153 ExtrapolationDivergenceCounter(0);
156 if (timeStampVerified ==
false)
159 uint32_t tmp = ExtrapolationDivergenceCounter();
161 ExtrapolationDivergenceCounter(tmp);
164 IsInitialized(
false);
174 if (IsInitialized() ==
false)
179 double relTimeStamp = extraPolateRelativeTimeStamp(curtick);
180 double corrTime = relTimeStamp + this->FirstTimeStamp();
181 sec = (uint32_t)corrTime;
182 double frac = corrTime - sec;
183 nanoSec = (uint32_t)(1E9 * frac);
189 double dTAbs = fabs(relTimeStamp1 - relTimeStamp2);
190 if (dTAbs < AllowedTimeDeviation())
203 if (numberValInFifo < fifoSize)
207 std::vector<uint64_t> tickFifoUnwrap;
208 std::vector<double> clockFifoUnwrap;
209 clockFifoUnwrap.resize(fifoSize);
210 tickFifoUnwrap.resize(fifoSize);
211 uint64_t tickOffset = 0;
212 clockFifoUnwrap[0] = 0.00;
213 tickFifoUnwrap[0] = 0;
214 FirstTimeStamp(this->clockFifo[0]);
215 FirstTick(this->tickFifo[0]);
217 uint64_t tickDivisor = 0x100000000;
221 for (
int i = 1; i < fifoSize; i++)
223 if (tickFifo[i] < tickFifo[i - 1])
225 tickOffset += tickDivisor;
227 tickFifoUnwrap[i] = tickOffset + tickFifo[i] - FirstTick();
228 clockFifoUnwrap[i] = (this->clockFifo[i] - FirstTimeStamp());
235 for (
int i = 0; i < fifoSize; i++)
237 sum_xy += tickFifoUnwrap[i] * clockFifoUnwrap[i];
238 sum_x += tickFifoUnwrap[i];
239 sum_y += clockFifoUnwrap[i];
240 sum_xx += tickFifoUnwrap[i] * tickFifoUnwrap[i];
244 double m = (fifoSize * sum_xy - sum_x * sum_y) / (fifoSize * sum_xx - sum_x*sum_x);
247 for (
int i = 0; i < fifoSize; i++)
249 double yesti = m * tickFifoUnwrap[i];
250 if (this->nearSameTimeStamp(yesti, clockFifoUnwrap[i]))
257 if (matchCnt == fifoSize)
259 InterpolationSlope(m);
267 bool SoftwarePLL::getDemoFileData(std::string fileName, std::vector<uint32_t>& tickVec,std::vector<uint32_t>& secVec, std::vector<uint32_t>& nanoSecVec )
269 std::ifstream file(fileName);
276 while (file >> row) {
279 uint32_t tickVal = (uint32_t)std::stoi(row[0]);
280 uint32_t secVal = (uint32_t)std::stoi(row[1]);
281 uint32_t nanoSecVal = (uint32_t)std::stoi(row[2]);
282 tickVec.push_back(tickVal);
283 secVec.push_back(secVal);
284 nanoSecVec.push_back(nanoSecVal);
298 std::cout <<
"Running testbed for SofwarePLL" << std::endl;
299 uint32_t curtick = 0;
304 uint32_t nanoSec = 0;
305 double tickPerSec = 1E6;
306 uint32_t tickInc = 1000;
309 std::vector<uint32_t> tickVec;
310 std::vector<uint32_t> secVec;
311 std::vector<uint32_t> nanoSecVec;
314 bool testWithDataFile =
true;
315 if (testWithDataFile)
318 maxLoop = tickVec.size();
321 for (
int i = 0; i < maxLoop; i++)
323 if (testWithDataFile)
325 curtick = tickVec[i];
327 nanoSec = nanoSecVec[i];
333 double deltaT = tickInc / tickPerSec;
334 nanoSec += (int)(deltaT * 1E9);
345 printf(
"Before correction: %3d.%09d\n", sec, nanoSec);
346 uint32_t org_sec = sec;
347 uint32_t org_nanoSec = nanoSec;
351 bool corrected =
false;
352 if ((nanoSec != org_nanoSec) || (sec != org_sec))
356 printf(
"After correction : %3d.%09d %s %s\n", sec, nanoSec, bRet ?
"OK " :
"DISMISS", corrected ?
"MODI." :
"OK ");
364 #ifdef softwarePLL_MAINTEST 365 int main(
int argc,
char **argv)
367 printf(
"Test for softwarePLL-Class\n");
bool getDemoFileData(std::string fileName, std::vector< uint32_t > &tickVec, std::vector< uint32_t > &secVec, std::vector< uint32_t > &nanoSecVec)
void readNextRow(std::istream &str)
double extraPolateRelativeTimeStamp(uint32_t tick)
std::string const & operator[](std::size_t index) const
bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick)
bool pushIntoFifo(double curTimeStamp, uint32_t curtick)
int main(int argc, char **argv)
Startup routine - if called with no argmuments we assume debug session. Set scanner name variable by ...
std::istream & operator>>(std::istream &str, CSVRow &data)
static const uint32_t MaxExtrapolationCounter
static const double MaxAllowedTimeDeviation
bool nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2)
int findDiffInFifo(double diff, double tol)
bool updateInterpolationSlope()
std::vector< std::string > m_data
bool updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
Updates PLL internale State should be called only with network send timestamps.