diff --git a/lib/hooks/dft.cpp b/lib/hooks/dft.cpp index 65b141a9b..0b662d99e 100644 --- a/lib/hooks/dft.cpp +++ b/lib/hooks/dft.cpp @@ -33,7 +33,6 @@ #include #include - namespace villas { namespace node { @@ -64,8 +63,7 @@ protected: struct format_type *format; - double* smp_memory; - double* pps_memory; + double** smp_memory; std::complex** dftMatrix; std::complex* dftResults; double* filterWindowCoefficents; @@ -90,11 +88,12 @@ protected: std::complex omega; std::complex 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[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; its.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]; } diff --git a/lib/hooks/pps_ts.cpp b/lib/hooks/pps_ts.cpp index 00f1c71ca..440ffeee5 100644 --- a/lib/hooks/pps_ts.cpp +++ b/lib/hooks/pps_ts.cpp @@ -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++;