diff --git a/lib/hooks/pps_ts.cpp b/lib/hooks/pps_ts.cpp index 4593ec53b..bfbd2bec2 100644 --- a/lib/hooks/pps_ts.cpp +++ b/lib/hooks/pps_ts.cpp @@ -38,26 +38,28 @@ class PpsTsHook : public Hook { protected: double lastValue; double thresh; - double realSmpRate; + double periodTime;//Period of sample frequency (Villas samples) unsigned idx; uint64_t lastSeqNr; unsigned edgeCounter; double pll_gain; timespec realTime; uint64_t last_sequence; + double y; public: PpsTsHook(struct vpath *p, struct vnode *n, int fl, int prio, bool en = true) : Hook(p, n, fl, prio, en), lastValue(0), thresh(1.5), - realSmpRate(0), + periodTime(1), idx(0), lastSeqNr(0), edgeCounter(0), pll_gain(1), realTime({ 0, 0 }), - last_sequence(0) + last_sequence(0), + y(0) { } @@ -99,7 +101,16 @@ public: double timeErr; double changeVal; - static const double targetVal = 0.5e9; + static const double targetVal = 0.5; + + + realTime.tv_nsec += periodTime * 1e9 / 10000; + + if (realTime.tv_nsec >= 1e9) { + realTime.tv_sec++; + realTime.tv_nsec -= 1e9; + } + if (isEdge) { if (edgeCounter == 2) { @@ -109,48 +120,53 @@ public: realTime.tv_sec = now.tv_sec + 1; else realTime.tv_sec = now.tv_sec; - realTime.tv_nsec = targetVal; + realTime.tv_nsec = targetVal * 1e9; } lastSeqNr = seqNr; edgeCounter++; - timeErr = ( targetVal - realTime.tv_nsec); + timeErr = ( targetVal - (realTime.tv_nsec / 1e9) ); if (edgeCounter >= 2){//pll mode - changeVal = pll_gain * -1 * timeErr; + changeVal = pll_gain * timeErr; + + double k2 = 1.; + double p = 0.99; + + y = p * timeErr + ( 1 - p) * y; + changeVal -= k2 * y; + //changeVal = pll_gain; /*if(timeErr > 0 ) - realSmpRate -= changeVal; + periodTime -= changeVal; else if(timeErr < 0){*/ - realSmpRate += changeVal; + periodTime += changeVal; //} - if(realSmpRate > 10100.)realSmpRate = 10100; - if(realSmpRate < 9900.)realSmpRate = 9900; + double nominalSmpRate = 1; + if(periodTime > 1.1/nominalSmpRate)periodTime = 1.1/nominalSmpRate; + if(periodTime < 0.9/nominalSmpRate)periodTime = 0.9/nominalSmpRate; } - //info("Edge detected: seq=%u, realTime.sec=%ld, realTime.nsec=%f, smpRate=%f, pll_gain=%f", seqNr, realTime.tv_sec, (1e9 - realTime.tv_nsec + 1e9/realSmpRate), realSmpRate, pll_gain); + //info("Edge detected: seq=%u, realTime.sec=%ld, realTime.nsec=%f, smpRate=%f, pll_gain=%f", seqNr, realTime.tv_sec, (1e9 - realTime.tv_nsec + 1e9/periodTime), periodTime, pll_gain); + } + + + + + + if(isEdge){ + info("Edge detected: seq=%lu, realTime.nsec=%lu, timeErr=%f , timePeriod=%f, changeVal=%f", seqNr,realTime.tv_nsec, timeErr, periodTime, changeVal); } if (edgeCounter < 2) return Hook::Reason::SKIP_SAMPLE; - else - realTime.tv_nsec += 1.e9 / realSmpRate; - - if (realTime.tv_nsec >= 1e9) { - realTime.tv_sec++; - realTime.tv_nsec -= 1e9; - } - - if(isEdge){ - info("Edge detected: seq=%lu, realTime.nsec=%ld, timeErr=%f , smpRate=%f, changeVal=%f", seqNr,realTime.tv_nsec, timeErr, realSmpRate, changeVal); - } /* Update timestamp */ smp->ts.origin = realTime; smp->flags |= (int) SampleFlags::HAS_TS_ORIGIN; if((smp->sequence - last_sequence) > 1 ) - warning("Calculation is not Realtime. %li sampled missed",smp->sequence - last_sequence); + warning("Samples missed: %li sampled missed", smp->sequence - last_sequence); last_sequence = smp->sequence;