diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp index b9d2bec..b12f2f8 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp @@ -14,6 +14,12 @@ // Needed to get headers in ecmc right... #define ECMC_IS_PLUGIN +#define ECMC_PLUGIN_ASYN_PREFIX "plugin.fft" +#define ECMC_PLUGIN_ASYN_ENABLE "enable" +#define ECMC_PLUGIN_ASYN_RAWDATA "rawdata" +#define ECMC_PLUGIN_ASYN_FFT_AMP "fftamplitude" + +#include #include "ecmcFFT.h" #include "ecmcFFTDefs.h" #include "ecmcPluginClient.h" @@ -59,6 +65,9 @@ ecmcFFT::ecmcFFT(int fftIndex, // index of this object (if several is cr dataItem_ = NULL; fftDouble_ = NULL; asynPort_ = NULL; + asynEnable_ = NULL; + asynRawData_ = NULL; // Input data + asynFFTAmp_ = NULL; // Result elementsInBuffer_ = 0; fftCalcDone_ = 0; callbackHandle_ = -1; @@ -69,6 +78,7 @@ ecmcFFT::ecmcFFT(int fftIndex, // index of this object (if several is cr cfgNfft_ = ECMC_PLUGIN_DEFAULT_NFFT; // samples in fft (must be n^2) cfgDcRemove_ = 0; cfgApplyScale_ = 1; // Scale as default to get correct amplitude in fft + cfgEnable_ = 0; // start disabled (enable over asyn) asynPort_ = (ecmcAsynPortDriver*) getEcmcAsynPortDriver(); if(!asynPort_) { throw std::runtime_error("Asyn port NULL"); @@ -83,12 +93,15 @@ ecmcFFT::ecmcFFT(int fftIndex, // index of this object (if several is cr scale_ = 1.0 / cfgNfft_; // sqrt((double)cfgNfft_); // Allocate buffers - dataBuffer_ = new double[cfgNfft_]; - fftBuffer_ = new std::complex[cfgNfft_]; + dataBuffer_ = new double[cfgNfft_]; // Raw input data (real) + fftBuffer_ = new std::complex[cfgNfft_]; // FFT result (complex) + fftBufferAmp_ = new double[cfgNfft_]; // FFT result amplitude (real) clearBuffers(); // Allocate KissFFT fftDouble_ = new kissfft(cfgNfft_,false); + + initAsyn(); } ecmcFFT::~ecmcFFT() { @@ -151,6 +164,13 @@ void ecmcFFT::parseConfigStr(char *configStr) { pThisOption += strlen(ECMC_PLUGIN_DC_REMOVE_OPTION_CMD); cfgDcRemove_ = atoi(pThisOption); } + + // ECMC_PLUGIN_ENABLE_OPTION_CMD + else if (!strncmp(pThisOption, ECMC_PLUGIN_ENABLE_OPTION_CMD, strlen(ECMC_PLUGIN_ENABLE_OPTION_CMD))) { + pThisOption += strlen(ECMC_PLUGIN_ENABLE_OPTION_CMD); + cfgEnable_ = atoi(pThisOption); + } + pThisOption = pNextOption; } free(pOptions); @@ -200,8 +220,10 @@ void ecmcFFT::dataUpdatedCallback(uint8_t* data, if(elementsInBuffer_ >= cfgNfft_) { //Buffer full if(!fftCalcDone_){ - calcFFT(); - scaleFFT(); + calcFFT(); // FFT cacluation + scaleFFT(); // Scale FFT + calcFFTAmp(); // Calculate amplitude + if(cfgDbgMode_){ printComplexArray(fftBuffer_, cfgNfft_, @@ -262,18 +284,17 @@ void ecmcFFT::dataUpdatedCallback(uint8_t* data, void ecmcFFT::addDataToBuffer(double data) { if(dataBuffer_ && (elementsInBuffer_ < cfgNfft_) ) { - //if(cfgApplyScale_) { - // dataBuffer_[elementsInBuffer_] = data * scale_; - //} else { - dataBuffer_[elementsInBuffer_] = data; - //} + dataBuffer_[elementsInBuffer_] = data; } elementsInBuffer_ ++; } void ecmcFFT::clearBuffers() { - memset(dataBuffer_, 0, cfgNfft_ * sizeof(double)); - memset(fftBuffer_, 0, cfgNfft_ * sizeof(std::complex)); + memset(dataBuffer_, 0, cfgNfft_ * sizeof(double)); + memset(fftBufferAmp_, 0, cfgNfft_ * sizeof(double)); + for(unsigned int i = 0; i < cfgNfft_; ++i) { + fftBuffer_[i] = {0,0}; + } elementsInBuffer_ = 0; fftCalcDone_ = 0; } @@ -293,6 +314,12 @@ void ecmcFFT::scaleFFT() { } } +void ecmcFFT::calcFFTAmp() { + for(unsigned int i = 0 ; i < cfgNfft_ ; ++i ) { + fftBufferAmp_[i] = std::abs(fftBuffer_[i]); + } +} + void ecmcFFT::printEcDataArray(uint8_t* data, size_t size, ecmcEcDataType dt, @@ -494,3 +521,72 @@ size_t ecmcFFT::getEcDataTypeByteSize(ecmcEcDataType dt){ return 0; } + +// register a dummy asyn parameter "plugin.adv.counter" +void ecmcFFT::initAsyn() { + + if(!asynPort_) { + throw std::runtime_error("Asyn port NULL"); + } + + // Add enable "plugin.fft%d.enable" + std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_ENABLE; + asynEnable_ = asynPort_->addNewAvailParam(paramName.c_str(), // name + asynParamInt32, // asyn type + (uint8_t *)&(cfgEnable_),// pointer to data + sizeof(cfgEnable_), // size of data + ECMC_EC_S32, // ecmc data type + 0); // die if fail + + if(!asynEnable_) { + throw std::runtime_error("Failed to create asyn parameter \"" + paramName +"\".\n"); + } + asynEnable_->setAllowWriteToEcmc(true); + asynEnable_->refreshParam(1); // read once into asyn param lib + + // Add rawdata "plugin.fft%d.rawdata" + paramName =ECMC_PLUGIN_ASYN_PREFIX + std::to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_RAWDATA; + + asynRawData_ = asynPort_->addNewAvailParam(paramName.c_str(), // name + asynParamFloat64Array, // asyn type + (uint8_t *)dataBuffer_, // pointer to data + cfgNfft_*sizeof(double), // size of data + ECMC_EC_F64, // ecmc data type + 0); // die if fail + + if(!asynRawData_) { + throw std::runtime_error("Failed to create asyn parameter \"" + paramName +"\".\n"); + } + asynRawData_->setAllowWriteToEcmc(false); + asynRawData_->refreshParam(1); // read once into asyn param lib + + // Add fft amplitude "plugin.fft%d.fftamplitude" + paramName = ECMC_PLUGIN_ASYN_PREFIX + std::to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_FFT_AMP; + + asynFFTAmp_ = asynPort_->addNewAvailParam(paramName.c_str(), // name + asynParamFloat64Array, // asyn type + (uint8_t *)fftBufferAmp_, // pointer to data + cfgNfft_*sizeof(double), // size of data + ECMC_EC_F64, // ecmc data type + 0); // die if fail + + if(!asynFFTAmp_) { + throw std::runtime_error("Failed to create asyn parameter \"" + paramName +"\".\n"); + } + + asynFFTAmp_->setAllowWriteToEcmc(false); + asynFFTAmp_->refreshParam(1); // read once into asyn param lib +} + +// // increase value of counter and refresh asyn param +// void increaseCounter(){ +// counter++; +// if(paramCount){ +// paramCount->refreshParamRT(0); +// // "callParamCallbacks" are handled in ecmc rt thread so don't call +// } +// } + \ No newline at end of file diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h index 215d9fa..345ed78 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h @@ -45,13 +45,16 @@ class ecmcFFT { void addDataToBuffer(double data); void calcFFT(); void scaleFFT(); + void calcFFTAmp(); + void initAsyn(); static int dataTypeSupported(ecmcEcDataType dt); ecmcDataItem *dataItem_; ecmcAsynPortDriver *asynPort_; kissfft* fftDouble_; - double* dataBuffer_; - std::complex* fftBuffer_; // Result + double* dataBuffer_; // Input data (real) + std::complex* fftBuffer_; // Result (complex) + double* fftBufferAmp_; // Resulting amplitude (abs of fftBuffer_) size_t elementsInBuffer_; // ecmc callback handle for use when deregister at unload int callbackHandle_; @@ -64,7 +67,14 @@ class ecmcFFT { int cfgDbgMode_; // Config: allow dbg printouts int cfgApplyScale_; // Config: apply scale 1/nfft int cfgDcRemove_; // Config: remove dc (average) - size_t cfgNfft_; // Config: Data set size + size_t cfgNfft_; // Config: Data set size + int cfgEnable_; // Config: Enable data acq./calc. + + // Asyn + ecmcAsynDataItem* asynEnable_; + ecmcAsynDataItem* asynRawData_; // Raw data (input) array (double) + ecmcAsynDataItem* asynFFTAmp_; // FFT amplitude array (double) + // Some generic utility functions static uint8_t getUint8(uint8_t* data); diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h index 77d5cdc..198bc24 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h @@ -20,6 +20,7 @@ #define ECMC_PLUGIN_NFFT_OPTION_CMD "NFFT=" #define ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD "APPLY_SCALE=" #define ECMC_PLUGIN_DC_REMOVE_OPTION_CMD "DC_REMOVE=" +#define ECMC_PLUGIN_ENABLE_OPTION_CMD "ENABLE=" /** Just one error code in "c" part of plugin (error handled with exceptions i c++ part) */ diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcPluginFFT.c b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcPluginFFT.c index af01809..8d6a2ef 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcPluginFFT.c +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcPluginFFT.c @@ -37,7 +37,7 @@ static char* lastConfStr = NULL; int adv_exampleConstruct(char *configStr) { //This module is allowed to load several times so no need to check if loaded - + // create FFT object and register data callback lastConfStr = strdup(configStr); return createFFT(configStr); @@ -89,8 +89,12 @@ struct ecmcPluginData pluginDataDef = { // Description .desc = "FFT plugin for use with ecmc.", // Option description - .optionDesc = "\n "ECMC_PLUGIN_DBG_OPTION_CMD"1/0 : Enables/disables printouts from plugin.\n" - " "ECMC_PLUGIN_SOURCE_OPTION_CMD" : Sets source variable for FFT (example: ec0.s1.AI_1).", + .optionDesc = "\n "ECMC_PLUGIN_DBG_OPTION_CMD"1/0 : Enables/disables printouts from plugin.\n" + " "ECMC_PLUGIN_SOURCE_OPTION_CMD" : Sets source variable for FFT (example: ec0.s1.AI_1).\n" + " "ECMC_PLUGIN_NFFT_OPTION_CMD" : Data points to collect.\n" + " "ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD"<1/0> : Apply scale.\n" + " "ECMC_PLUGIN_DC_REMOVE_OPTION_CMD"<1/0> : Remove DC offset of input data (SOURCE).\n" + " "ECMC_PLUGIN_ENABLE_OPTION_CMD"<1/0> : Enable data acq. and calcs (can be controlled over asyn).", // Plugin version .version = ECMC_EXAMPLE_PLUGIN_VERSION, // Optional construct func, called once at load. NULL if not definded.