dft: cleanup the mess I made when trying to put the dumper in an independed branch

include/villas/dumper.hpp

@@ -45,7 +45,6 @@ public:
 
 	int openSocket();
 	int closeSocket();
-	
 
 	void writeDataCSV(unsigned len, double *yData, double *xData = nullptr);
 	void writeDataBinary(unsigned len, double *yData, double *xData = nullptr);

lib/hooks/dft.cpp

@@ -28,6 +28,7 @@
 #include <cinttypes>
 #include <complex>
 #include <vector>
+#include <villas/timing.h>
 
 #include <villas/dumper.hpp>
 #include <villas/hook.hpp>
@@ -43,12 +44,12 @@ namespace node {
 class DftHook : public Hook {
 
 protected:
-	enum PaddingType {
+	enum class PaddingType {
 		ZERO,
 		SIG_REPEAT
 	};
 
-	enum WindowType {
+	enum class WindowType {
 		NONE,
 		FLATTOP,
 		HANN,
@@ -127,6 +128,7 @@ public:
 		Hook(p, n, fl, prio, en),
 		windowType(WindowType::NONE),
 		paddingType(PaddingType::ZERO),
+		freqEstType(FreqEstimationType::NONE),
 		smpMemory(),
 #ifdef DFT_MEM_DUMP
 		ppsMemory(),
@@ -620,9 +622,6 @@ public:
 	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;
@@ -630,10 +629,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");
@@ -655,13 +654,20 @@ public:
 		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);
+		windowMultiplier = ceil(((double) sampleRate / windowSize) / frequencyResolution);
 
 		freqCount = ceil((endFreqency - startFrequency) / frequencyResolution) + 1;
 
-		logger->debug("FreqCount : {}", freqCount);
-
 		/* Initialize matrix of dft coeffients */
 		matrix.clear();
 		for (unsigned i = 0; i < freqCount; i++)
@@ -675,8 +681,6 @@ public:
 		}
 
 		filterWindowCoefficents.resize(windowSize);
-		absDftResults.resize(freqCount);
-		absDftFreqs.resize(freqCount);
 
 		for (unsigned i = 0; i < freqCount; i++) {
 			absFrequencies.emplace_back(startFrequency + i * frequencyResolution);
@@ -718,10 +722,6 @@ public:
 			"signal_index", &jsonChannelList,
 			"pps_index", &ppsIndex
 		);
-
-		dftRate.tv_sec = (int) (1 / dftRateIn);
-		dftRate.tv_nsec = fmod( 1 / dftRateIn, 1) * 1e9;
-
 		if (ret)
 			throw ConfigError(json, err, "node-config-hook-dft");
 
@@ -788,6 +788,13 @@ public:
 	{
 		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);
 
@@ -832,8 +839,8 @@ public:
 				ppsSigSync.writeDataBinary(windowSize, tmpPPSWindow);
 #endif
 
-				//ppsSigSync->writeDataBinary(windowSize, tmpPPSWindow);	
-			//}
+				ppsSigSync->writeDataBinary(windowSize, tmpPPSWindow);	
+			}
 		
 			#pragma omp parallel for
 			for (unsigned i = 0; i < signalIndex.size(); i++) {
@@ -842,9 +849,7 @@ public:
 				calculateDft(PaddingType::ZERO, smpMemory[i], results[i], smpMemPos);
 
 				unsigned maxPos = 0;
-				double tmpImag[freqCount], tmpReal[freqCount], absVal[freqCount];
 
-				
 				for (unsigned j = 0; j < freqCount; j++) {
 					int multiplier = paddingType == PaddingType::ZERO
 						? 1
@@ -855,12 +860,7 @@ public:
 						currentResult.amplitude = absResults[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)
@@ -877,17 +877,13 @@ public:
 				}
 				}
 
-				if (dftCalcCnt > 1) {
-					if (phasorFreq)
-						phasorFreq->writeData(1, &maxF);
+				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 */
 
-					if (phasorPhase)
-						phasorPhase->writeData(1, &(smp->data[i * 4 + 2].f));
 				}
 
 				lastResult = currentResult;
@@ -906,7 +902,7 @@ public:
 			calcCount++;
 		}
 
-		if ((smp->sequence - lastSequence) > 1)
+		if (smp->sequence - lastSequence > 1)
 			logger->warn("Calculation is not Realtime. {} sampled missed", smp->sequence - lastSequence);
 
 		lastSequence = smp->sequence;
@@ -917,6 +913,9 @@ public:
 		return Reason::SKIP_SAMPLE;
 	}
 
+	/**
+	 * This function generates the furie coeffients for the calculateDft function
+	 */
 	void generateDftMatrix()
 	{
 		using namespace std::complex_literals;
@@ -1060,6 +1059,7 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 		}
 	}
 
+<<<<<<< HEAD
 	/**
 	 * This function calculates the discrete furie transform of the input signal
 	 */
@@ -1069,6 +1069,13 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 =======
 	void calculateDft(enum PaddingType padding, std::vector<double> &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<double> &ringBuffer, std::vector<std::complex<double>> &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 */
@@ -1091,8 +1098,9 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 #endif
 =======
 
-		//if (origSigSync)
-		// 	origSigSync->writeDataBinary(windowSize, tmpSmpWindow);
+		if (origSigSync)
+		 	origSigSync->writeDataBinary(windowSize, tmpSmpWindow);
+
 #endif
 
 		for (unsigned i = 0; i < windowSize; i++)
@@ -1100,6 +1108,7 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 
 #ifdef DFT_MEM_DUMP
 
+<<<<<<< HEAD
 <<<<<<< HEAD
 		if (windowdSigSync)
 <<<<<<< HEAD
@@ -1112,15 +1121,21 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 		//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++) {
-			dftResults[i] = 0;
+			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];
 =======
@@ -1129,11 +1144,15 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 =======
 						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
-						dftResults[i] += 0;
+						results[i] += 0;
 				}
 				else if (padding == PaddingType::SIG_REPEAT) /* Repeat samples */
 <<<<<<< HEAD
+<<<<<<< HEAD
 <<<<<<< HEAD
 					results[i] += tmpSmpWindow[j % windowSize] * matrix[i][j];
 =======
@@ -1142,17 +1161,23 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 =======
 					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))
+					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));
@@ -1208,6 +1233,30 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 
 		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 */