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

Add asynparam, option enable, option descriptions.

Browse Source
This commit is contained in:
Anders Sandström
2020-04-06 16:34:12 +02:00
parent 26fb2ac183
commit 69e17fcae7
4 changed files with 128 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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 <string>
#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<double>[cfgNfft_];
dataBuffer_ = new double[cfgNfft_]; // Raw input data (real)
fftBuffer_ = new std::complex<double>[cfgNfft_]; // FFT result (complex)
fftBufferAmp_ = new double[cfgNfft_]; // FFT result amplitude (real)
clearBuffers();
// Allocate KissFFT
fftDouble_ = new kissfft<double>(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<double>));
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
// }
// }

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

@@ -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<double>* fftDouble_;
double* dataBuffer_;
std::complex<double>* fftBuffer_; // Result
double* dataBuffer_; // Input data (real)
std::complex<double>* 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);

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

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

10
ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcPluginFFT.c
View File

@@ -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"<source> : 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"<source> : Sets source variable for FFT (example: ec0.s1.AI_1).\n"
" "ECMC_PLUGIN_NFFT_OPTION_CMD"<nfft> : 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.