diff --git a/lib/hooks/dft.cpp b/lib/hooks/dft.cpp index 4b72ea278..009a2bec8 100644 --- a/lib/hooks/dft.cpp +++ b/lib/hooks/dft.cpp @@ -204,10 +204,6 @@ public: ppsMemory.resize(windowSize, 0.0); #endif - //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); @@ -447,472 +443,6 @@ public: return Reason::SKIP_SAMPLE; } - /** - * This function generates the furie coeffients for the calculateDft function/** DFT hook. - * - * @author Manuel Pitz - * @copyright 2014-2020, 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 -#include - -#include -#include -#include -#include - -/* Uncomment to enable dumper of memory windows */ -//#define DFT_MEM_DUMP - -namespace villas { -namespace node { - -class DftHook : public Hook { - -protected: - enum class PaddingType { - ZERO, - SIG_REPEAT - }; - - enum class WindowType { - NONE, - FLATTOP, - HANN, - HAMMING - }; - - enum class FreqEstimationType { - NONE, - QUADRATIC - }; - - struct Point { - double x; - double y; - }; - - struct Phasor { - double frequency; - double amplitude; - double phase; - double rocof; /**< Rate of change of frequency. */ - }; - - enum WindowType windowType; - enum PaddingType paddingType; - enum FreqEstimationType freqEstType; - - std::vector> smpMemory; -#ifdef DFT_MEM_DUMP - std::vector ppsMemory; -#endif - std::vector>> matrix; - std::vector>> results; - std::vector filterWindowCoefficents; - std::vector> absResults; - std::vector absFrequencies; - - uint64_t calcCount; - unsigned sampleRate; - double startFrequency; - double endFreqency; - double frequencyResolution; - unsigned rate; - unsigned ppsIndex; - unsigned windowSize; - unsigned windowMultiplier; /**< Multiplyer for the window to achieve frequency resolution */ - unsigned freqCount; /**< Number of requency bins that are calculated */ - bool sync; - - uint64_t smpMemPos; - uint64_t lastSequence; - - std::complex omega; - - double windowCorrectionFactor; - struct timespec lastCalc; - double nextCalc; - - std::vector signalIndex; /**< A list of signalIndex to do dft on */ - Phasor lastResult; - - std::string dumperPrefix; - bool dumperEnable; -#ifdef DFT_MEM_DUMP - Dumper origSigSync; - Dumper windowdSigSync; - Dumper ppsSigSync; -#endif - Dumper phasorRocof; - Dumper phasorPhase; - Dumper phasorAmplitude; - Dumper phasorFreq; - -public: - DftHook(struct vpath *p, struct vnode *n, int fl, int prio, bool en = true) : - Hook(p, n, fl, prio, en), - windowType(WindowType::NONE), - paddingType(PaddingType::ZERO), - freqEstType(FreqEstimationType::NONE), - smpMemory(), -#ifdef DFT_MEM_DUMP - ppsMemory(), -#endif - matrix(), - results(), - filterWindowCoefficents(), - absResults(), - absFrequencies(), - calcCount(0), - sampleRate(0), - startFrequency(0), - endFreqency(0), - frequencyResolution(0), - rate(0), - ppsIndex(0), - windowSize(0), - windowMultiplier(0), - freqCount(0), - sync(0), - smpMemPos(0), - lastSequence(0), - windowCorrectionFactor(0), - lastCalc({0, 0}), - nextCalc(0.0), - signalIndex(), - lastResult({0,0,0,0}), - dumperPrefix("/tmp/plot/"), - dumperEnable(logger->level() <= SPDLOG_LEVEL_DEBUG), -#ifdef DFT_MEM_DUMP - origSigSync(dumperPrefix + "origSigSync"), - windowdSigSync(dumperPrefix + "windowdSigSync"), - ppsSigSync(dumperPrefix + "ppsSigSync"), -#endif - phasorRocof(dumperPrefix + "phasorRocof"), - phasorPhase(dumperPrefix + "phasorPhase"), - phasorAmplitude(dumperPrefix + "phasorAmplitude"), - phasorFreq(dumperPrefix + "phasorFreq") - { } - - virtual void prepare() - { - signal_list_clear(&signals); - for (unsigned i = 0; i < signalIndex.size(); i++) { - struct signal *freqSig; - struct signal *amplSig; - struct signal *phaseSig; - 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); - - if (!freqSig || !amplSig || !phaseSig || !rocofSig) - throw RuntimeError("Failed to create new signals"); - - vlist_push(&signals, freqSig); - vlist_push(&signals, amplSig); - vlist_push(&signals, phaseSig); - vlist_push(&signals, rocofSig); - } - - /* Initialize sample memory */ - smpMemory.clear(); - for (unsigned i = 0; i < signalIndex.size(); i++) - smpMemory.emplace_back(windowSize, 0.0); - -#ifdef DFT_MEM_DUMP - /* Initialize temporary ppsMemory */ - ppsMemory.clear(); - ppsMemory.resize(windowSize, 0.0); -#endif - - /* 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); - - freqCount = ceil((endFreqency - startFrequency) / frequencyResolution) + 1; - - /* Initialize matrix of dft coeffients */ - matrix.clear(); - for (unsigned i = 0; i < freqCount; i++) - matrix.emplace_back(windowSize * windowMultiplier, 0.0); - - /* Initalize dft results matrix */ - results.clear(); - for (unsigned i = 0; i < signalIndex.size(); i++){ - results.emplace_back(freqCount, 0.0); - absResults.emplace_back(freqCount, 0.0); - } - - filterWindowCoefficents.resize(windowSize); - - for (unsigned i = 0; i < freqCount; i++) { - absFrequencies.emplace_back(startFrequency + i * frequencyResolution); - } - - generateDftMatrix(); - calculateWindow(windowType); - - state = State::PREPARED; - } - - virtual void parse(json_t *json) - { - int ret; - int windowSizeFactor = 1; - - const char *paddingTypeC = nullptr; - const char *windowTypeC = nullptr; - const char *freqEstimateTypeC = nullptr; - - json_error_t err; - json_t *jsonChannelList = nullptr; - - assert(state != State::STARTED); - - Hook::parse(json); - - ret = json_unpack_ex(json, &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", &startFrequency, - "end_freqency", &endFreqency, - "frequency_resolution", &frequencyResolution, - "dft_rate", &rate, - "window_size_factor", &windowSizeFactor, - "window_type", &windowTypeC, - "padding_type", &paddingTypeC, - "freq_estimate_type", &freqEstimateTypeC, - "sync", &sync, - "signal_index", &jsonChannelList, - "pps_index", &ppsIndex - ); - if (ret) - throw ConfigError(json, err, "node-config-hook-dft"); - - if (jsonChannelList != nullptr) { - signalIndex.clear(); - if (jsonChannelList->type == JSON_ARRAY) { - size_t i; - json_t *jsonValue; - json_array_foreach(jsonChannelList, i, jsonValue) { - if (!json_is_number(jsonValue)) - throw ConfigError(jsonValue, "node-config-hook-dft-channel", "Values must be given as array of integer values!"); - - auto idx = json_number_value(jsonValue); - - signalIndex.push_back(idx); - } - } - else if (jsonChannelList->type == JSON_INTEGER) { - if (!json_is_number(jsonChannelList)) - throw ConfigError(jsonChannelList, "node-config-hook-dft-channel", "Value must be given as integer value!"); - - auto idx = json_number_value(jsonChannelList); - - signalIndex.push_back(idx); - } - else - throw ConfigError(jsonChannelList, "node-config-hook-dft-signal-index", "Could not parse channel list."); - } - else - throw ConfigError(jsonChannelList, "node-config-node-signal", "No parameter signalIndex given."); - - windowSize = sampleRate * windowSizeFactor / (double) rate; - logger->debug("Set windows size to {} samples which fits 1 / rate {}s", windowSize, 1.0 / rate); - - if (!windowTypeC) - logger->info("No Window type given, assume no windowing"); - else if (strcmp(windowTypeC, "flattop") == 0) - windowType = WindowType::FLATTOP; - else if (strcmp(windowTypeC, "hamming") == 0) - windowType = WindowType::HAMMING; - else if (strcmp(windowTypeC, "hann") == 0) - windowType = WindowType::HANN; - else - throw ConfigError(json, "node-config-hook-dft-window-type", "Invalid window type: {}", windowTypeC); - - if (!paddingTypeC) - logger->info("No Padding type given, assume no zeropadding"); - else if (strcmp(paddingTypeC, "signal_repeat") == 0) - paddingType = PaddingType::SIG_REPEAT; - else - throw ConfigError(json, "node-config-hook-dft-padding-type", "Padding type {} not recognized", paddingTypeC); - - if (!freqEstimateTypeC) { - logger->info("No Frequency estimation type given, assume no none"); - freqEstType = FreqEstimationType::NONE; - } - else if (strcmp(freqEstimateTypeC, "quadratic") == 0) - freqEstType = FreqEstimationType::QUADRATIC; - - state = State::PARSED; - } - - virtual void check() - { - assert(state == State::PARSED); - - 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 RuntimeError("End frequency must be smaller than sampleRate {}", sampleRate); - - if (frequencyResolution > (double) sampleRate / windowSize) - throw RuntimeError("The maximum frequency resolution with smaple_rate:{} and window_site:{} is {}", sampleRate, windowSize, ((double)sampleRate/windowSize)); - - state = State::CHECKED; - } - - virtual Hook::Reason process(struct sample *smp) - { - assert(state == State::STARTED); - - for (unsigned i = 0; i < signalIndex.size(); i++) - smpMemory[i][smpMemPos % windowSize] = smp->data[signalIndex[i]].f; - -#ifdef DFT_MEM_DUMP - ppsMemory[smpMemPos % windowSize] = smp->data[ppsIndex].f; -#endif - smpMemPos++; - - bool run = false; - if (sync) { - double smpNsec = smp->ts.origin.tv_sec * 1e9 + smp->ts.origin.tv_nsec; - - if (smpNsec > nextCalc) { - run = true; - nextCalc = (smp->ts.origin.tv_sec + (((calcCount % rate) + 1) / (double) rate)) * 1e9; - } - } - - if (run) { - lastCalc = smp->ts.origin; - -#ifdef DFT_MEM_DUMP - double tmpPPSWindow[windowSize]; - - for (unsigned i = 0; i< windowSize; i++) - tmpPPSWindow[i] = ppsMemory[(i + smpMemPos) % windowSize]; - - if (dumperEnable) - ppsSigSync.writeDataBinary(windowSize, tmpPPSWindow); -#endif - - ppsSigSync->writeDataBinary(windowSize, tmpPPSWindow); - } - - #pragma omp parallel for - for (unsigned i = 0; i < signalIndex.size(); i++) { - Phasor currentResult = {0,0,0,0}; - - calculateDft(PaddingType::ZERO, smpMemory[i], results[i], smpMemPos); - - unsigned maxPos = 0; - - for (unsigned j = 0; j < freqCount; j++) { - int multiplier = paddingType == PaddingType::ZERO - ? 1 - : windowMultiplier; - absResults[i][j] = abs(results[i][j]) * 2 / (windowSize * windowCorrectionFactor * multiplier); - if (currentResult.amplitude < absResults[i][j]) { - currentResult.frequency = absFrequencies[j]; - currentResult.amplitude = absResults[i][j]; - maxPos = j; - } - } - - 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 = { absFrequencies[maxPos - 1], absResults[i][maxPos - 1] }; - Point b = { absFrequencies[maxPos + 0], absResults[i][maxPos + 0] }; - Point c = { absFrequencies[maxPos + 1], absResults[i][maxPos + 1] }; - - Point estimate = quadraticEstimation(a, b, c, maxPos); - currentResult.frequency = estimate.x; - currentResult.amplitude = estimate.y; - } - } - } - - if (windowSize < smpMemPos) { - - smp->data[i * 4 + 0].f = currentResult.frequency; /* Frequency */ - smp->data[i * 4 + 1].f = (currentResult.amplitude / pow(2, 0.5)); /* Amplitude */ - smp->data[i * 4 + 2].f = atan2(results[i][maxPos].imag(), results[i][maxPos].real()); /* Phase */ - smp->data[i * 4 + 3].f = (currentResult.frequency - lastResult.frequency) / (double)rate; /* RoCof */ - - } - - lastResult = currentResult; - } - - // The following is a debug output and currently only for channel 0 - if (dumperEnable && windowSize * 5 < smpMemPos){ - phasorFreq.writeDataBinary(1, &(smp->data[0 * 4 + 0].f)); - phasorPhase.writeDataBinary(1, &(smp->data[0 * 4 + 2].f)); - phasorAmplitude.writeDataBinary(1, &(smp->data[0 * 4 + 1].f)); - phasorRocof.writeDataBinary(1, &(smp->data[0 * 4 + 3].f)); - } - - smp->length = windowSize < smpMemPos ? signalIndex.size() * 4 : 0; - - calcCount++; - } - - if (smp->sequence - lastSequence > 1) - logger->warn("Calculation is not Realtime. {} sampled missed", smp->sequence - lastSequence); - - lastSequence = smp->sequence; - - if (run && windowSize < smpMemPos) - return Reason::OK; - - return Reason::SKIP_SAMPLE; - } - /** * This function generates the furie coeffients for the calculateDft function */ @@ -942,7 +472,6 @@ public: tmpSmpWindow[i] = ringBuffer[(i + ringBufferPos) % windowSize]; #ifdef DFT_MEM_DUMP -<<<<<<< HEAD if (dumperEnable) origSigSync.writeDataBinary(windowSize, tmpSmpWindow); #endif @@ -971,11 +500,9 @@ public: } } -<<<<<<< HEAD /** * This function prepares the selected window coefficents */ -<<<<<<< HEAD void calculateWindow(enum WindowType windowTypeIn) { switch (windowTypeIn) { @@ -1048,224 +575,4 @@ static HookPlugin>>>>>> dft: change memory layout for dft results to provide one vector per signal - void calculateDft(enum PaddingType padding, std::vector &ringBuffer, std::vector> &results, unsigned ringBufferPos) -======= - void calculateDft(enum PaddingType padding, std::vector &ringBuffer, unsigned ringBufferPos) ->>>>>>> dft: tmp debug version -======= - - /** - * This function calculates the discrete furie transform of the input signal - */ - void calculateDft(enum PaddingType padding, std::vector &ringBuffer, std::vector> &results, unsigned ringBufferPos) ->>>>>>> dft: cleanup the mess I made when trying to put the dumper in an independed branch - { - /* RingBuffer size needs to be equal to windowSize - * prepare sample window The following parts can be combined */ - double tmpSmpWindow[windowSize]; - - for (unsigned i = 0; i< windowSize; i++) - tmpSmpWindow[i] = ringBuffer[(i + ringBufferPos) % windowSize]; - -#ifdef DFT_MEM_DUMP - if (dumperEnable) - origSigSync.writeDataBinary(windowSize, tmpSmpWindow); -#endif - - for (unsigned i = 0; i < windowSize; i++) - tmpSmpWindow[i] *= filterWindowCoefficents[i]; - -#ifdef DFT_MEM_DUMP - if (dumperEnable) - windowdSigSync.writeDataBinary(windowSize, tmpSmpWindow); -#endif -======= - - if (origSigSync) - origSigSync->writeDataBinary(windowSize, tmpSmpWindow); - -#endif - - for (unsigned i = 0; i < windowSize; i++) - tmpSmpWindow[i] *= filterWindowCoefficents[i]; - -#ifdef DFT_MEM_DUMP - -<<<<<<< HEAD -<<<<<<< HEAD - if (windowdSigSync) -<<<<<<< HEAD - windowdSigSync->writeData(windowSize, tmpSmpWindow); ->>>>>>> dft: ifdef for full mem dump of dft -======= - windowdSigSync->writeData(windowSize, tmpSmpWindow); ->>>>>>> dft: code cleanup -======= - //if (windowdSigSync) - // windowdSigSync->writeDataBinary(windowSize, tmpSmpWindow); ->>>>>>> dft: tmp debug version -======= - if (windowdSigSync) - windowdSigSync->writeDataBinary(windowSize, tmpSmpWindow); ->>>>>>> dft: cleanup the mess I made when trying to put the dumper in an independed branch - -#endif - - for (unsigned i = 0; i < freqCount; i++) { - results[i] = 0; - - for (unsigned j = 0; j < windowSize * windowMultiplier; j++) { - if (padding == PaddingType::ZERO) { - if (j < (windowSize)) -<<<<<<< HEAD -<<<<<<< HEAD -<<<<<<< HEAD - results[i] += tmpSmpWindow[j] * matrix[i][j]; -======= - results[i] += tmpSmpWindow[j] * dftMatrix[i][j]; ->>>>>>> dft: change memory layout for dft results to provide one vector per signal -======= - dftResults[i] += tmpSmpWindow[j] * dftMatrix[i][j]; ->>>>>>> dft: tmp debug version -======= - results[i] += tmpSmpWindow[j] * dftMatrix[i][j]; ->>>>>>> dft: cleanup the mess I made when trying to put the dumper in an independed branch - else - results[i] += 0; - } - else if (padding == PaddingType::SIG_REPEAT) /* Repeat samples */ -<<<<<<< HEAD -<<<<<<< HEAD -<<<<<<< HEAD - results[i] += tmpSmpWindow[j % windowSize] * matrix[i][j]; -======= - results[i] += tmpSmpWindow[j % windowSize] * dftMatrix[i][j]; ->>>>>>> dft: change memory layout for dft results to provide one vector per signal -======= - dftResults[i] += tmpSmpWindow[j % windowSize] * dftMatrix[i][j]; ->>>>>>> dft: tmp debug version -======= - results[i] += tmpSmpWindow[j % windowSize] * dftMatrix[i][j]; ->>>>>>> dft: cleanup the mess I made when trying to put the dumper in an independed branch - } - } - } - - /** - * 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)) - + 0.277263158 * cos(4 * M_PI * i / (windowSize)) - - 0.083578947 * cos(6 * M_PI * i / (windowSize)) - + 0.006947368 * cos(8 * M_PI * i / (windowSize)); - windowCorrectionFactor += filterWindowCoefficents[i]; - } - break; - - case WindowType::HAMMING: - case WindowType::HANN: { - double a0 = 0.5; /* This is the hann window */ - if (windowTypeIn == WindowType::HAMMING) - a0 = 25./46; - - for (unsigned i = 0; i < windowSize; i++) { - filterWindowCoefficents[i] = a0 - (1 - a0) * cos(2 * M_PI * i / (windowSize)); - windowCorrectionFactor += filterWindowCoefficents[i]; - } - - break; - - } - - default: - for (unsigned i = 0; i < windowSize; i++) { - filterWindowCoefficents[i] = 1; - windowCorrectionFactor += filterWindowCoefficents[i]; - } - break; - } - - windowCorrectionFactor /= 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 = startFrequency + 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 }; - } - - /** - * 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 */ -static char n[] = "dft"; -static char d[] = "This hook calculates the dft on a window"; -static HookPlugin p; - -} /* namespace node */ -} /* namespace villas */ - /** @} */