motion/ecmc_plugin_fft
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add configs APPLY_SCALE and DC_REMOVE. WIP..

Browse Source
This commit is contained in:
Anders Sandström
2020-04-05 14:46:29 +02:00
parent 5b4dc96543
commit cdb8414224
3 changed files with 89 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.cpp
View File

@@ -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<double>[fftBufferSize_];
dataBuffer_ = new double[cfgNfft_];
fftBuffer_ = new std::complex<double>[cfgNfft_];
clearBuffers();
// Allocate KissFFT
fftDouble_ = new kissfft<double>(nfft_,false);
fftDouble_ = new kissfft<double>(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<double>));
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<double>* fftBuff,
void ecmcFFT::printComplexArray(std::complex<double>* fftBuff,
size_t elements,
int objId) {
printf("fft id: %d, results: \n",objId);

37
ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFT.h
View File

@@ -42,25 +42,28 @@ class ecmcFFT {
void calcFFT();
void addDataToBuffer(double data);
static int dataTypeSupported(ecmcEcDataType dt);
ecmcDataItem *dataItem_;
ecmcAsynPortDriver *asynPort_;
kissfft<double>* fftDouble_;
double* dataBuffer_;
size_t dataBufferSize_;
std::complex<double>* 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<double>* 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<double>* fftBuff,
size_t elements,
int objId);
static void printEcDataArray(uint8_t* data,
size_t size,
ecmcEcDataType dt,
int objId);
static void printComplexArray(std::complex<double>* fftBuff,
size_t elements,
int objId);
};
#endif /* ECMC_FFT_H_ */

13
ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTDefs.h
View File

@@ -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_ */