From cdb8414224c3ec88af911677bca69874ef551e0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Sun, 5 Apr 2020 14:46:29 +0200 Subject: [PATCH] Add configs APPLY_SCALE and DC_REMOVE. WIP.. --- .../ecmcPlugin_FFTApp/src/ecmcFFT.cpp | 93 ++++++++++++------- .../ecmcPlugin_FFTApp/src/ecmcFFT.h | 37 ++++---- .../ecmcPlugin_FFTApp/src/ecmcFFTDefs.h | 13 ++- 3 files changed, 89 insertions(+), 54 deletions(-) diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp index 3473f06..c1fd54d 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp @@ -21,7 +21,7 @@ #define PRINT_IF_DBG_MODE(fmt, ...) \ { \ - if(dbgMode_){ \ + if(cfgDbgMode_){ \ printf(fmt, ## __VA_ARGS__); \ } \ } \ @@ -52,36 +52,46 @@ void f_dataUpdatedCallback(uint8_t* data, size_t size, ecmcEcDataType dt, void* * - invalid_argument * - runtime_error */ -ecmcFFT::ecmcFFT(int fftIndex, // index of this object +ecmcFFT::ecmcFFT(int fftIndex, // index of this object (if several is created) char* configStr) { - dataSourceStr_ = NULL; + cfgDataSourceStr_ = NULL; dataBuffer_ = NULL; dataItem_ = NULL; fftDouble_ = NULL; asynPort_ = NULL; elementsInBuffer_ = 0; - dbgMode_ = 0; fftCalcDone_ = 0; callbackHandle_ = -1; objectId_ = fftIndex; - nfft_ = ECMC_PLUGIN_DEFAULT_NFFT; // samples in fft (must be n^2) + scale_ = 1.0; + // Config defaults + cfgDbgMode_ = 0; + 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 + asynPort_ = (ecmcAsynPortDriver*) getEcmcAsynPortDriver(); if(!asynPort_) { throw std::runtime_error("Asyn port NULL"); } - parseConfigStr(configStr); // Assigns Configs + parseConfigStr(configStr); // Assigns all configs + // Check valid nfft + if(cfgNfft_ <= 0) { + throw std::out_of_range("NFFT must be > 0 and even N^2."); + } + // set scale factor + scale_ = 1.0 / (double)cfgNfft_; + connectToDataSource(); // Also assigns dataItem_ // Allocate buffers - dataBufferSize_ = nfft_ * sizeof(double); - dataBuffer_ = new double[dataBufferSize_]; - fftBufferSize_ = (nfft_ * sizeof(double)) / 2 + 1; - fftBuffer_ = new std::complex[fftBufferSize_]; + dataBuffer_ = new double[cfgNfft_]; + fftBuffer_ = new std::complex[cfgNfft_]; clearBuffers(); // Allocate KissFFT - fftDouble_ = new kissfft(nfft_,false); + fftDouble_ = new kissfft(cfgNfft_,false); } ecmcFFT::~ecmcFFT() { @@ -92,8 +102,8 @@ ecmcFFT::~ecmcFFT() { if(callbackHandle_ >= 0) { dataItem_->deregDataUpdatedCallback(callbackHandle_); } - if(dataSourceStr_) { - free(dataSourceStr_); + if(cfgDataSourceStr_) { + free(cfgDataSourceStr_); } if(fftDouble_) { delete fftDouble_; @@ -118,34 +128,50 @@ void ecmcFFT::parseConfigStr(char *configStr) { // ECMC_PLUGIN_DBG_OPTION_CMD if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_OPTION_CMD))) { pThisOption += strlen(ECMC_PLUGIN_DBG_OPTION_CMD); - dbgMode_ = atoi(pThisOption); + cfgDbgMode_ = atoi(pThisOption); } // ECMC_PLUGIN_SOURCE_OPTION_CMD else if (!strncmp(pThisOption, ECMC_PLUGIN_SOURCE_OPTION_CMD, strlen(ECMC_PLUGIN_SOURCE_OPTION_CMD))) { pThisOption += strlen(ECMC_PLUGIN_SOURCE_OPTION_CMD); // get string to next ';' - dataSourceStr_=strdup(pThisOption); + cfgDataSourceStr_=strdup(pThisOption); } // ECMC_PLUGIN_NFFT_OPTION_CMD else if (!strncmp(pThisOption, ECMC_PLUGIN_NFFT_OPTION_CMD, strlen(ECMC_PLUGIN_NFFT_OPTION_CMD))) { pThisOption += strlen(ECMC_PLUGIN_NFFT_OPTION_CMD); // get string to next ';' - nfft_ = atoi(pThisOption); + cfgNfft_ = atoi(pThisOption); + } + + // ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD + else if (!strncmp(pThisOption, ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD, strlen(ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD))) { + pThisOption += strlen(ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD); + // get string to next ';' + cfgApplyScale_ = atoi(pThisOption); + } + + // ECMC_PLUGIN_DC_REMOVE_OPTION_CMD + else if (!strncmp(pThisOption, ECMC_PLUGIN_DC_REMOVE_OPTION_CMD, strlen(ECMC_PLUGIN_DC_REMOVE_OPTION_CMD))) { + pThisOption += strlen(ECMC_PLUGIN_DC_REMOVE_OPTION_CMD); + // get string to next ';' + cfgDcRemove_ = atoi(pThisOption); } pThisOption = pNextOption; } free(pOptions); } - if(!dataSourceStr_) { + + // Data source must be defined + if(!cfgDataSourceStr_) { throw std::invalid_argument( "Data source not defined."); } } void ecmcFFT::connectToDataSource() { // Get dataItem - dataItem_ = (ecmcDataItem*) getEcmcDataItem(dataSourceStr_); + dataItem_ = (ecmcDataItem*) getEcmcDataItem(cfgDataSourceStr_); if(!dataItem_) { throw std::runtime_error("Data item NULL."); } @@ -153,7 +179,6 @@ void ecmcFFT::connectToDataSource() { // Register data callback callbackHandle_ = dataItem_->regDataUpdatedCallback(f_dataUpdatedCallback, this); if (callbackHandle_ < 0) { - callbackHandle_ = -1; throw std::runtime_error( "Failed to register data source callback."); } @@ -171,21 +196,21 @@ void ecmcFFT::dataUpdatedCallback(uint8_t* data, return; } - if(dbgMode_) { - printData(data, size, dt, objectId_); + if(cfgDbgMode_) { + printEcDataArray(data, size, dt, objectId_); - if(elementsInBuffer_ == nfft_) { + if(elementsInBuffer_ == cfgNfft_) { printf("Buffer full (%zu elements appended).\n",elementsInBuffer_); } } - if(elementsInBuffer_ >= nfft_) { + if(elementsInBuffer_ >= cfgNfft_) { //Buffer full if(!fftCalcDone_){ calcFFT(); - if(dbgMode_){ - printResult(fftBuffer_, - fftBufferSize_, + if(cfgDbgMode_){ + printComplexArray(fftBuffer_, + cfgNfft_, objectId_); } // Buffer new data @@ -238,16 +263,20 @@ void ecmcFFT::dataUpdatedCallback(uint8_t* data, } void ecmcFFT::addDataToBuffer(double data) { - if(dataBuffer_ && elementsInBuffer_ < nfft_) { - dataBuffer_[elementsInBuffer_] = data; + if(dataBuffer_ && elementsInBuffer_ < cfgNfft_) { + if(cfgApplyScale_) { + dataBuffer_[elementsInBuffer_] = data*scale_; + } else { + dataBuffer_[elementsInBuffer_] = data; + } } elementsInBuffer_ ++; } void ecmcFFT::clearBuffers() { - memset(dataBuffer_, 0, dataBufferSize_); + memset(dataBuffer_, 0, cfgNfft_ * sizeof(double)); + memset(fftBuffer_, 0, cfgNfft_ * sizeof(std::complex)); elementsInBuffer_ = 0; - memset(fftBuffer_, 0, fftBufferSize_); fftCalcDone_ = 0; } @@ -256,7 +285,7 @@ void ecmcFFT::calcFFT() { fftCalcDone_ = 1; } -void ecmcFFT::printData(uint8_t* data, +void ecmcFFT::printEcDataArray(uint8_t* data, size_t size, ecmcEcDataType dt, int objId) { @@ -305,7 +334,7 @@ void ecmcFFT::printData(uint8_t* data, } } -void ecmcFFT::printResult(std::complex* fftBuff, +void ecmcFFT::printComplexArray(std::complex* fftBuff, size_t elements, int objId) { printf("fft id: %d, results: \n",objId); diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h index a1535e9..860e382 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h @@ -42,25 +42,28 @@ class ecmcFFT { void calcFFT(); void addDataToBuffer(double data); + static int dataTypeSupported(ecmcEcDataType dt); + ecmcDataItem *dataItem_; ecmcAsynPortDriver *asynPort_; + kissfft* fftDouble_; double* dataBuffer_; - size_t dataBufferSize_; std::complex* fftBuffer_; // Result - size_t fftBufferSize_; - size_t nfft_; size_t elementsInBuffer_; // ecmc callback handle for use when deregister at unload int callbackHandle_; - int dbgMode_; //Allow dbg printouts int fftCalcDone_; - char* dataSourceStr_; - // A unique object id for this fft (if plugin is more than once) - int objectId_; - static int dataTypeSupported(ecmcEcDataType dt); - kissfft* fftDouble_; + int objectId_; // Unique object id + double scale_; // Config: Data set size - // Some utility functions + // Config options + char* cfgDataSourceStr_; // Config: data source string + 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 + + // Some generic utility functions static uint8_t getUint8(uint8_t* data); static int8_t getInt8(uint8_t* data); static uint16_t getUint16(uint8_t* data); @@ -72,13 +75,13 @@ class ecmcFFT { static float getFloat32(uint8_t* data); static double getFloat64(uint8_t* data); static size_t getEcDataTypeByteSize(ecmcEcDataType dt); - static void printData(uint8_t* data, - size_t size, - ecmcEcDataType dt, - int objId); - static void printResult(std::complex* fftBuff, - size_t elements, - int objId); + static void printEcDataArray(uint8_t* data, + size_t size, + ecmcEcDataType dt, + int objId); + static void printComplexArray(std::complex* fftBuff, + size_t elements, + int objId); }; #endif /* ECMC_FFT_H_ */ diff --git a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h index 9aaa942..77d5cdc 100644 --- a/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h +++ b/ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h @@ -15,14 +15,17 @@ #define ECMC_FFT_DEFS_H_ // Options -#define ECMC_PLUGIN_DBG_OPTION_CMD "DBG_PRINT=" -#define ECMC_PLUGIN_SOURCE_OPTION_CMD "SOURCE=" -#define ECMC_PLUGIN_NFFT_OPTION_CMD "NFFT=" +#define ECMC_PLUGIN_DBG_OPTION_CMD "DBG_PRINT=" +#define ECMC_PLUGIN_SOURCE_OPTION_CMD "SOURCE=" +#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=" -// Just one error code in "c" part of plugin +/** Just one error code in "c" part of plugin +(error handled with exceptions i c++ part) */ #define ECMC_PLUGIN_FFT_ERROR_CODE 1 // Default size (must be n²) -#define ECMC_PLUGIN_DEFAULT_NFFT 8192 +#define ECMC_PLUGIN_DEFAULT_NFFT 4096 #endif /* ECMC_FFT_DEFS_H_ */