diff --git a/lib/hooks/dft.cpp b/lib/hooks/dft.cpp index 470f3b4fb..f64ae4dd7 100644 --- a/lib/hooks/dft.cpp +++ b/lib/hooks/dft.cpp @@ -28,12 +28,14 @@ #include #include #include +#include #include #include #include #include -#include +#include +#include #define DFT_MEM_DUMP @@ -43,41 +45,55 @@ namespace node { class DftHook : public Hook { protected: - enum PaddingType { + enum class PaddingType { ZERO, SIG_REPEAT }; - enum WindowType { + enum class 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; + + struct format_type *format; 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; - struct timespec dftRate; + unsigned dftRate; unsigned ppsIndex; unsigned windowSize; unsigned windowMultiplier; /**< Multiplyer for the window to achieve frequency resolution */ @@ -99,42 +115,51 @@ public: Hook(p, n, fl, prio, en), windowType(WindowType::NONE), paddingType(PaddingType::ZERO), + freqEstType(FreqEstimationType::NONE), smpMemory(), + ppsMemory(), dftMatrix(), dftResults(), filterWindowCoefficents(), absDftResults(), absDftFreqs(), - dftCalcCnt(0), + dftCalcCount(0), sampleRate(0), startFreqency(0), endFreqency(0), frequencyResolution(0), - dftRate({0, 0}), + dftRate(0), ppsIndex(0), windowSize(0), windowMultiplier(0), freqCount(0), + syncDft(0), + smpMemPos(0), + lastSequence(0), windowCorretionFactor(0), lastDftCal({0, 0}), signalIndex() { - bool debug = false; - if (debug) { + logger = logging.get("hook:dft"); + + format = format_type_lookup("villas.human"); + + if (logger->level() <= SPDLOG_LEVEL_DEBUG) { +#ifdef DFT_MEM_DUMP 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; @@ -142,10 +167,10 @@ public: struct signal *rocofSig; /* Add signals */ - 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); + 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); if (!freqSig || !amplSig || !phaseSig || !rocofSig) throw RuntimeError("Failed to create new signals"); @@ -155,28 +180,40 @@ 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); - dftResults.resize(freqCount); - filterWindowCoefficents.resize(windowSize); - absDftResults.resize(freqCount); - absDftFreqs.resize(freqCount); + /* 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); + } - for (unsigned i = 0; i < absDftFreqs.size(); i++) - absDftFreqs[i] = startFreqency + i * frequencyResolution; + filterWindowCoefficents.resize(windowSize); + + for (unsigned i = 0; i < freqCount; i++) { + absDftFreqs.emplace_back(startFreqency + i * frequencyResolution); + } generateDftMatrix(); calculateWindow(windowType); @@ -186,9 +223,8 @@ public: virtual void parse(json_t *cfg) { - const char *paddingTypeC = nullptr, *windowTypeC= nullptr; + const char *paddingTypeC = nullptr, *windowTypeC = nullptr, *freqEstimateTypeC = nullptr; int ret; - double dftRateIn = 0; json_error_t err; json_t *jsonChannelList = nullptr; @@ -197,22 +233,20 @@ public: Hook::parse(cfg); - 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}", + 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}", "sample_rate", &sampleRate, "start_freqency", &startFreqency, "end_freqency", &endFreqency, "frequency_resolution", &frequencyResolution, - "dft_rate", &dftRateIn, + "dft_rate", &dftRate, "window_size", &windowSize, "window_type", &windowTypeC, "padding_type", &paddingTypeC, + "freq_estimate_type", &freqEstimateTypeC, "sync", &syncDft, - "signal_index", &jsonChannelList + "signal_index", &jsonChannelList, + "pps_index", &ppsIndex ); - - dftRate.tv_sec = (int) (1 / dftRateIn); - dftRate.tv_nsec = fmod( 1 / dftRateIn, 1) * 1e9; - if (ret) throw ConfigError(cfg, err, "node-config-hook-dft"); @@ -270,6 +304,13 @@ 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); @@ -286,6 +327,10 @@ 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; @@ -315,59 +360,58 @@ public: 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->writeDataBinary(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); - if (dftCalcCnt > 1) { - if (phasorFreq) - phasorFreq->writeData(1, &maxF); + maxF = estimate.x; + maxA = estimate.y; + } + } - smp->data[i * 4].f = maxF; /* Frequency */ + if (windowSize < smpMemPos) { + + smp->data[i * 4 + 0].f = maxF; /* Frequency */ smp->data[i * 4 + 1].f = (maxA / pow(2, 0.5)); /* Amplitude */ - smp->data[i * 4 + 2].f = atan2(dftResults[maxPos].imag(), dftResults[maxPos].real()); /* Phase */ + smp->data[i * 4 + 2].f = atan2(dftResults[i][maxPos].imag(), dftResults[i][maxPos].real()); /* Phase */ smp->data[i * 4 + 3].f = 0; /* RoCof */ - if (phasorPhase) - phasorPhase->writeData(1, &(smp->data[i * 4 + 2].f)); } } @@ -388,17 +432,20 @@ 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) + if(runDft && windowSize < smpMemPos) return Reason::OK; return Reason::SKIP_SAMPLE; } + /** + * This function generates the furie coeffients for the calculateDft function + */ void generateDftMatrix() { using namespace std::complex_literals; @@ -412,7 +459,11 @@ public: } } - void calculateDft(enum PaddingType padding, std::vector &ringBuffer, unsigned ringBufferPos) + + /** + * This function calculates the discrete furie transform of the input signal + */ + void calculateDft(enum PaddingType padding, std::vector &ringBuffer, std::vector> &results, unsigned ringBufferPos) { /* RingBuffer size needs to be equal to windowSize * prepare sample window The following parts can be combined */ @@ -423,8 +474,9 @@ public: #ifdef DFT_MEM_DUMP - //if (origSigSync) - // origSigSync->writeDataBinary(windowSize, tmpSmpWindow); + if (origSigSync) + origSigSync->writeDataBinary(windowSize, tmpSmpWindow); + #endif for (unsigned i = 0; i < windowSize; i++) @@ -432,33 +484,37 @@ public: #ifdef DFT_MEM_DUMP - //if (windowdSigSync) - // windowdSigSync->writeDataBinary(windowSize, tmpSmpWindow); + if (windowdSigSync) + windowdSigSync->writeDataBinary(windowSize, tmpSmpWindow); #endif for (unsigned i = 0; i < freqCount; i++) { - dftResults[i] = 0; + results[i] = 0; + for (unsigned j = 0; j < windowSize * windowMultiplier; j++) { if (padding == PaddingType::ZERO) { if (j < (windowSize)) - dftResults[i] += tmpSmpWindow[j] * dftMatrix[i][j]; + results[i] += tmpSmpWindow[j] * dftMatrix[i][j]; else - dftResults[i] += 0; + results[i] += 0; } else if (padding == PaddingType::SIG_REPEAT) /* Repeat samples */ - dftResults[i] += tmpSmpWindow[j % windowSize] * dftMatrix[i][j]; + results[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)); @@ -490,6 +546,30 @@ 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 */