/* Timestamp hook. * * Author: Steffen Vogel * SPDX-FileCopyrightText: 2014-2023 Institute for Automation of Complex Power Systems, RWTH Aachen University * SPDX-License-Identifier: Apache-2.0 */ #include #include #include #include #include namespace villas { namespace node { class PpsTsHook : public SingleSignalHook { protected: enum Mode { SIMPLE, HORIZON, } mode; uint64_t lastSequence; double lastValue; double threshold; bool isSynced; bool isLocked; struct timespec tsVirt; double timeError; // In seconds double periodEstimate; // In seconds double periodErrorCompensation; // In seconds double period; // In seconds uintmax_t cntEdges; uintmax_t cntSmps; uintmax_t cntSmpsTotal; unsigned horizonCompensation; unsigned horizonEstimation; unsigned currentSecond; std::vector filterWindow; public: PpsTsHook(Path *p, Node *n, int fl, int prio, bool en = true) : SingleSignalHook(p, n, fl, prio, en), mode(Mode::SIMPLE), lastSequence(0), lastValue(0), threshold(1.5), isSynced(false), isLocked(false), timeError(0.0), periodEstimate(0.0), periodErrorCompensation(0.0), period(0.0), cntEdges(0), cntSmps(0), cntSmpsTotal(0), horizonCompensation(10), horizonEstimation(10), currentSecond(0), filterWindow(horizonEstimation + 1, 0) {} virtual void parse(json_t *json) { int ret; json_error_t err; assert(state != State::STARTED); SingleSignalHook::parse(json); const char *mode_str = nullptr; double fSmps = 1.0; ret = json_unpack_ex(json, &err, 0, "{ s?: s, s?: f, s?: F, s?: i, s?: i }", "mode", &mode_str, "threshold", &threshold, "expected_smp_rate", &fSmps, "horizon_estimation", &horizonEstimation, "horizon_compensation", &horizonCompensation); if (ret) throw ConfigError(json, err, "node-config-hook-pps_ts"); period = 1.0 / fSmps; currentSecond = time(nullptr); if (mode_str) { if (!strcmp(mode_str, "simple")) mode = Mode::SIMPLE; else if (!strcmp(mode_str, "horizon")) mode = Mode::HORIZON; else throw ConfigError(json, "node-config-hook-pps_ts-mode", "Unsupported mode: {}", mode_str); } state = State::PARSED; } virtual villas::node::Hook::Reason process(struct Sample *smp) { switch (mode) { case Mode::SIMPLE: return processSimple(smp); case Mode::HORIZON: return processHorizon(smp); default: return Reason::ERROR; } } villas::node::Hook::Reason processSimple(struct Sample *smp) { assert(state == State::STARTED); // Get value of PPS signal float value = smp->data[signalIndex].f; // TODO check if it is really float // Detect Edge bool isEdge = lastValue < threshold && value > threshold; if (isEdge) { tsVirt.tv_sec = currentSecond + 1; tsVirt.tv_nsec = 0; period = 1.0 / cntSmps; cntSmps = 0; cntEdges++; currentSecond = 0; } else { struct timespec tsPeriod = time_from_double(period); tsVirt = time_add(&tsVirt, &tsPeriod); } lastValue = value; cntSmps++; if (!currentSecond && tsVirt.tv_nsec > 0.5e9) //take the second somewere in the center of the last second to reduce impact of system clock error currentSecond = time(nullptr); if (cntEdges < 5) return Hook::Reason::SKIP_SAMPLE; smp->ts.origin = tsVirt; smp->flags |= (int)SampleFlags::HAS_TS_ORIGIN; if ((smp->sequence - lastSequence) > 1) logger->warn("Samples missed: {} sampled missed", smp->sequence - lastSequence); lastSequence = smp->sequence; return Hook::Reason::OK; } villas::node::Hook::Reason processHorizon(struct Sample *smp) { assert(state == State::STARTED); // Get value of PPS signal float value = smp->data[signalIndex].f; // TODO check if it is really float // Detect Edge bool isEdge = lastValue < threshold && value > threshold; lastValue = value; if (isEdge) { if (isSynced) { if (tsVirt.tv_nsec > 0.5e9) timeError += 1.0 - (tsVirt.tv_nsec / 1.0e9); else timeError -= (tsVirt.tv_nsec / 1.0e9); filterWindow[cntEdges % filterWindow.size()] = cntSmpsTotal; // Estimated sample period over last 'horizonEstimation' seconds unsigned int tmp = cntEdges < filterWindow.size() ? cntEdges : horizonEstimation; double cntSmpsAvg = (cntSmpsTotal - filterWindow[(cntEdges - tmp) % filterWindow.size()]) / tmp; periodEstimate = 1.0 / cntSmpsAvg; periodErrorCompensation = timeError / (cntSmpsAvg * horizonCompensation); period = periodEstimate + periodErrorCompensation; } else { tsVirt.tv_sec = time(nullptr); tsVirt.tv_nsec = 0; isSynced = true; cntEdges = 0; cntSmpsTotal = 0; } cntSmps = 0; cntEdges++; logger->debug( "Time Error is: {} periodEstimate {} periodErrorCompensation {}", timeError, periodEstimate, periodErrorCompensation); } cntSmps++; cntSmpsTotal++; if (cntEdges < 5) return Hook::Reason::SKIP_SAMPLE; smp->ts.origin = tsVirt; smp->flags |= (int)SampleFlags::HAS_TS_ORIGIN; struct timespec tsPeriod = time_from_double(period); tsVirt = time_add(&tsVirt, &tsPeriod); if ((smp->sequence - lastSequence) > 1) logger->warn("Samples missed: {} sampled missed", smp->sequence - lastSequence); lastSequence = smp->sequence; return Hook::Reason::OK; } }; // Register hook static char n[] = "pps_ts"; static char d[] = "Timestamp samples based GPS PPS signal"; static HookPlugin p; } // namespace node } // namespace villas