mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
dft: more cleanups
This commit is contained in:
Manuel Pitz
parent
9ad4e42b17
commit
6a525f7f89
2 changed files with 164 additions and 142 deletions
|
|
@ -33,7 +33,6 @@
|
|||
#include <villas/plugin.h>
|
||||
#include <complex>
|
||||
|
||||
|
||||
namespace villas {
|
||||
namespace node {
|
||||
|
||||
|
|
@ -64,8 +63,7 @@ protected:
|
|||
|
||||
struct format_type *format;
|
||||
|
||||
double* smp_memory;
|
||||
double* pps_memory;
|
||||
double** smp_memory;
|
||||
std::complex<double>** dftMatrix;
|
||||
std::complex<double>* dftResults;
|
||||
double* filterWindowCoefficents;
|
||||
|
|
@ -90,11 +88,12 @@ protected:
|
|||
std::complex<double> omega;
|
||||
std::complex<double> M_I;
|
||||
|
||||
|
||||
double window_corretion_factor;
|
||||
timespec last_dft_cal;
|
||||
|
||||
|
||||
int* signal_index;//a list of signal_index to do dft on
|
||||
uint signalCnt;//number of signal_index given by config file
|
||||
|
||||
|
||||
public:
|
||||
DftHook(struct vpath *p, struct vnode *n, int fl, int prio, bool en = true) :
|
||||
|
|
@ -115,7 +114,8 @@ public:
|
|||
last_sequence(0),
|
||||
M_I(0.0,1.0),
|
||||
window_corretion_factor(0),
|
||||
last_dft_cal({0,0})
|
||||
last_dft_cal({0,0}),
|
||||
signalCnt(0)
|
||||
{
|
||||
format = format_type_lookup("villas.human");
|
||||
|
||||
|
|
@ -128,50 +128,57 @@ public:
|
|||
|
||||
}
|
||||
|
||||
virtual void prepare(){
|
||||
virtual ~DftHook()
|
||||
{
|
||||
delete smp_memory;
|
||||
delete origSigSync;
|
||||
delete ppsSigSync;
|
||||
delete windowdSigSync;
|
||||
delete phasorPhase;
|
||||
delete phasorAmpitude;
|
||||
delete phasorFreq;
|
||||
|
||||
struct signal *freq_sig;
|
||||
struct signal *ampl_sig;
|
||||
struct signal *phase_sig;
|
||||
}
|
||||
|
||||
/* Add signals */
|
||||
freq_sig = signal_create("amplitude", nullptr, SignalType::FLOAT);
|
||||
ampl_sig = signal_create("phase", nullptr, SignalType::FLOAT);
|
||||
phase_sig = signal_create("frequency", nullptr, SignalType::FLOAT);
|
||||
virtual void prepare() {
|
||||
|
||||
signal_list_clear(&signals);
|
||||
|
||||
if (!freq_sig || !ampl_sig || !phase_sig)
|
||||
throw RuntimeError("Failed to create new signals");
|
||||
smp_memory = new double*[signalCnt];
|
||||
|
||||
for (uint i = 0; i < signalCnt; i++) {
|
||||
struct signal *freqSig;
|
||||
struct signal *amplSig;
|
||||
struct signal *phaseSig;
|
||||
struct signal *rocofSig;
|
||||
|
||||
vlist_push(&signals, freq_sig);
|
||||
vlist_push(&signals, ampl_sig);
|
||||
vlist_push(&signals, phase_sig);
|
||||
/* 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);
|
||||
|
||||
|
||||
//offset = vlist_length(&signals) - 1;//needs to be cleaned up
|
||||
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);
|
||||
|
||||
window_multiplier = ceil( ( (double)sample_rate / window_size ) / frequency_resolution);//calculate how much zero padding ist needed for a needed resolution
|
||||
smp_memory[i] = new double[window_size];
|
||||
if (!smp_memory)
|
||||
throw MemoryAllocationError();
|
||||
|
||||
for (uint j = 0; j < window_size; j++)
|
||||
smp_memory[i][j] = 0;
|
||||
}
|
||||
|
||||
freq_count = ceil( ( end_freqency - start_freqency ) / frequency_resolution) + 1;
|
||||
window_multiplier = ceil(((double)sample_rate / window_size) / frequency_resolution);//calculate how much zero padding ist needed for a needed resolution
|
||||
|
||||
freq_count = ceil((end_freqency - start_freqency) / frequency_resolution) + 1;
|
||||
|
||||
//init sample memory
|
||||
smp_memory = new double[window_size];
|
||||
if (!smp_memory)
|
||||
throw MemoryAllocationError();
|
||||
|
||||
for(uint i = 0; i < window_size; i++)
|
||||
smp_memory[i] = 0;
|
||||
|
||||
|
||||
pps_memory = new double[window_size];
|
||||
if (!pps_memory)
|
||||
throw MemoryAllocationError();
|
||||
|
||||
for(uint i = 0; i < window_size; i++)
|
||||
pps_memory[i] = 0;
|
||||
|
||||
|
||||
//init matrix of dft coeffients
|
||||
|
|
@ -179,7 +186,7 @@ public:
|
|||
if (!dftMatrix)
|
||||
throw MemoryAllocationError();
|
||||
|
||||
for(uint i = 0; i < freq_count; i++) {
|
||||
for (uint i = 0; i < freq_count; i++) {
|
||||
dftMatrix[i] = new std::complex<double>[window_size * window_multiplier]();
|
||||
if (!dftMatrix[i])
|
||||
throw MemoryAllocationError();
|
||||
|
|
@ -192,24 +199,21 @@ public:
|
|||
absDftResults = new double[freq_count];
|
||||
|
||||
absDftFreqs = new double[freq_count];
|
||||
for(uint i=0; i < freq_count; i++)
|
||||
for (uint i=0; i < freq_count; i++)
|
||||
absDftFreqs[i] = start_freqency + i * frequency_resolution;
|
||||
|
||||
genDftMatrix();
|
||||
calcWindow(window_type);
|
||||
|
||||
|
||||
state = State::PREPARED;
|
||||
|
||||
}
|
||||
|
||||
|
||||
virtual void start()
|
||||
{
|
||||
|
||||
assert(state == State::PREPARED || state == State::STOPPED);
|
||||
|
||||
|
||||
state = State::STARTED;
|
||||
}
|
||||
|
||||
|
|
@ -217,7 +221,6 @@ public:
|
|||
{
|
||||
assert(state == State::STARTED);
|
||||
|
||||
|
||||
state = State::STOPPED;
|
||||
}
|
||||
|
||||
|
|
@ -227,13 +230,15 @@ public:
|
|||
int ret;
|
||||
json_error_t err;
|
||||
|
||||
json_t *json_channel_list = nullptr;
|
||||
|
||||
assert(state != State::STARTED);
|
||||
|
||||
Hook::parse(cfg);
|
||||
|
||||
state = State::PARSED;
|
||||
|
||||
ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: F, s?: F, s?: F, s?: i , s?: i, s?: s, s?: s, s?: b}",
|
||||
ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: F, s?: F, s?: F, s?: i , s?: i, s?: s, s?: s, s?: b, s?: o}",
|
||||
"sample_rate", &sample_rate,
|
||||
"start_freqency", &start_freqency,
|
||||
"end_freqency", &end_freqency,
|
||||
|
|
@ -242,47 +247,71 @@ public:
|
|||
"window_size", &window_size,
|
||||
"window_type", &window_type_c,
|
||||
"padding_type", &padding_type_c,
|
||||
"sync", &sync_dft
|
||||
|
||||
"sync", &sync_dft,
|
||||
"signal_index", &json_channel_list
|
||||
);
|
||||
|
||||
if(!window_type_c) {
|
||||
if (json_channel_list != nullptr) {
|
||||
if (json_channel_list->type == JSON_ARRAY) {
|
||||
signalCnt = json_array_size(json_channel_list);
|
||||
signal_index = new int[signalCnt];
|
||||
|
||||
size_t i;
|
||||
json_t *json_value;
|
||||
json_array_foreach(json_channel_list, i, json_value) {
|
||||
if (!json_is_number(json_value))
|
||||
throw ConfigError(json_value, "node-config-hook-dft-channel", "Values must be given as array of integer values!");
|
||||
signal_index[i] = json_number_value(json_value);
|
||||
}
|
||||
}else if (json_channel_list->type == JSON_INTEGER) {
|
||||
signalCnt = 1;
|
||||
signal_index = new int[signalCnt];
|
||||
if (!json_is_number(json_channel_list))
|
||||
throw ConfigError(json_channel_list, "node-config-hook-dft-channel", "Value must be given as integer value!");
|
||||
signal_index[0] = json_number_value(json_channel_list);
|
||||
}
|
||||
else
|
||||
warning("Could not parse channel list. Please check documentation for syntax");
|
||||
}
|
||||
else
|
||||
throw ConfigError(json_channel_list, "node-config-node-signal", "No parameter channel given.");
|
||||
|
||||
if (!window_type_c) {
|
||||
info("No Window type given, assume no windowing");
|
||||
window_type = windowType::NONE;
|
||||
} else if(strcmp(window_type_c, "flattop") == 0)
|
||||
} else if (strcmp(window_type_c, "flattop") == 0)
|
||||
window_type = windowType::FLATTOP;
|
||||
else if(strcmp(window_type_c, "hamming") == 0)
|
||||
else if (strcmp(window_type_c, "hamming") == 0)
|
||||
window_type = windowType::HAMMING;
|
||||
else if(strcmp(window_type_c, "hann") == 0)
|
||||
else if (strcmp(window_type_c, "hann") == 0)
|
||||
window_type = windowType::HANN;
|
||||
else {
|
||||
info("Window type %s not recognized, assume no windowing",window_type_c);
|
||||
window_type = windowType::NONE;
|
||||
}
|
||||
|
||||
if(!padding_type_c) {
|
||||
if (!padding_type_c) {
|
||||
info("No Padding type given, assume no zeropadding");
|
||||
padding_type = paddingType::ZERO;
|
||||
} else if(strcmp(padding_type_c, "signal_repeat") == 0) {
|
||||
} else if (strcmp(padding_type_c, "signal_repeat") == 0) {
|
||||
padding_type = paddingType::SIG_REPEAT;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
info("Padding type %s not recognized, assume zero padding",padding_type_c);
|
||||
padding_type = paddingType::ZERO;
|
||||
}
|
||||
|
||||
|
||||
if(end_freqency < 0 || end_freqency > sample_rate){
|
||||
if (end_freqency < 0 || end_freqency > sample_rate) {
|
||||
error("End frequency must be smaller than sample_rate (%i)",sample_rate);
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if(frequency_resolution > ((double)sample_rate/window_size)){
|
||||
error("The maximum frequency resolution with smaple_rate:%i and window_site:%i is %f",sample_rate, window_size, ((double)sample_rate/window_size) );
|
||||
if (frequency_resolution > ((double)sample_rate/window_size)) {
|
||||
error("The maximum frequency resolution with smaple_rate:%i and window_site:%i is %f",sample_rate, window_size, ((double)sample_rate/window_size));
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (ret)
|
||||
throw ConfigError(cfg, err, "node-config-hook-dft");
|
||||
}
|
||||
|
|
@ -290,150 +319,143 @@ public:
|
|||
virtual Hook::Reason process(sample *smp)
|
||||
{
|
||||
assert(state == State::STARTED);
|
||||
|
||||
smp_memory[smp_mem_pos % window_size] = smp->data[0].f;
|
||||
pps_memory[smp_mem_pos % window_size] = smp->data[1].f;
|
||||
smp_mem_pos++ ;
|
||||
for (uint i=0; i< signalCnt; i++) {
|
||||
smp_memory[i][smp_mem_pos % window_size] = smp->data[signal_index[i]].f;
|
||||
}
|
||||
smp_mem_pos++;
|
||||
|
||||
bool runDft = false;
|
||||
if( sync_dft ) {
|
||||
if( last_dft_cal.tv_sec != smp->ts.origin.tv_sec )
|
||||
if (sync_dft) {
|
||||
if (last_dft_cal.tv_sec != smp->ts.origin.tv_sec)
|
||||
runDft = true;
|
||||
}
|
||||
last_dft_cal = smp->ts.origin;
|
||||
|
||||
if( runDft ) {
|
||||
calcDft(paddingType::ZERO);
|
||||
double maxF = 0;
|
||||
double maxA = 0;
|
||||
int maxPos = 0;
|
||||
|
||||
if (runDft) {
|
||||
for (uint i = 0; i < signalCnt; i++){
|
||||
calcDft(paddingType::ZERO, smp_memory[i], smp_mem_pos);
|
||||
double maxF = 0;
|
||||
double maxA = 0;
|
||||
int maxPos = 0;
|
||||
|
||||
|
||||
for(uint i=0; i<freq_count; i++){
|
||||
absDftResults[i] = abs(dftResults[i]) * 2 / (window_size * window_corretion_factor * ((padding_type == paddingType::ZERO)?1:window_multiplier) );
|
||||
if(maxA < absDftResults[i]){
|
||||
maxF = absDftFreqs[i];
|
||||
maxA = absDftResults[i];
|
||||
maxPos = i;
|
||||
for (uint i=0; i<freq_count; i++) {
|
||||
absDftResults[i] = abs(dftResults[i]) * 2 / (window_size * window_corretion_factor * ((padding_type == paddingType::ZERO)?1:window_multiplier));
|
||||
if (maxA < absDftResults[i]) {
|
||||
maxF = absDftFreqs[i];
|
||||
maxA = absDftResults[i];
|
||||
maxPos = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
info("sec=%ld, nsec=%ld freq: %f \t phase: %f \t amplitude: %f",last_dft_cal.tv_sec, smp->ts.origin.tv_nsec, maxF, atan2(dftResults[maxPos].imag(), dftResults[maxPos].real()), (maxA / pow(2,1./2)) );
|
||||
|
||||
if(dftCalcCnt > 1) {
|
||||
double tmpPhase = atan2(dftResults[maxPos].imag(), dftResults[maxPos].real());
|
||||
phasorPhase->writeData(1,&tmpPhase);
|
||||
//double tmpMaxA = maxA / pow(2,1./2);
|
||||
//phasorAmpitude->writeData(1,&tmpMaxA);
|
||||
phasorFreq->writeData(1,&maxF);
|
||||
}
|
||||
|
||||
//info("sec=%ld, nsec=%ld freq: %f \t phase: %f \t amplitude: %f",last_dft_cal.tv_sec, smp->ts.origin.tv_nsec, maxF, atan2(dftResults[maxPos].imag(), dftResults[maxPos].real()), (maxA / pow(2,1./2)));
|
||||
|
||||
if (dftCalcCnt > 1) {
|
||||
//double tmpMaxA = maxA / pow(2,1./2);
|
||||
//phasorAmpitude->writeData(1,&tmpMaxA);
|
||||
phasorFreq->writeData(1,&maxF);
|
||||
|
||||
smp->data[i * 4].f = maxF;//frequency
|
||||
smp->data[i * 4 + 1].f = (maxA / pow(2,1./2));//amplitude
|
||||
smp->data[i * 4 + 2].f = atan2(dftResults[maxPos].imag(), dftResults[maxPos].real());//phase
|
||||
smp->data[i * 4 + 3].f = 0;//rocof
|
||||
|
||||
phasorPhase->writeData(1,&(smp->data[i * 4 + 2].f));
|
||||
}
|
||||
}
|
||||
dftCalcCnt++;
|
||||
smp->length = signalCnt * 4;
|
||||
}
|
||||
|
||||
|
||||
if((smp->sequence - last_sequence) > 1 )
|
||||
|
||||
if ((smp->sequence - last_sequence) > 1)
|
||||
warning("Calculation is not Realtime. %li sampled missed",smp->sequence - last_sequence);
|
||||
|
||||
last_sequence = smp->sequence;
|
||||
|
||||
return Reason::OK;
|
||||
if (runDft)
|
||||
return Reason::OK;
|
||||
|
||||
return Reason::SKIP_SAMPLE;
|
||||
}
|
||||
|
||||
virtual ~DftHook()
|
||||
{
|
||||
//delete smp_memory;
|
||||
|
||||
delete origSigSync;
|
||||
delete ppsSigSync;
|
||||
delete windowdSigSync;
|
||||
delete phasorPhase;
|
||||
delete phasorAmpitude;
|
||||
delete phasorFreq;
|
||||
|
||||
}
|
||||
|
||||
void genDftMatrix(){
|
||||
void genDftMatrix() {
|
||||
using namespace std::complex_literals;
|
||||
|
||||
omega = exp((-2 * M_PI * M_I) / (double)(window_size * window_multiplier));
|
||||
uint startBin = floor( start_freqency / frequency_resolution );
|
||||
uint startBin = floor(start_freqency / frequency_resolution);
|
||||
|
||||
|
||||
for( uint i = 0; i < freq_count ; i++){
|
||||
for( uint j=0 ; j < window_size * window_multiplier ; j++){
|
||||
for (uint i = 0; i < freq_count ; i++) {
|
||||
for (uint j=0 ; j < window_size * window_multiplier ; j++) {
|
||||
dftMatrix[i][j] = pow(omega, (i + startBin) * j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void calcDft(paddingType padding){
|
||||
|
||||
/** mem size needs to be equal to window size **/
|
||||
void calcDft(paddingType padding, double* ringBuffer, uint ringBufferPos) {
|
||||
|
||||
//prepare sample window The following parts can be combined
|
||||
double tmp_smp_window[window_size];
|
||||
double tmp_smp_pps[window_size];
|
||||
|
||||
//uint lastEdge = 0;
|
||||
//uint edgeCount = 0;
|
||||
for(uint i = 0; i< window_size; i++){
|
||||
tmp_smp_window[i] = smp_memory[( i + smp_mem_pos) % window_size];
|
||||
tmp_smp_pps[i] = pps_memory[( i + smp_mem_pos) % window_size];
|
||||
for (uint i = 0; i< window_size; i++) {
|
||||
tmp_smp_window[i] = ringBuffer[(i + ringBufferPos) % window_size];
|
||||
}
|
||||
|
||||
origSigSync->writeData(window_size,tmp_smp_window);
|
||||
ppsSigSync->writeData(window_size,tmp_smp_pps);
|
||||
|
||||
if(dftCalcCnt > 1)
|
||||
if (dftCalcCnt > 1)
|
||||
phasorAmpitude->writeData(1,&tmp_smp_window[window_size - 1]);
|
||||
|
||||
for(uint i = 0; i< window_size; i++) {
|
||||
for (uint i = 0; i< window_size; i++) {
|
||||
tmp_smp_window[i] *= filterWindowCoefficents[i];
|
||||
}
|
||||
windowdSigSync->writeData(window_size,tmp_smp_window);
|
||||
//dumpData("/tmp/plot/signal_windowed",tmp_smp_window,window_size);
|
||||
|
||||
//dumpData("/tmp/plot/smp_window",smp_memory,window_size);
|
||||
|
||||
for( uint i=0; i < freq_count; i++){
|
||||
for (uint i=0; i < freq_count; i++) {
|
||||
dftResults[i] = 0;
|
||||
for(uint j=0; j < window_size * window_multiplier; j++){
|
||||
if(padding == paddingType::ZERO){
|
||||
if(j < (window_size)){
|
||||
for (uint j=0; j < window_size * window_multiplier; j++) {
|
||||
if (padding == paddingType::ZERO) {
|
||||
if (j < (window_size)) {
|
||||
dftResults[i]+= tmp_smp_window[j] * dftMatrix[i][j];
|
||||
}else{
|
||||
}
|
||||
else{
|
||||
dftResults[i]+= 0;
|
||||
}
|
||||
}else if(padding == paddingType::SIG_REPEAT){//repeate samples
|
||||
}
|
||||
else if (padding == paddingType::SIG_REPEAT) {//repeate samples
|
||||
dftResults[i]+= tmp_smp_window[j % window_size] * dftMatrix[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void calcWindow(windowType window_type_in){
|
||||
void calcWindow(windowType window_type_in) {
|
||||
|
||||
|
||||
if(window_type_in == windowType::FLATTOP){
|
||||
for(uint i=0; i < window_size; i++){
|
||||
if (window_type_in == windowType::FLATTOP) {
|
||||
for (uint i=0; i < window_size; i++) {
|
||||
filterWindowCoefficents[i] = 0.21557895
|
||||
- 0.41663158 * cos(2 * M_PI * i / ( window_size ))
|
||||
+ 0.277263158 * cos(4 * M_PI * i / ( window_size ))
|
||||
- 0.083578947 * cos(6 * M_PI * i / ( window_size ))
|
||||
+ 0.006947368 * cos(8 * M_PI * i / ( window_size ));
|
||||
- 0.41663158 * cos(2 * M_PI * i / (window_size))
|
||||
+ 0.277263158 * cos(4 * M_PI * i / (window_size))
|
||||
- 0.083578947 * cos(6 * M_PI * i / (window_size))
|
||||
+ 0.006947368 * cos(8 * M_PI * i / (window_size));
|
||||
window_corretion_factor += filterWindowCoefficents[i];
|
||||
}
|
||||
}else if(window_type_in == windowType::HAMMING || window_type_in == windowType::HANN){
|
||||
}else if (window_type_in == windowType::HAMMING || window_type_in == windowType::HANN) {
|
||||
double a_0 = 0.5;//this is the hann window
|
||||
if(window_type_in == windowType::HAMMING)
|
||||
if (window_type_in == windowType::HAMMING)
|
||||
a_0 = 25./46;
|
||||
|
||||
for(uint i=0; i < window_size; i++){
|
||||
for (uint i=0; i < window_size; i++) {
|
||||
filterWindowCoefficents[i] = a_0
|
||||
- (1 - a_0) * cos(2 * M_PI * i / ( window_size ));
|
||||
- (1 - a_0) * cos(2 * M_PI * i / (window_size));
|
||||
window_corretion_factor += filterWindowCoefficents[i];
|
||||
}
|
||||
}else{
|
||||
for(uint i=0; i < window_size; i++){
|
||||
}
|
||||
else {
|
||||
for (uint i=0; i < window_size; i++) {
|
||||
filterWindowCoefficents[i] = 1;
|
||||
window_corretion_factor += filterWindowCoefficents[i];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -127,7 +127,7 @@ public:
|
|||
unsigned int tmp = cntEdges < filtLen ? cntEdges : horizonEst;
|
||||
double cntSmpsAvg = (cntSmpsTotal - filtWin[(cntEdges - tmp) % filtLen]) / tmp;
|
||||
periodEst = 1.0 / cntSmpsAvg;
|
||||
info("cntSmpsAvg %f", cntSmpsAvg);
|
||||
//info("cntSmpsAvg %f", cntSmpsAvg);
|
||||
periodErrComp = timeErr / (cntSmpsAvg * horizonComp);
|
||||
period = periodEst + periodErrComp;
|
||||
}
|
||||
|
|
@ -141,7 +141,7 @@ public:
|
|||
cntSmps = 0;
|
||||
cntEdges++;
|
||||
|
||||
info("Time Error is: %f periodEst %f periodErrComp %f", timeErr, periodEst, periodErrComp);
|
||||
//info("Time Error is: %f periodEst %f periodErrComp %f", timeErr, periodEst, periodErrComp);
|
||||
}
|
||||
|
||||
cntSmps++;
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue