mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
dft: cleanup the mess I made when trying to put the dumper in an independed branch
This commit is contained in:
Manuel Pitz
parent
292fbed681
commit
945a5baec1
1 changed files with 154 additions and 74 deletions
|
|
@ -28,12 +28,14 @@
|
|||
#include <cinttypes>
|
||||
#include <complex>
|
||||
#include <vector>
|
||||
#include <villas/timing.h>
|
||||
|
||||
#include <villas/dumper.hpp>
|
||||
#include <villas/hook.hpp>
|
||||
#include <villas/path.h>
|
||||
#include <villas/sample.h>
|
||||
#include <villas/format.hpp>
|
||||
#include <villas/io.h>
|
||||
#include <villas/plugin.h>
|
||||
|
||||
#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<Dumper> origSigSync;
|
||||
std::shared_ptr<Dumper> windowdSigSync;
|
||||
std::shared_ptr<Dumper> phasorPhase;
|
||||
std::shared_ptr<Dumper> phasorAmplitude;
|
||||
std::shared_ptr<Dumper> phasorFreq;
|
||||
std::shared_ptr<Dumper> ppsSigSync;
|
||||
|
||||
enum WindowType windowType;
|
||||
enum PaddingType paddingType;
|
||||
enum FreqEstimationType freqEstType;
|
||||
|
||||
struct format_type *format;
|
||||
|
||||
std::vector<std::vector<double>> smpMemory;
|
||||
std::vector<double> ppsMemory;//this is just temporary for debugging
|
||||
std::vector<std::vector<std::complex<double>>> dftMatrix;
|
||||
std::vector<std::complex<double>> dftResults;
|
||||
std::vector<std::vector<std::complex<double>>> dftResults;
|
||||
std::vector<double> filterWindowCoefficents;
|
||||
std::vector<double> absDftResults;
|
||||
std::vector<std::vector<double>> absDftResults;
|
||||
std::vector<double> 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<Dumper>("/tmp/plot/origSigSync");
|
||||
windowdSigSync = std::make_shared<Dumper>("/tmp/plot/windowdSigSync");
|
||||
ppsSigSync = std::make_shared<Dumper>("/tmp/plot/ppsSigSync");
|
||||
#endif
|
||||
phasorPhase = std::make_shared<Dumper>("/tmp/plot/phasorPhase");
|
||||
phasorAmplitude = std::make_shared<Dumper>("/tmp/plot/phasorAmplitude");
|
||||
phasorFreq = std::make_shared<Dumper>("/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<double> &ringBuffer, unsigned ringBufferPos)
|
||||
|
||||
/**
|
||||
* 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)
|
||||
{
|
||||
/* 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 */
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue