diff --git a/lib/hooks/dft.cpp b/lib/hooks/dft.cpp index be464cf63..470f3b4fb 100644 --- a/lib/hooks/dft.cpp +++ b/lib/hooks/dft.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include @@ -36,59 +35,49 @@ #include #include +#define DFT_MEM_DUMP + namespace villas { namespace node { class DftHook : public Hook { protected: - enum class PaddingType { + enum PaddingType { ZERO, SIG_REPEAT }; - enum class WindowType { + enum WindowType { NONE, FLATTOP, HANN, HAMMING }; - enum class FreqEstimationType { - NONE, - QUADRATIC - }; - - struct Point { - double x; - double y; - }; - std::shared_ptr origSigSync; std::shared_ptr windowdSigSync; std::shared_ptr phasorPhase; std::shared_ptr phasorAmplitude; std::shared_ptr phasorFreq; - std::shared_ptr ppsSigSync; enum WindowType windowType; enum PaddingType paddingType; - enum FreqEstimationType freqEstType; std::vector> smpMemory; - std::vector ppsMemory;//this is just temporary for debugging std::vector>> dftMatrix; - std::vector>> dftResults; + std::vector> dftResults; std::vector filterWindowCoefficents; - std::vector> absDftResults; + std::vector absDftResults; std::vector absDftFreqs; + uint64_t dftCalcCount; unsigned sampleRate; double startFreqency; double endFreqency; double frequencyResolution; - unsigned dftRate; + struct timespec dftRate; unsigned ppsIndex; unsigned windowSize; unsigned windowMultiplier; /**< Multiplyer for the window to achieve frequency resolution */ @@ -110,51 +99,42 @@ public: Hook(p, n, fl, prio, en), windowType(WindowType::NONE), paddingType(PaddingType::ZERO), - freqEstType(FreqEstimationType::NONE), smpMemory(), - ppsMemory(), dftMatrix(), dftResults(), filterWindowCoefficents(), absDftResults(), absDftFreqs(), - dftCalcCount(0), + dftCalcCnt(0), sampleRate(0), startFreqency(0), endFreqency(0), frequencyResolution(0), - dftRate(0), + dftRate({0, 0}), ppsIndex(0), windowSize(0), windowMultiplier(0), freqCount(0), - syncDft(0), - smpMemPos(0), - lastSequence(0), windowCorretionFactor(0), lastDftCal({0, 0}), signalIndex() { - logger = logging.get("hook:dft"); - - format = format_type_lookup("villas.human"); - - if (logger->level() <= SPDLOG_LEVEL_DEBUG) { -#ifdef DFT_MEM_DUMP + bool debug = false; + if (debug) { origSigSync = std::make_shared("/tmp/plot/origSigSync"); windowdSigSync = std::make_shared("/tmp/plot/windowdSigSync"); - ppsSigSync = std::make_shared("/tmp/plot/ppsSigSync"); -#endif phasorPhase = std::make_shared("/tmp/plot/phasorPhase"); phasorAmplitude = std::make_shared("/tmp/plot/phasorAmplitude"); phasorFreq = std::make_shared("/tmp/plot/phasorFreq"); - } } virtual void prepare() { signal_list_clear(&signals); + + /* Initialize sample memory */ + smpMemory.clear(); for (unsigned i = 0; i < signalIndex.size(); i++) { struct signal *freqSig; struct signal *amplSig; @@ -162,10 +142,10 @@ public: struct signal *rocofSig; /* Add signals */ - freqSig = signal_create("amplitude", "V", SignalType::FLOAT); - amplSig = signal_create("phase", "rad", SignalType::FLOAT); - phaseSig = signal_create("frequency", "Hz", SignalType::FLOAT); - rocofSig = signal_create("rocof", "Hz/s", SignalType::FLOAT); + freqSig = signal_create("amplitude", nullptr, SignalType::FLOAT); + amplSig = signal_create("phase", nullptr, SignalType::FLOAT); + phaseSig = signal_create("frequency", nullptr, SignalType::FLOAT); + rocofSig = signal_create("rocof", nullptr, SignalType::FLOAT); if (!freqSig || !amplSig || !phaseSig || !rocofSig) throw RuntimeError("Failed to create new signals"); @@ -175,40 +155,28 @@ public: vlist_push(&signals, phaseSig); vlist_push(&signals, rocofSig); - + smpMemory.emplace_back(windowSize, 0.0); } - /* Initialize sample memory */ - smpMemory.clear(); - for (unsigned i = 0; i < signalIndex.size(); i++) - smpMemory.emplace_back(windowSize, 0.0); - - /* Initialize temporary ppsMemory */ - ppsMemory.clear(); - ppsMemory.resize(windowSize, 0.0); - /* Calculate how much zero padding ist needed for a needed resolution */ - windowMultiplier = ceil(((double) sampleRate / windowSize) / frequencyResolution); + windowMultiplier = ceil(((double)sampleRate / windowSize) / frequencyResolution); freqCount = ceil((endFreqency - startFreqency) / frequencyResolution) + 1; + logger->debug("FreqCount : {}", freqCount); + /* Initialize matrix of dft coeffients */ dftMatrix.clear(); for (unsigned i = 0; i < freqCount; i++) dftMatrix.emplace_back(windowSize * windowMultiplier, 0.0); - /* Initalize dft results matrix */ - dftResults.clear(); - for (unsigned i = 0; i < signalIndex.size(); i++){ - dftResults.emplace_back(freqCount, 0.0); - absDftResults.emplace_back(freqCount, 0.0); - } - + dftResults.resize(freqCount); filterWindowCoefficents.resize(windowSize); + absDftResults.resize(freqCount); + absDftFreqs.resize(freqCount); - for (unsigned i = 0; i < freqCount; i++) { - absDftFreqs.emplace_back(startFreqency + i * frequencyResolution); - } + for (unsigned i = 0; i < absDftFreqs.size(); i++) + absDftFreqs[i] = startFreqency + i * frequencyResolution; generateDftMatrix(); calculateWindow(windowType); @@ -218,8 +186,9 @@ public: virtual void parse(json_t *cfg) { - const char *paddingTypeC = nullptr, *windowTypeC = nullptr, *freqEstimateTypeC = nullptr; + const char *paddingTypeC = nullptr, *windowTypeC= nullptr; int ret; + double dftRateIn = 0; json_error_t err; json_t *jsonChannelList = nullptr; @@ -228,20 +197,22 @@ public: Hook::parse(cfg); - ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: F, s?: F, s?: F, s?: i , s?: i, s?: s, s?: s, s?: s, s?: b, s?: o}", + ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: F, s?: F, s?: F, s?: F , s?: i, s?: s, s?: s, s?: s, s?: b, s?: o}", "sample_rate", &sampleRate, "start_freqency", &startFreqency, "end_freqency", &endFreqency, "frequency_resolution", &frequencyResolution, - "dft_rate", &dftRate, + "dft_rate", &dftRateIn, "window_size", &windowSize, "window_type", &windowTypeC, "padding_type", &paddingTypeC, - "freq_estimate_type", &freqEstimateTypeC, "sync", &syncDft, - "signal_index", &jsonChannelList, - "pps_index", &ppsIndex + "signal_index", &jsonChannelList ); + + dftRate.tv_sec = (int) (1 / dftRateIn); + dftRate.tv_nsec = fmod( 1 / dftRateIn, 1) * 1e9; + if (ret) throw ConfigError(cfg, err, "node-config-hook-dft"); @@ -299,13 +270,6 @@ public: paddingType = PaddingType::ZERO; } - if (!freqEstimateTypeC) { - logger->info("No Frequency estimation type given, assume no none"); - freqEstType = FreqEstimationType::NONE; - } - else if (strcmp(freqEstimateTypeC, "quadratic") == 0) - freqEstType = FreqEstimationType::QUADRATIC; - if (endFreqency < 0 || endFreqency > sampleRate) throw ConfigError(cfg, err, "node-config-hook-dft", "End frequency must be smaller than sampleRate {}", sampleRate); @@ -322,88 +286,101 @@ public: for (unsigned i = 0; i < signalIndex.size(); i++) smpMemory[i][smpMemPos % windowSize] = smp->data[signalIndex[i]].f; - // Debugging for pps signal this should only be temporary - if (ppsSigSync) - ppsMemory[smpMemPos % windowSize] = smp->data[ppsIndex].f; - smpMemPos++; bool runDft = false; if (syncDft) { - struct timespec timeDiff = time_diff(&lastDftCal, &smp->ts.origin); - if ((timeDiff.tv_sec*1e9+timeDiff.tv_nsec) > (1e9/dftRate)) + //struct timespec timeDiff = time_diff(&lastDftCal, &smp->ts.origin); + + //double nextCalc = (lastDftCal.tv_sec + dftRate.tv_sec) * 1e9 + lastDftCal.tv_nsec + dftRate.tv_nsec; + + //double dftRateNSec = dftRate.tv_sec * 1e9 + dftRate.tv_nsec; + //double smpNsec = smp->ts.origin.tv_sec * 1e9 + smp->ts.origin.tv_nsec; + + + + + //if ( nextCalc < (smp->ts.origin.tv_sec * 1e9 + smp->ts.origin.tv_nsec) && fmod( smpNsec, dftRateNSec ) < (110*1e9/(sampleRate))) + // runDft = true; + + if (smp->ts.origin.tv_sec != lastDftCal.tv_sec) runDft = true; + + //if ((fmod(smp->ts.origin.tv_sec*1e9 + smp->ts.origin.tv_nsec, 1e9/dftRate) < (1e9/(dftRate+1))) && ((timeDiff.tv_sec*1e9+timeDiff.tv_nsec) > (1e9/dftRate))) + //if ((timeDiff.tv_sec*1e9+timeDiff.tv_nsec) > (1e9/dftRate) && ) + // runDft = true; } if (runDft) { lastDftCal = smp->ts.origin; // Debugging for pps signal this should only be temporary - if (ppsSigSync) { - double tmpPPSWindow[windowSize]; + //if (ppsSigSync) { + //double tmpPPSWindow[windowSize]; - for (unsigned i = 0; i< windowSize; i++) - tmpPPSWindow[i] = ppsMemory[(i + smpMemPos) % windowSize]; + //for (unsigned i = 0; i< windowSize; i++) + // tmpPPSWindow[i] = ppsMemory[(i + smpMemPos) % windowSize]; - ppsSigSync->writeData(windowSize, tmpPPSWindow); - } + //ppsSigSync->writeDataBinary(windowSize, tmpPPSWindow); + //} #pragma omp parallel for for (unsigned i = 0; i < signalIndex.size(); i++) { calculateDft(PaddingType::ZERO, smpMemory[i], dftResults[i], smpMemPos); + + + double maxF = 0; double maxA = 0; unsigned maxPos = 0; + double tmpImag[freqCount], tmpReal[freqCount], absVal[freqCount]; + for (unsigned j = 0; j < freqCount; j++) { int multiplier = paddingType == PaddingType::ZERO ? 1 : windowMultiplier; absDftResults[i][j] = abs(dftResults[i][j]) * 2 / (windowSize * windowCorretionFactor * multiplier); + absVal[j] = absDftResults[i][j]; if (maxA < absDftResults[i][j]) { maxF = absDftFreqs[j]; maxA = absDftResults[i][j]; maxPos = j; } + tmpImag[j] = dftResults[i][maxPos].imag(); + tmpReal[j] = dftResults[i][maxPos].real(); } + windowdSigSync->writeDataBinary(freqCount, tmpImag); + origSigSync -> writeDataBinary(freqCount, absVal); + ppsSigSync->writeDataBinary(freqCount, tmpReal); - if (freqEstType == FreqEstimationType::QUADRATIC) { - if (maxPos < 1 || maxPos >= freqCount - 1) - logger->warn("Maximum frequency bin lies on window boundary. Using non-estimated results!"); - else { - Point a = { absDftFreqs[maxPos - 1], absDftResults[i][maxPos - 1] }; - Point b = { absDftFreqs[maxPos + 0], absDftResults[i][maxPos + 0] }; - Point c = { absDftFreqs[maxPos + 1], absDftResults[i][maxPos + 1] }; - - Point estimate = quadraticEstimation(a, b, c, maxPos); - maxF = estimate.x; - maxA = estimate.y; - } - } + if (dftCalcCnt > 1) { + if (phasorFreq) + phasorFreq->writeData(1, &maxF); - if (windowSize < smpMemPos) { - - smp->data[i * 4 + 0].f = maxF; /* Frequency */ + smp->data[i * 4].f = maxF; /* Frequency */ smp->data[i * 4 + 1].f = (maxA / pow(2, 0.5)); /* Amplitude */ - smp->data[i * 4 + 2].f = atan2(dftResults[i][maxPos].imag(), dftResults[i][maxPos].real()); /* Phase */ + smp->data[i * 4 + 2].f = atan2(dftResults[maxPos].imag(), dftResults[maxPos].real()); /* Phase */ smp->data[i * 4 + 3].f = 0; /* RoCof */ + if (phasorPhase) + phasorPhase->writeData(1, &(smp->data[i * 4 + 2].f)); } } //the following is a debug output and currently only for channel 0 if (windowSize < smpMemPos){ if (phasorFreq) - phasorFreq->writeData(1, &(smp->data[0 * 4 + 0].f)); + phasorFreq->writeDataBinary(1, &(smp->data[0 * 4 + 0].f)); if (phasorPhase) - phasorPhase->writeData(1, &(smp->data[0 * 4 + 2].f)); + phasorPhase->writeDataBinary(1, &(smp->data[0 * 4 + 2].f)); if (phasorAmplitude) - phasorAmplitude->writeData(1, &(smp->data[0 * 4 + 1].f)); + phasorAmplitude->writeDataBinary(1, &(smp->data[0 * 4 + 1].f)); } smp->length = windowSize < smpMemPos ? signalIndex.size() * 4 : 0; @@ -411,20 +388,17 @@ public: dftCalcCount++; } - if (smp->sequence - lastSequence > 1) + if ((smp->sequence - lastSequence) > 1) logger->warn("Calculation is not Realtime. {} sampled missed", smp->sequence - lastSequence); lastSequence = smp->sequence; - if(runDft && windowSize < smpMemPos) + if (runDft) return Reason::OK; return Reason::SKIP_SAMPLE; } - /** - * This function generates the furie coeffients for the calculateDft function - */ void generateDftMatrix() { using namespace std::complex_literals; @@ -438,11 +412,7 @@ public: } } - - /** - * This function calculates the discrete furie transform of the input signal - */ - void calculateDft(enum PaddingType padding, std::vector &ringBuffer, std::vector> &results, unsigned ringBufferPos) + void calculateDft(enum PaddingType padding, std::vector &ringBuffer, unsigned ringBufferPos) { /* RingBuffer size needs to be equal to windowSize * prepare sample window The following parts can be combined */ @@ -453,12 +423,8 @@ public: #ifdef DFT_MEM_DUMP - if (origSigSync) - origSigSync->writeData(windowSize, tmpSmpWindow); - - if (dftCalcCount > 1 && phasorAmplitude) - phasorAmplitude->writeData(1, &tmpSmpWindow[windowSize - 1]); - + //if (origSigSync) + // origSigSync->writeDataBinary(windowSize, tmpSmpWindow); #endif for (unsigned i = 0; i < windowSize; i++) @@ -466,37 +432,33 @@ public: #ifdef DFT_MEM_DUMP - if (windowdSigSync) - windowdSigSync->writeData(windowSize, tmpSmpWindow); + //if (windowdSigSync) + // windowdSigSync->writeDataBinary(windowSize, tmpSmpWindow); #endif for (unsigned i = 0; i < freqCount; i++) { - results[i] = 0; - + dftResults[i] = 0; for (unsigned j = 0; j < windowSize * windowMultiplier; j++) { if (padding == PaddingType::ZERO) { if (j < (windowSize)) - results[i] += tmpSmpWindow[j] * dftMatrix[i][j]; + dftResults[i] += tmpSmpWindow[j] * dftMatrix[i][j]; else - results[i] += 0; + dftResults[i] += 0; } else if (padding == PaddingType::SIG_REPEAT) /* Repeat samples */ - results[i] += tmpSmpWindow[j % windowSize] * dftMatrix[i][j]; + dftResults[i] += tmpSmpWindow[j % windowSize] * dftMatrix[i][j]; } } } - /** - * This function prepares the selected window coefficents - */ void calculateWindow(enum WindowType windowTypeIn) { switch (windowTypeIn) { case WindowType::FLATTOP: for (unsigned i = 0; i < windowSize; i++) { - filterWindowCoefficents[i] = 0.21557895 - - 0.41663158 * cos(2 * M_PI * i / (windowSize)) + filterWindowCoefficents[i] = 0.21557895 + - 0.41663158 * cos(2 * M_PI * i / (windowSize)) + 0.277263158 * cos(4 * M_PI * i / (windowSize)) - 0.083578947 * cos(6 * M_PI * i / (windowSize)) + 0.006947368 * cos(8 * M_PI * i / (windowSize)); @@ -528,30 +490,6 @@ public: windowCorretionFactor /= windowSize; } - - /** - * This function is calculating the mximum based on a quadratic interpolation - * - * This function is based on the following paper: - * https://mgasior.web.cern.ch/pap/biw2004.pdf - * https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/ - * * - * In particular equation 10 - */ - Point quadraticEstimation(const Point &a, const Point &b, const Point &c, unsigned maxFBin) - { - // Frequency estimation - double maxBinEst = (double) maxFBin + (c.y - a.y) / (2 * (2 * b.y - a.y - c.y)); - double y_Fmax = startFreqency + maxBinEst * frequencyResolution; // convert bin to frequency - - // Amplitude estimation - double f = (a.x * (b.y - c.y) + b.x * (c.y - a.y) + c.x * (a.y - b.y)) / ((a.x - b.x) * (a.x - c.x) * (c.x - b.x)); - double g = (pow(a.x, 2) * (b.y - c.y) + pow(b.x, 2) * (c.y - a.y) + pow(c.x, 2) * (a.y - b.y)) / ((a.x - b.x) * (a.x - c.x) * (b.x - c.x)); - double h = (pow(a.x, 2) * (b.x * c.y - c.x * b.y) + a.x * (pow(c.x, 2) * b.y - pow(b.x,2) * c.y)+ b.x * c.x * a.y * (b.x - c.x)) / ((a.x - b.x) * (a.x - c.x) * (b.x - c.x)); - double i = f * pow(y_Fmax,2) + g * y_Fmax + h; - - return { y_Fmax, i }; - } }; /* Register hook */