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

WIP (not compile)

Browse Source
This commit is contained in:
Anders Sandström
2020-04-06 21:45:48 +02:00
parent a051ff6a37
commit 32b7b39f41
6 changed files with 200 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,14 +14,14 @@
// 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"
#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"
#define ECMC_PLUGIN_ASYN_FFT_MODE "mode"
#include <string>
#include <sstream>
#include "ecmcFFT.h"
#include "ecmcFFTDefs.h"
#include "ecmcPluginClient.h"
#include "ecmcAsynPortDriver.h"
@@ -73,12 +73,15 @@ ecmcFFT::ecmcFFT(int fftIndex, // index of this object (if several is cr
callbackHandle_ = -1;
objectId_ = fftIndex;
scale_ = 1.0;
triggOnce_ = 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
cfgEnable_ = 0; // start disabled (enable over asyn)
cfgMode_ = TRIGG;
asynPort_ = (ecmcAsynPortDriver*) getEcmcAsynPortDriver();
if(!asynPort_) {
throw std::runtime_error("Asyn port NULL");
@@ -135,42 +138,53 @@ void ecmcFFT::parseConfigStr(char *configStr) {
pNextOption++; /* Jump to (possible) next */
}
// ECMC_PLUGIN_DBG_OPTION_CMD
// ECMC_PLUGIN_DBG_OPTION_CMD (1/0)
if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_DBG_OPTION_CMD);
cfgDbgMode_ = atoi(pThisOption);
}
// ECMC_PLUGIN_SOURCE_OPTION_CMD
// ECMC_PLUGIN_SOURCE_OPTION_CMD (Source string)
else if (!strncmp(pThisOption, ECMC_PLUGIN_SOURCE_OPTION_CMD, strlen(ECMC_PLUGIN_SOURCE_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_SOURCE_OPTION_CMD);
cfgDataSourceStr_=strdup(pThisOption);
}
// ECMC_PLUGIN_NFFT_OPTION_CMD
// ECMC_PLUGIN_NFFT_OPTION_CMD (1/0)
else if (!strncmp(pThisOption, ECMC_PLUGIN_NFFT_OPTION_CMD, strlen(ECMC_PLUGIN_NFFT_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_NFFT_OPTION_CMD);
cfgNfft_ = atoi(pThisOption);
}
// ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD
// ECMC_PLUGIN_APPLY_SCALE_OPTION_CMD (1/0)
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);
cfgApplyScale_ = atoi(pThisOption);
}
// ECMC_PLUGIN_DC_REMOVE_OPTION_CMD
// ECMC_PLUGIN_DC_REMOVE_OPTION_CMD (1/0)
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);
cfgDcRemove_ = atoi(pThisOption);
}
// ECMC_PLUGIN_ENABLE_OPTION_CMD
// ECMC_PLUGIN_ENABLE_OPTION_CMD (1/0)
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);
}
// ECMC_PLUGIN_MODE_OPTION_CMD CONT/TRIGG
else if (!strncmp(pThisOption, ECMC_PLUGIN_MODE_OPTION_CMD, strlen(ECMC_PLUGIN_MODE_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_MODE_OPTION_CMD);
if(!strncmp(pThisOption, ECMC_PLUGIN_MODE_CONT_OPTION,strlen(ECMC_PLUGIN_MODE_CONT_OPTION))){
cfgMode_ = CONT;
}
if(!strncmp(pThisOption, ECMC_PLUGIN_MODE_TRIGG_OPTION,strlen(ECMC_PLUGIN_MODE_TRIGG_OPTION))){
cfgMode_ = TRIGG;
}
}
pThisOption = pNextOption;
}
free(pOptions);
@@ -208,7 +222,10 @@ void ecmcFFT::dataUpdatedCallback(uint8_t* data,
if(!dataBuffer_ || !cfgEnable_) {
return;
}
if (cfgMode_ == TRIGG && !triggOnce_ ) {
return; // Wait for trigger from plc or asyn
}
if(cfgDbgMode_) {
printEcDataArray(data, size, dt, objectId_);
@@ -298,7 +315,8 @@ void ecmcFFT::clearBuffers() {
memset(dataBuffer_, 0, cfgNfft_ * sizeof(double));
memset(fftBufferAmp_, 0, cfgNfft_ * sizeof(double));
for(unsigned int i = 0; i < cfgNfft_; ++i) {
fftBuffer_[i] = {0,0};
fftBuffer_[i].real(0);
fftBuffer_[i].imag(0);
}
elementsInBuffer_ = 0;
fftCalcDone_ = 0;
@@ -535,8 +553,8 @@ void ecmcFFT::initAsyn() {
}
// Add enable "plugin.fft%d.enable"
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_ENABLE;
std::string paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_ENABLE;
asynEnable_ = asynPort_->addNewAvailParam(paramName.c_str(), // name
asynParamInt32, // asyn type
(uint8_t *)&(cfgEnable_),// pointer to data
@@ -551,7 +569,7 @@ void ecmcFFT::initAsyn() {
asynEnable_->refreshParam(1); // read once into asyn param lib
// Add rawdata "plugin.fft%d.rawdata"
paramName =ECMC_PLUGIN_ASYN_PREFIX + std::to_string(objectId_) +
paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_RAWDATA;
asynRawData_ = asynPort_->addNewAvailParam(paramName.c_str(), // name
@@ -568,25 +586,58 @@ void ecmcFFT::initAsyn() {
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_) +
paramName = ECMC_PLUGIN_ASYN_PREFIX + 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
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
asynFFTAmp_->refreshParam(1); // read once into asyn param lib
// Add fft mode "plugin.fft%d.mode"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_MODE;
asynFFTMode_ = asynPort_->addNewAvailParam(paramName.c_str(), // name
asynParamInt32, // asyn type
(uint8_t *)cfgMode_, // pointer to data
sizeof(cfgMode_), // size of data
ECMC_EC_S32, // ecmc data type
0); // die if fail
if(!asynFFTMode_) {
throw std::runtime_error("Failed to create asyn parameter \"" + paramName +"\".\n");
}
asynFFTMode_->setAllowWriteToEcmc(true);
asynFFTMode_->refreshParam(1); // read once into asyn param lib
}
// Avoid issues with std:to_string()
std::string ecmcFFT::to_string(int value) {
std::ostringstream os;
os << value;
return os.str();
}
void ecmcFFT::setEnable(int enable) {
cfgEnable_ = enable;
}
void ecmcFFT::triggFFT() {
clearBuffers();
triggOnce_ = 1;
}
void ecmcFFT::setModeFFT(FFT_MODE mode) {
cfgMode_ = mode;
}

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

@@ -15,7 +15,9 @@
#include <stdexcept>
#include "ecmcDataItem.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcFFTDefs.h"
#include "inttypes.h"
#include <string>
#include "kissfft/kissfft.hh"
class ecmcFFT {
@@ -39,7 +41,9 @@ class ecmcFFT {
// Call just before realtime because then all data sources should be available
void connectToDataSource();
void setEnable(int enable);
void setModeFFT(FFT_MODE mode);
void clearBuffers();
void triggFFT();
private:
void parseConfigStr(char *configStr);
@@ -61,6 +65,7 @@ class ecmcFFT {
int callbackHandle_;
int fftCalcDone_;
int objectId_; // Unique object id
int triggOnce_;
double scale_; // Config: Data set size
// Config options
@@ -70,12 +75,13 @@ class ecmcFFT {
int cfgDcRemove_; // Config: remove dc (average)
size_t cfgNfft_; // Config: Data set size
int cfgEnable_; // Config: Enable data acq./calc.
FFT_MODE cfgMode_; // Config: Mode continous or triggered.
// Asyn
ecmcAsynDataItem* asynEnable_;
ecmcAsynDataItem* asynEnable_; // Enable/disable acq./calcs
ecmcAsynDataItem* asynRawData_; // Raw data (input) array (double)
ecmcAsynDataItem* asynFFTAmp_; // FFT amplitude array (double)
ecmcAsynDataItem* asynFFTMode_; // FFT mode cont/trigg
// Some generic utility functions
static uint8_t getUint8(uint8_t* data);
@@ -96,6 +102,7 @@ class ecmcFFT {
static void printComplexArray(std::complex<double>* fftBuff,
size_t elements,
int objId);
static std::string to_string(int value);
};
#endif /* ECMC_FFT_H_ */

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

@@ -21,6 +21,16 @@
#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="
// CONT, TRIGG
#define ECMC_PLUGIN_MODE_OPTION_CMD "MODE="
#define ECMC_PLUGIN_MODE_CONT_OPTION "CONT"
#define ECMC_PLUGIN_MODE_TRIGG_OPTION "TRIGG"
enum FFT_MODE{
NO_MODE = 0,
CONT = 1,
TRIGG = 2,
};
/** Just one error code in "c" part of plugin
(error handled with exceptions i c++ part) */

22
ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTWrap.cpp
View File

@@ -88,3 +88,25 @@ int clearFFT(int fftIndex) {
}
return 0;
}
int triggFFT(int fftIndex) {
try {
ffts.at(fftIndex)->triggFFT();
}
catch(std::exception& e) {
printf("Exception: %s. FFT index out of range.\n",e.what());
return ECMC_PLUGIN_FFT_ERROR_CODE;
}
return 0;
}
int setModeFFT(int fftIndex, FFT_MODE mode) {
try {
ffts.at(fftIndex)->setMode(mode);
}
catch(std::exception& e) {
printf("Exception: %s. FFT index out of range.\n",e.what());
return ECMC_PLUGIN_FFT_ERROR_CODE;
}
return 0;
}

4
ecmcPlugin_FFT-loc/ecmcPlugin_FFTApp/src/ecmcFFTWrap.h
View File

@@ -11,6 +11,7 @@
\*************************************************************************/
#ifndef ECMC_FFT_WRAP_H_
#define ECMC_FFT_WRAP_H_
#include "ecmcFFTDefs.h"
# ifdef __cplusplus
extern "C" {
@@ -21,7 +22,8 @@ int linkDataToFFTs();
void deleteAllFFTs();
int enableFFT(int fftIndex, int enable);
int clearFFT(int fftIndex);
int triggFFT(int fftIndex);
int setModeFFT(int fftIndex, FFT_MODE mode);
# ifdef __cplusplus
}
# endif // ifdef __cplusplus

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

@@ -34,7 +34,7 @@ static char* lastConfStr = NULL;
* Return value other than 0 will be considered error.
* configStr can be used for configuration parameters.
**/
int fft_exampleConstruct(char *configStr)
int fftConstruct(char *configStr)
{
//This module is allowed to load several times so no need to check if loaded
@@ -46,7 +46,7 @@ int fft_exampleConstruct(char *configStr)
/** Optional function.
* Will be called once at unload.
**/
void fft_exampleDestruct(void)
void fftDestruct(void)
{
deleteAllFFTs();
if(lastConfStr){
@@ -60,7 +60,7 @@ void fft_exampleDestruct(void)
* this plugin to react on ecmc errors
* Return value other than 0 will be considered to be an error code in ecmc.
**/
int fft_exampleRealtime(int ecmcError)
int fftRealtime(int ecmcError)
{
lastEcmcError = ecmcError;
return 0;
@@ -68,7 +68,7 @@ int fft_exampleRealtime(int ecmcError)
/** Link to data source here since all sources should be availabe at this stage
**/
int fft_exampleEnterRT(){
int fftEnterRT(){
return linkDataToFFTs();
}
@@ -76,7 +76,7 @@ int fft_exampleEnterRT(){
* Will be called once just before leaving realtime mode
* Return value other than 0 will be considered error.
**/
int fft_exampleExitRT(void){
int fftExitRT(void){
return 0;
}
@@ -90,6 +90,16 @@ double fft_enable(double index, double enable) {
return (double)enableFFT((int)index, (int)enable);
}
// Plc function for trigg new measurement
double fft_trigg(double index) {
return (double)triggFFT((int)index);
}
// Plc function for enable
double fft_mode(double index, double mode) {
return (double)setModeFFT((int)index, (FFT_MODE)mode);
}
// Register data for plugin so ecmc know what to use
struct ecmcPluginData pluginDataDef = {
// Allways use ECMC_PLUG_VERSION_MAGIC
@@ -99,12 +109,14 @@ 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"
.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).",
" "ECMC_PLUGIN_ENABLE_OPTION_CMD"<1/0> : Enable data acq. and calcs (can be controlled over asyn)."
" "ECMC_PLUGIN_MODE_OPTION_CMD"<CONT/TRIGG> : Continious or triggered mode."
,
// Plugin version
.version = ECMC_EXAMPLE_PLUGIN_VERSION,
// Optional construct func, called once at load. NULL if not definded.
@@ -119,7 +131,7 @@ struct ecmcPluginData pluginDataDef = {
.realtimeExitFnc = fft_exampleExitRT,
// PLC funcs
.funcs[0] =
{ /*----customPlcFunc2----*/
{ /*----fft_clear----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "fft_clear",
// Function description
@@ -141,7 +153,7 @@ struct ecmcPluginData pluginDataDef = {
.funcArg10 = NULL,
},
.funcs[1] =
{ /*----customPlcFunc2----*/
{ /*----fft_enable----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "fft_enable",
// Function description
@@ -162,9 +174,65 @@ struct ecmcPluginData pluginDataDef = {
.funcArg9 = NULL,
.funcArg10 = NULL,
},
.funcs[2] = {0}, // last element set all to zero..
.funcs[2] =
{ /*----fft_trigg----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "fft_trigg",
// Function description
.funcDesc = "double fft_trigg(index) : Trigg new measurement for fft[index].",
/**
* 7 different prototypes allowed (only doubles since reg in plc).
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
**/
.funcArg0 = NULL,
.funcArg1 = fft_trigg,
.funcArg2 = NULL,
.funcArg3 = NULL,
.funcArg4 = NULL,
.funcArg5 = NULL,
.funcArg6 = NULL,
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = NULL,
},
.funcs[3] =
{ /*----fft_mode----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "fft_mode",
// Function description
.funcDesc = "double fft_mode(index, mode) : Set mode Cont(1)/Trigg(2) for fft[index].",
/**
* 7 different prototypes allowed (only doubles since reg in plc).
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
**/
.funcArg0 = NULL,
.funcArg1 = NULL,
.funcArg2 = fft_trigg,
.funcArg3 = NULL,
.funcArg4 = NULL,
.funcArg5 = NULL,
.funcArg6 = NULL,
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = NULL,
},
.funcs[4] = {0}, // last element set all to zero..
// PLC consts
.consts[0] = {0}, // last element set all to zero..
/* CONTINIOUS MODE = 1 */
.consts[0] = {
.constName = "fft_MODE_CONT",
.constDesc = "Continious mode",
.constValue = CONT
},
/* TRIGGERED MODE = 2 */
.consts[1] = {
.constName = "fft_MODE_TRIGG",
.constDesc = "Triggered mode",
.constValue = TRIGG
},
.consts[2] = {0}, // last element set all to zero..
};
ecmc_plugin_register(pluginDataDef);