diff --git a/lib/hooks/CMakeLists.txt b/lib/hooks/CMakeLists.txt index ddd867516..17a4f6634 100644 --- a/lib/hooks/CMakeLists.txt +++ b/lib/hooks/CMakeLists.txt @@ -24,7 +24,7 @@ set(HOOK_SRC average.cpp cast.cpp decimate.cpp - dp.cpp + #dp.cpp drop.cpp dump.cpp ebm.cpp @@ -32,7 +32,6 @@ set(HOOK_SRC gate.cpp jitter_calc.cpp limit_rate.cpp - print.cpp restart.cpp scale.cpp shift_seq.cpp @@ -40,6 +39,8 @@ set(HOOK_SRC skip_first.cpp stats.cpp ts.cpp + pps_ts.cpp + print.cpp ) add_library(hooks STATIC ${HOOK_SRC}) diff --git a/lib/hooks/pps_ts.cpp b/lib/hooks/pps_ts.cpp new file mode 100644 index 000000000..fa5ea693c --- /dev/null +++ b/lib/hooks/pps_ts.cpp @@ -0,0 +1,142 @@ +/** Timestamp hook. + * + * @author Steffen Vogel + * @copyright 2014-2019, Institute for Automation of Complex Power Systems, EONERC + * @license GNU General Public License (version 3) + * + * VILLASnode + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + *********************************************************************************/ + +/** @addtogroup hooks Hook functions + * @{ + */ + +#include +#include +#include + +#include + +namespace villas { +namespace node { + +class PpsTsHook : public Hook { + +protected: + double lastValue; + double thresh; + double realSmpRate; + unsigned idx; + int lastSeqNr; + unsigned edgeCounter; + + timespec realTime; + +public: + PpsTsHook(struct path *p, struct node *n, int fl, int prio, bool en = true) : + Hook(p, n, fl, prio, en), + lastValue(0), + thresh(1.5), + realSmpRate(0), + idx(0), + lastSeqNr(0), + edgeCounter(0), + realTime({ 0, 0 }) + { + } + + virtual void parse(json_t *cfg) + { + int ret; + json_error_t err; + + assert(state != STATE_STARTED); + + ret = json_unpack_ex(cfg, &err, 0, "{ s: i, s?: f }", + "signal_index", &idx, + "threshold", &thresh + ); + if (ret) + throw ConfigError(cfg, err, "node-config-hook-pps_ts"); + + info("parsed config thresh=%f signal_index=%d", thresh, idx); + + state = STATE_PARSED; + } + + virtual int process(sample *smp) + { + assert(state == STATE_STARTED); + + /* Get value of PPS signal */ + float value = smp->data[idx].f; // TODO check if it is really float + int seqNr = smp->sequence; + + /* Detect Edge */ + bool isEdge = lastValue < thresh && value > thresh; + if (isEdge) { + if (edgeCounter >= 1) + realSmpRate = seqNr - lastSeqNr; + if (edgeCounter == 1) { + auto now = time_now(); + + if (now.tv_nsec >= 0.5e9) + realTime.tv_sec = now.tv_sec + 1; + else + realTime.tv_sec = now.tv_sec; + } + + lastSeqNr = seqNr; + + info("Edge detected: seq=%u, realTime.sec=%ld, realTime.nsec=%ld, smpRate=%f", seqNr, realTime.tv_sec, realTime.tv_nsec, realSmpRate); + + edgeCounter++; + } + + lastValue = value; + + if (edgeCounter < 2) + return HOOK_SKIP_SAMPLE; + else if (edgeCounter == 2 && isEdge) + realTime.tv_nsec = 0; + else + realTime.tv_nsec += 1e9 / realSmpRate; + + if (realTime.tv_nsec >= 1000000000) { + realTime.tv_sec++; + realTime.tv_nsec -= 1000000000; + } + + /* Update timestamp */ + smp->ts.origin = realTime; + smp->flags |= SAMPLE_HAS_TS_ORIGIN; + + return HOOK_OK; + } +}; + +/* Register hook */ +static HookPlugin p( + "pps_ts", + "Timestamp samples based GPS PPS signal", + HOOK_NODE_READ | HOOK_NODE_WRITE | HOOK_PATH, + 99 +); + +} /* namespace node */ +} /* namespace villas */ + +/** @} */