From 027c5c5d16052b5b7c55a35a3392fabc9dde647d Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Mon, 1 Mar 2021 13:31:28 +0100 Subject: [PATCH] Cleanup --- .../ecmc_plugin_socketcanApp/src/ecmcFFT.cpp | 1057 ----------------- .../ecmc_plugin_socketcanApp/src/ecmcFFT.h | 153 --- .../src/ecmcFFTDefs.h | 54 - .../src/ecmcFFTWrap.cpp | 133 --- .../src/ecmcFFTWrap.h | 112 -- ...{ecmcPluginFFT.c => ecmcPluginSocketCAN.c} | 62 + 6 files changed, 62 insertions(+), 1509 deletions(-) delete mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.cpp delete mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.h delete mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTDefs.h delete mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.cpp delete mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.h rename ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/{ecmcPluginFFT.c => ecmcPluginSocketCAN.c} (91%) diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.cpp deleted file mode 100644 index 7456600..0000000 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.cpp +++ /dev/null @@ -1,1057 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2019 European Spallation Source ERIC -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcFFT.cpp -* -* Created on: Mar 22, 2020 -* Author: anderssandstrom -* Credits to https://github.com/sgreg/dynamic-loading -* -\*************************************************************************/ - -// 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_PPDATA "preprocdata" -#define ECMC_PLUGIN_ASYN_FFT_AMP "fftamplitude" -#define ECMC_PLUGIN_ASYN_FFT_MODE "mode" -#define ECMC_PLUGIN_ASYN_FFT_STAT "status" -#define ECMC_PLUGIN_ASYN_FFT_SOURCE "source" -#define ECMC_PLUGIN_ASYN_FFT_TRIGG "trigg" -#define ECMC_PLUGIN_ASYN_FFT_X_FREQS "fftxaxis" -#define ECMC_PLUGIN_ASYN_NFFT "nfft" -#define ECMC_PLUGIN_ASYN_RATE "samplerate" -#define ECMC_PLUGIN_ASYN_BUFF_ID "buffid" - - -#include -#include "ecmcFFT.h" -#include "ecmcPluginClient.h" -#include "ecmcAsynPortDriver.h" -#include "ecmcAsynPortDriverUtils.h" -#include "epicsThread.h" - - -// New data callback from ecmc -static int printMissingObjError = 1; - -/** This callback will not be used (sample data inteface is used instead to get an stable sample freq) - since the callback is called when data is updated it might */ -void f_dataUpdatedCallback(uint8_t* data, size_t size, ecmcEcDataType dt, void* obj) { - if(!obj) { - if(printMissingObjError){ - printf("%s/%s:%d: Error: Callback object NULL.. Data will not be added to buffer.\n", - __FILE__, __FUNCTION__, __LINE__); - printMissingObjError = 0; - return; - } - } - ecmcFFT * fftObj = (ecmcFFT*)obj; - - // Call the correct fft object with new data - fftObj->dataUpdatedCallback(data,size,dt); -} - -void f_worker(void *obj) { - if(!obj) { - printf("%s/%s:%d: Error: Worker thread FFT object NULL..\n", - __FILE__, __FUNCTION__, __LINE__); - return; - } - ecmcFFT * fftObj = (ecmcFFT*)obj; - fftObj->doCalcWorker(); -} - -/** ecmc FFT class - * This object can throw: - * - bad_alloc - * - invalid_argument - * - runtime_error -*/ -ecmcFFT::ecmcFFT(int fftIndex, // index of this object (if several is created) - char* configStr, - char* portName) - : asynPortDriver(portName, - 1, /* maxAddr */ - asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | - asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | - asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask | - asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */ - asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | - asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | - asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask | - asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */ - ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/ - 1, /* Autoconnect */ - 0, /* Default priority */ - 0) /* Default stack size */ - { - cfgDataSourceStr_ = NULL; - rawDataBuffer_ = NULL; - dataItem_ = NULL; - dataItemInfo_ = NULL; - fftDouble_ = NULL; - status_ = NO_STAT; - elementsInBuffer_ = 0; - fftWaitingForCalc_= 0; - destructs_ = 0; - callbackHandle_ = -1; - objectId_ = fftIndex; - scale_ = 1.0; - triggOnce_ = 0; - cycleCounter_ = 0; - ignoreCycles_ = 0; - dataSourceLinked_ = 0; - - // Asyn - asynEnableId_ = -1; // Enable/disable acq./calcs - asynRawDataId_ = -1; // Raw data (input) array (double) - asynPPDataId_ = -1; // Pre-processed data array (double) - asynFFTAmpId_ = -1; // FFT amplitude array (double) - asynFFTModeId_ = -1; // FFT mode (cont/trigg) - asynFFTStatId_ = -1; // FFT status (no_stat/idle/acq/calc) - asynSourceId_ = -1; // SOURCE - asynTriggId_ = -1; // Trigg new measurement - asynFFTXAxisId_ = -1; // FFT X-axis frequencies - asynNfftId_ = -1; // Nfft - asynSRateId_ = -1; // Sample rate Hz - asynElementsInBuffer_= -1; - - ecmcSampleRateHz_ = getEcmcSampleRate(); - cfgFFTSampleRateHz_ = ecmcSampleRateHz_; - cfgDataSampleRateHz_ = ecmcSampleRateHz_; - - // Config defaults - cfgDbgMode_ = 0; - cfgNfft_ = ECMC_PLUGIN_DEFAULT_NFFT; // samples in fft (must be n^2) - cfgDcRemove_ = 0; - cfgLinRemove_ = 0; - //cfgApplyScale_ = 1; // Scale as default to get correct amplitude in fft - cfgEnable_ = 0; // start disabled (enable over asyn) - cfgMode_ = TRIGG; - cfgScale_ = 1.0; - - parseConfigStr(configStr); // Assigns all configs - // Check valid nfft - if(cfgNfft_ <= 0) { - throw std::out_of_range("NFFT must be > 0 and even N^2."); - } - - // Check valid sample rate - if(cfgFFTSampleRateHz_ <= 0) { - throw std::out_of_range("FFT Invalid sample rate"); - } - if(cfgFFTSampleRateHz_ > ecmcSampleRateHz_) { - printf("Warning FFT sample rate faster than ecmc rate. FFT rate will be set to ecmc rate.\n"); - cfgFFTSampleRateHz_ = ecmcSampleRateHz_; - } - - // Se if any data update cycles should be ignored - // example ecmc 1000Hz, fft 100Hz then ignore 9 cycles (could be strange if not multiples) - ignoreCycles_ = ecmcSampleRateHz_ / cfgFFTSampleRateHz_ -1; - - // set scale factor - scale_ = 1.0 / ((double)cfgNfft_); // sqrt((double)cfgNfft_); - - // Allocate buffers - rawDataBuffer_ = new double[cfgNfft_]; // Raw input data (real) - prepProcDataBuffer_ = new double[cfgNfft_]; // Data for preprocessing - fftBufferInput_ = new std::complex[cfgNfft_]; // FFT input (complex) - fftBufferResult_ = new std::complex[cfgNfft_]; // FFT result (complex) - fftBufferResultAmp_ = new double[cfgNfft_ / 2 + 1]; // FFT result amplitude (real) - fftBufferXAxis_ = new double[cfgNfft_ / 2 + 1]; // FFT x axis with freqs - clearBuffers(); - - // Allocate KissFFT - fftDouble_ = new kissfft(cfgNfft_,false); - - // Create worker thread - std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_); - if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker, this) == NULL) { - throw std::runtime_error("Error: Failed create worker thread."); - } - - initAsyn(); -} - -ecmcFFT::~ecmcFFT() { - // kill worker - destructs_ = 1; // maybe need todo in other way.. - doCalcEvent_.signal(); - - if(rawDataBuffer_) { - delete[] rawDataBuffer_; - } - - if(prepProcDataBuffer_) { - delete[] prepProcDataBuffer_; - } - - // De register callback when unload - if(callbackHandle_ >= 0) { - dataItem_->deregDataUpdatedCallback(callbackHandle_); - } - if(cfgDataSourceStr_) { - free(cfgDataSourceStr_); - } - if(fftDouble_) { - delete fftDouble_; - } - if (fftBufferInput_){ - delete[] fftBufferInput_; - } -} - -void ecmcFFT::parseConfigStr(char *configStr) { - - // check config parameters - if (configStr && configStr[0]) { - char *pOptions = strdup(configStr); - char *pThisOption = pOptions; - char *pNextOption = pOptions; - - while (pNextOption && pNextOption[0]) { - pNextOption = strchr(pNextOption, ';'); - if (pNextOption) { - *pNextOption = '\0'; /* Terminate */ - pNextOption++; /* Jump to (possible) next */ - } - - // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0) - if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD); - cfgDbgMode_ = atoi(pThisOption); - } - - // 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 (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 (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_RM_DC_OPTION_CMD (1/0) - else if (!strncmp(pThisOption, ECMC_PLUGIN_RM_DC_OPTION_CMD, strlen(ECMC_PLUGIN_RM_DC_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_RM_DC_OPTION_CMD); - cfgDcRemove_ = atoi(pThisOption); - } - - // ECMC_PLUGIN_RM_LIN_OPTION_CMD (1/0) - else if (!strncmp(pThisOption, ECMC_PLUGIN_RM_LIN_OPTION_CMD, strlen(ECMC_PLUGIN_RM_LIN_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_RM_LIN_OPTION_CMD); - cfgLinRemove_ = atoi(pThisOption); - } - - // 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; - } - } - - // ECMC_PLUGIN_RATE_OPTION_CMD rate in HZ - else if (!strncmp(pThisOption, ECMC_PLUGIN_RATE_OPTION_CMD, strlen(ECMC_PLUGIN_RATE_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_RATE_OPTION_CMD); - cfgFFTSampleRateHz_ = atof(pThisOption); - } - - // ECMC_PLUGIN_SCALE_OPTION_CMD rate in HZ - else if (!strncmp(pThisOption, ECMC_PLUGIN_SCALE_OPTION_CMD, strlen(ECMC_PLUGIN_SCALE_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_SCALE_OPTION_CMD); - cfgScale_ = atof(pThisOption); - } - - pThisOption = pNextOption; - } - free(pOptions); - } - - // Data source must be defined... - if(!cfgDataSourceStr_) { - throw std::invalid_argument( "Data source not defined."); - } -} - -void ecmcFFT::connectToDataSource() { - /* Check if already linked (one call to enterRT per loaded FFT lib (FFT object)) - But link should only happen once!!*/ - if( dataSourceLinked_ ) { - return; - } - - // Get dataItem - dataItem_ = (ecmcDataItem*) getEcmcDataItem(cfgDataSourceStr_); - if(!dataItem_) { - throw std::runtime_error( "Data item NULL." ); - } - - dataItemInfo_ = dataItem_->getDataItemInfo(); - - // Register data callback - callbackHandle_ = dataItem_->regDataUpdatedCallback(f_dataUpdatedCallback, this); - if (callbackHandle_ < 0) { - throw std::runtime_error( "Failed to register data source callback."); - } - - // Check data source - if( !dataTypeSupported(dataItem_->getEcmcDataType()) ) { - throw std::invalid_argument( "Data type not supported." ); - } - - // Add oversampling - cfgDataSampleRateHz_ = cfgFFTSampleRateHz_ * dataItem_->getEcmcDataSize()/dataItem_->getEcmcDataElementSize(); - setDoubleParam(asynSRateId_, cfgDataSampleRateHz_); - callParamCallbacks(); - - dataSourceLinked_ = 1; - updateStatus(IDLE); -} - -void ecmcFFT::dataUpdatedCallback(uint8_t* data, - size_t size, - ecmcEcDataType dt) { - - if(fftWaitingForCalc_) { - return; - } - // No buffer or full or not enabled - if(!rawDataBuffer_ || !cfgEnable_) { - return; - } - - // See if data should be ignored - if(cycleCounter_ < ignoreCycles_) { - cycleCounter_++; - return; // ignore this callback - } - - cycleCounter_ = 0; - - if (cfgMode_ == TRIGG && !triggOnce_ ) { - updateStatus(IDLE); - return; // Wait for trigger from plc or asyn - } - - if(cfgDbgMode_) { - printEcDataArray(data, size, dt, objectId_); - - if(elementsInBuffer_ == cfgNfft_) { - printf("Buffer full (%zu elements appended).\n",elementsInBuffer_); - } - } - - if(elementsInBuffer_ >= cfgNfft_) { - //Buffer full - if(!fftWaitingForCalc_){ - // Perform calcs - updateStatus(CALC); - fftWaitingForCalc_ = 1; - doCalcEvent_.signal(); // let worker start - } - return; - } - - updateStatus(ACQ); - - size_t dataElementSize = getEcDataTypeByteSize(dt); - - uint8_t *pData = data; - for(unsigned int i = 0; i < size / dataElementSize; ++i) { - //printf("dataElementSize=%d, size=%d\n",dataElementSize,size); - switch(dt) { - case ECMC_EC_U8: - addDataToBuffer((double)getUint8(pData)); - break; - case ECMC_EC_S8: - addDataToBuffer((double)getInt8(pData)); - break; - case ECMC_EC_U16: - addDataToBuffer((double)getUint16(pData)); - break; - case ECMC_EC_S16: - addDataToBuffer((double)getInt16(pData)); - break; - case ECMC_EC_U32: - addDataToBuffer((double)getUint32(pData)); - break; - case ECMC_EC_S32: - addDataToBuffer((double)getInt32(pData)); - break; - case ECMC_EC_U64: - addDataToBuffer((double)getUint64(pData)); - break; - case ECMC_EC_S64: - addDataToBuffer((double)getInt64(pData)); - break; - case ECMC_EC_F32: - addDataToBuffer((double)getFloat32(pData)); - break; - case ECMC_EC_F64: - addDataToBuffer((double)getFloat64(pData)); - break; - default: - break; - } - - pData += dataElementSize; - } -} - -void ecmcFFT::addDataToBuffer(double data) { - if(rawDataBuffer_ && (elementsInBuffer_ < cfgNfft_) ) { - rawDataBuffer_[elementsInBuffer_] = data* cfgScale_; - prepProcDataBuffer_[elementsInBuffer_] = data *cfgScale_; - } - elementsInBuffer_ ++; -} - -void ecmcFFT::clearBuffers() { - memset(rawDataBuffer_, 0, cfgNfft_ * sizeof(double)); - memset(prepProcDataBuffer_, 0, cfgNfft_ * sizeof(double)); - memset(fftBufferResultAmp_, 0, (cfgNfft_ / 2 + 1) * sizeof(double)); - memset(fftBufferXAxis_, 0, (cfgNfft_ / 2 + 1) * sizeof(double)); - for(unsigned int i = 0; i < cfgNfft_; ++i) { - fftBufferResult_[i].real(0); - fftBufferResult_[i].imag(0); - fftBufferInput_[i].real(0); - fftBufferInput_[i].imag(0); - } - elementsInBuffer_ = 0; -} - -void ecmcFFT::calcFFT() { - // move pre-processed data to fft input buffer - for(unsigned int i = 0; i < cfgNfft_; ++i) { - fftBufferInput_[i].real(prepProcDataBuffer_[i]); - fftBufferInput_[i].imag(0); - } - - // Do fft - fftDouble_->transform(fftBufferInput_, fftBufferResult_); -} - -void ecmcFFT::scaleFFT() { - // Always scale - //if(!cfgApplyScale_) { - // return; - //} - - for(unsigned int i = 0 ; i < cfgNfft_ ; ++i ) { - fftBufferResult_[i] = fftBufferResult_[i] * scale_; - } -} - -void ecmcFFT::calcFFTAmp() { - for(unsigned int i = 0 ; i < cfgNfft_ / 2 + 1 ; ++i ) { - fftBufferResultAmp_[i] = std::abs(fftBufferResult_[i]); - } -} - -// Should be enough todo once -void ecmcFFT::calcFFTXAxis() { - //fill x axis buffer with freqs - - double freq = 0; - size_t size = dataItemInfo_->dataSize; - - double deltaFreq = cfgDataSampleRateHz_* ((double)size / - (double)dataItemInfo_->dataElementSize) / ((double)(cfgNfft_)); - for(unsigned int i = 0; i < (cfgNfft_ / 2 + 1); ++i) { - fftBufferXAxis_[i] = freq; - freq = freq + deltaFreq; - } -} - -void ecmcFFT::removeDCOffset() { - if(!cfgDcRemove_) { - return; - } - - // calc average of preprocess buffer data - double sum = 0; - for(unsigned int i = 0; i < cfgNfft_; ++i ) { - sum += prepProcDataBuffer_[i]; - } - double avg = sum / ((double)cfgNfft_); - for(unsigned int i = 0; i < cfgNfft_; ++i ) { - prepProcDataBuffer_[i] = (prepProcDataBuffer_[i]-avg); - } -} - -void ecmcFFT::removeLin() { - if(!cfgLinRemove_) { - return; - } - - double k=0; - double m=0; - // calc least square (best fit of line) - if(leastSquare(cfgNfft_,prepProcDataBuffer_,&k,&m)) { - printf("%s/%s:%d: Error: " ECMC_PLUGIN_RM_LIN_OPTION_CMD " failed, divison by 0. Data will not be processed with the option/configuration.\n", - __FILE__, __FUNCTION__, __LINE__); - return; - } - - // remove linear component (now we have k and m (y=k*x+m)) - for(unsigned int x = 0; x < cfgNfft_; ++x ) { - prepProcDataBuffer_[x] = prepProcDataBuffer_[x] - (k*x + m); - } -} - -void ecmcFFT::printEcDataArray(uint8_t* data, - size_t size, - ecmcEcDataType dt, - int objId) { - printf("fft id: %d, data: ",objId); - - size_t dataElementSize = getEcDataTypeByteSize(dt); - - uint8_t *pData = data; - for(unsigned int i = 0; i < size / dataElementSize; ++i) { - switch(dt) { - case ECMC_EC_U8: - printf("%hhu\n",getUint8(pData)); - break; - case ECMC_EC_S8: - printf("%hhd\n",getInt8(pData)); - break; - case ECMC_EC_U16: - printf("%hu\n",getUint16(pData)); - break; - case ECMC_EC_S16: - printf("%hd\n",getInt16(pData)); - break; - case ECMC_EC_U32: - printf("%u\n",getUint32(pData)); - break; - case ECMC_EC_S32: - printf("%d\n",getInt32(pData)); - break; - case ECMC_EC_U64: - printf("%" PRIu64 "\n",getInt64(pData)); - break; - case ECMC_EC_S64: - printf("%" PRId64 "\n",getInt64(pData)); - break; - case ECMC_EC_F32: - printf("%f\n",getFloat32(pData)); - break; - case ECMC_EC_F64: - printf("%lf\n",getFloat64(pData)); - break; - default: - break; - } - - pData += dataElementSize; - } -} - -void ecmcFFT::printComplexArray(std::complex* fftBuff, - size_t elements, - int objId) { - printf("fft id: %d, results: \n",objId); - for(unsigned int i = 0 ; i < elements ; ++i ) { - printf("%d: %lf\n", i, std::abs(fftBuff[i])); - } -} - -int ecmcFFT::dataTypeSupported(ecmcEcDataType dt) { - - switch(dt) { - case ECMC_EC_NONE: - return 0; - break; - case ECMC_EC_B1: - return 0; - break; - case ECMC_EC_B2: - return 0; - break; - case ECMC_EC_B3: - return 0; - break; - case ECMC_EC_B4: - return 0; - break; - default: - return 1; - break; - } - return 1; -} - -uint8_t ecmcFFT::getUint8(uint8_t* data) { - return *data; -} - -int8_t ecmcFFT::getInt8(uint8_t* data) { - int8_t* p=(int8_t*)data; - return *p; -} - -uint16_t ecmcFFT::getUint16(uint8_t* data) { - uint16_t* p=(uint16_t*)data; - return *p; -} - -int16_t ecmcFFT::getInt16(uint8_t* data) { - int16_t* p=(int16_t*)data; - return *p; -} - -uint32_t ecmcFFT::getUint32(uint8_t* data) { - uint32_t* p=(uint32_t*)data; - return *p; -} - -int32_t ecmcFFT::getInt32(uint8_t* data) { - int32_t* p=(int32_t*)data; - return *p; -} - -uint64_t ecmcFFT::getUint64(uint8_t* data) { - uint64_t* p=(uint64_t*)data; - return *p; -} - -int64_t ecmcFFT::getInt64(uint8_t* data) { - int64_t* p=(int64_t*)data; - return *p; -} - -float ecmcFFT::getFloat32(uint8_t* data) { - float* p=(float*)data; - return *p; -} - -double ecmcFFT::getFloat64(uint8_t* data) { - double* p=(double*)data; - return *p; -} - -size_t ecmcFFT::getEcDataTypeByteSize(ecmcEcDataType dt){ - switch(dt) { - case ECMC_EC_NONE: - return 0; - break; - - case ECMC_EC_B1: - return 1; - break; - - case ECMC_EC_B2: - return 1; - break; - - case ECMC_EC_B3: - return 1; - break; - - case ECMC_EC_B4: - return 1; - break; - - case ECMC_EC_U8: - return 1; - break; - - case ECMC_EC_S8: - return 1; - break; - - case ECMC_EC_U16: - return 2; - break; - - case ECMC_EC_S16: - return 2; - break; - - case ECMC_EC_U32: - return 4; - break; - - case ECMC_EC_S32: - return 4; - break; - - case ECMC_EC_U64: - return 8; - break; - - case ECMC_EC_S64: - return 8; - break; - - case ECMC_EC_F32: - return 4; - break; - - case ECMC_EC_F64: - return 8; - break; - - default: - return 0; - break; - } - - return 0; -} - -void ecmcFFT::initAsyn() { - - // Add enable "plugin.fft%d.enable" - std::string paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_ENABLE; - - if( createParam(0, paramName.c_str(), asynParamInt32, &asynEnableId_) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter enable"); - } - setIntegerParam(asynEnableId_, cfgEnable_); - - // Add rawdata "plugin.fft%d.rawdata" - paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_RAWDATA; - - if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynRawDataId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter rawdata"); - } - doCallbacksFloat64Array(rawDataBuffer_, cfgNfft_, asynRawDataId_,0); - - // Add rawdata "plugin.fft%d.preprocdata" - paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_PPDATA; - - if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynPPDataId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter preprocdata"); - } - doCallbacksFloat64Array(prepProcDataBuffer_, cfgNfft_, asynPPDataId_,0); - - - - // Add fft amplitude "plugin.fft%d.fftamplitude" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_FFT_AMP; - - if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynFFTAmpId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter fftamplitude"); - } - doCallbacksFloat64Array(fftBufferResultAmp_, cfgNfft_/2+1, asynFFTAmpId_,0); - - // Add fft "plugin.fft%d.mode" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_FFT_MODE; - - if( createParam(0, paramName.c_str(), asynParamInt32, &asynFFTModeId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter mode"); - } - setIntegerParam(asynFFTModeId_, (epicsInt32)cfgMode_); - - // Add fft "plugin.fft%d.status" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_FFT_STAT; - - if( createParam(0, paramName.c_str(), asynParamInt32, &asynFFTStatId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter status"); - } - setIntegerParam(asynFFTStatId_, (epicsInt32)status_); - - // Add fft "plugin.fft%d.source" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_FFT_SOURCE; - - if( createParam(0, paramName.c_str(), asynParamInt8Array, &asynSourceId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter source"); - } - doCallbacksInt8Array(cfgDataSourceStr_, strlen(cfgDataSourceStr_), asynSourceId_,0); - - // Add fft "plugin.fft%d.trigg" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_FFT_TRIGG; - - if( createParam(0, paramName.c_str(), asynParamInt32, &asynTriggId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter trigg"); - } - setIntegerParam(asynTriggId_, (epicsInt32)triggOnce_); - - // Add fft "plugin.fft%d.fftxaxis" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_FFT_X_FREQS; - - if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynFFTXAxisId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter xaxisfreqs"); - } - doCallbacksFloat64Array(fftBufferXAxis_,cfgNfft_ / 2 + 1, asynFFTXAxisId_,0); - - // Add fft "plugin.fft%d.nfft" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_NFFT; - - if( createParam(0, paramName.c_str(), asynParamInt32, &asynNfftId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter nfft"); - } - setIntegerParam(asynNfftId_, (epicsInt32)cfgNfft_); - - // Add fft "plugin.fft%d.rate" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_RATE; - - if( createParam(0, paramName.c_str(), asynParamFloat64, &asynSRateId_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter rate"); - } - setDoubleParam(asynSRateId_, cfgDataSampleRateHz_); - - // Add fft "plugin.fft%d.buffid" - paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_BUFF_ID; - - if( createParam(0, paramName.c_str(), asynParamInt32, &asynElementsInBuffer_ ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter trigg"); - } - setIntegerParam(asynElementsInBuffer_, (epicsInt32)elementsInBuffer_); - - // Update integers - callParamCallbacks(); -} - -// 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; - setIntegerParam(asynEnableId_, enable); -} - -void ecmcFFT::triggFFT() { - clearBuffers(); - triggOnce_ = 1; - setIntegerParam(asynTriggId_,0); -} - -void ecmcFFT::setModeFFT(FFT_MODE mode) { - cfgMode_ = mode; - setIntegerParam(asynFFTModeId_,(epicsInt32)mode); -} - -FFT_STATUS ecmcFFT::getStatusFFT() { - return status_; -} - -void ecmcFFT::updateStatus(FFT_STATUS status) { - status_ = status; - setIntegerParam(asynFFTStatId_,(epicsInt32) status); - - setIntegerParam(asynElementsInBuffer_, (epicsInt32)elementsInBuffer_); - - callParamCallbacks(); -} - -// Called from low prio worker thread. Makes the hard work -void ecmcFFT::doCalcWorker() { - - while(true) { - doCalcEvent_.wait(); - if(destructs_) { - break; - } - // Pre-process - removeDCOffset(); // Remove dc on rawdata - removeLin(); // Remove fitted line - // Process - calcFFT(); // FFT cacluation - // Post-process - scaleFFT(); // Scale FFT - calcFFTAmp(); // Calculate amplitude from complex - calcFFTXAxis(); // Calculate x axis - - doCallbacksFloat64Array(rawDataBuffer_, cfgNfft_, asynRawDataId_, 0); - doCallbacksFloat64Array(prepProcDataBuffer_, cfgNfft_, asynPPDataId_, 0); - doCallbacksFloat64Array(fftBufferResultAmp_,cfgNfft_/2+1, asynFFTAmpId_, 0); - doCallbacksFloat64Array(fftBufferXAxis_, cfgNfft_/2+1, asynFFTXAxisId_,0); - callParamCallbacks(); - if(cfgDbgMode_){ - printComplexArray(fftBufferResult_, - cfgNfft_, - objectId_); - printEcDataArray((uint8_t*)rawDataBuffer_, - cfgNfft_*sizeof(double), - ECMC_EC_F64, - objectId_); - } - - clearBuffers(); - triggOnce_ = 0; // Wait for next trigger if in trigg mode - setIntegerParam(asynTriggId_,triggOnce_); - fftWaitingForCalc_ = 0; - } -} - -asynStatus ecmcFFT::writeInt32(asynUser *pasynUser, epicsInt32 value) { - int function = pasynUser->reason; - if( function == asynEnableId_ ) { - cfgEnable_ = value; - return asynSuccess; - } else if( function == asynFFTModeId_){ - cfgMode_ = (FFT_MODE)value; - return asynSuccess; - } else if( function == asynTriggId_){ - triggOnce_ = value > 0; - return asynSuccess; - } - return asynError; -} - -asynStatus ecmcFFT::readInt32(asynUser *pasynUser, epicsInt32 *value) { - int function = pasynUser->reason; - if( function == asynEnableId_ ) { - *value = cfgEnable_; - return asynSuccess; - } else if( function == asynFFTModeId_ ){ - *value = cfgMode_; - return asynSuccess; - } else if( function == asynTriggId_ ){ - *value = triggOnce_; - return asynSuccess; - }else if( function == asynFFTStatId_ ){ - *value = (epicsInt32)status_; - return asynSuccess; - }else if( function == asynNfftId_ ){ - *value = (epicsInt32)cfgNfft_; - return asynSuccess; - }else if( function == asynElementsInBuffer_){ - *value = (epicsInt32)elementsInBuffer_; - return asynSuccess; - } - - return asynError; -} - -asynStatus ecmcFFT::readFloat64Array(asynUser *pasynUser, epicsFloat64 *value, - size_t nElements, size_t *nIn) { - int function = pasynUser->reason; - if( function == asynRawDataId_ ) { - unsigned int ncopy = cfgNfft_; - if(nElements < ncopy) { - ncopy = nElements; - } - memcpy (value, rawDataBuffer_, ncopy); - *nIn = ncopy; - return asynSuccess; - } else if( function == asynPPDataId_) { - unsigned int ncopy = cfgNfft_; - if(nElements < ncopy) { - ncopy = nElements; - } - memcpy (value, prepProcDataBuffer_, ncopy); - *nIn = ncopy; - return asynSuccess; - } else if( function == asynFFTXAxisId_ ) { - unsigned int ncopy = cfgNfft_/ 2 + 1; - if(nElements < ncopy) { - ncopy = nElements; - } - memcpy (value, fftBufferXAxis_, ncopy); - *nIn = ncopy; - return asynSuccess; - } if( function == asynFFTAmpId_ ) { - unsigned int ncopy = cfgNfft_/ 2 + 1; - if(nElements < ncopy) { - ncopy = nElements; - } - memcpy (value, fftBufferResultAmp_, ncopy); - *nIn = ncopy; - return asynSuccess; - } - - *nIn = 0; - return asynError; -} - -asynStatus ecmcFFT::readInt8Array(asynUser *pasynUser, epicsInt8 *value, - size_t nElements, size_t *nIn) { - int function = pasynUser->reason; - if( function == asynSourceId_ ) { - unsigned int ncopy = strlen(cfgDataSourceStr_); - if(nElements < ncopy) { - ncopy = nElements; - } - memcpy (value, cfgDataSourceStr_, ncopy); - *nIn = ncopy; - return asynSuccess; - } - - *nIn = 0; - return asynError; -} - -asynStatus ecmcFFT::readFloat64(asynUser *pasynUser, epicsFloat64 *value) { - int function = pasynUser->reason; - if( function == asynSRateId_ ) { - *value = cfgDataSampleRateHz_; - return asynSuccess; - } - - return asynError; -} - -/* y = k*x+m */ -int ecmcFFT::leastSquare(int n, const double y[], double* k, double* m){ - double sumx = 0.0; - double sumx2 = 0.0; - double sumxy = 0.0; - double sumy = 0.0; - double sumy2 = 0.0; - - for (int x = 0; x < n; ++x){ - //simulate x by just index - sumx += x; - sumx2 += x * x; - sumxy += x * y[x]; - sumy += y[x]; - sumy2 += y[x] * y[x]; - } - - double denom = (n * sumx2 - sumx * sumx); - if (denom == 0) { - // Cannot dive by 0.. something wrong.. - *k = 0; - *m = 0; - return 1; // Error - } - - *k = (n * sumxy - sumx * sumy) / denom; - *m = (sumy * sumx2 - sumx * sumxy) / denom; - return 0; -} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.h deleted file mode 100644 index 3f0783f..0000000 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFT.h +++ /dev/null @@ -1,153 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2019 European Spallation Source ERIC -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcFFT.h -* -* Created on: Mar 22, 2020 -* Author: anderssandstrom -* -\*************************************************************************/ -#ifndef ECMC_FFT_H_ -#define ECMC_FFT_H_ - -#include -#include "ecmcDataItem.h" -#include "ecmcAsynPortDriver.h" -#include "ecmcFFTDefs.h" -#include "inttypes.h" -#include -#include "kissfft/kissfft.hh" - -class ecmcFFT : public asynPortDriver { - public: - - /** ecmc FFT class - * This object can throw: - * - bad_alloc - * - invalid_argument - * - runtime_error - * - out_of_range - */ - ecmcFFT(int fftIndex, // index of this object - char* configStr, - char* portName); - ~ecmcFFT(); - - // Add data to buffer (called from "external" callback) - void dataUpdatedCallback(uint8_t* data, - size_t size, - ecmcEcDataType dt); - // Call just before realtime because then all data sources should be available - void connectToDataSource(); - void setEnable(int enable); - void setModeFFT(FFT_MODE mode); - FFT_STATUS getStatusFFT(); - void clearBuffers(); - void triggFFT(); - void doCalcWorker(); // Called from worker thread calc the results - virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); - virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); - virtual asynStatus readFloat64Array(asynUser *pasynUser, epicsFloat64 *value, - size_t nElements, size_t *nIn); - virtual asynStatus readInt8Array(asynUser *pasynUser, epicsInt8 *value, - size_t nElements, size_t *nIn); - virtual asynStatus readFloat64(asynUser *pasynUser, epicsFloat64 *value); - - - private: - void parseConfigStr(char *configStr); - void addDataToBuffer(double data); - void calcFFT(); - void scaleFFT(); - void calcFFTAmp(); - void calcFFTXAxis(); - void removeDCOffset(); - void removeLin(); - void initAsyn(); - void updateStatus(FFT_STATUS status); // Also updates asynparam - static int dataTypeSupported(ecmcEcDataType dt); - - ecmcDataItem *dataItem_; - ecmcDataItemInfo *dataItemInfo_; - ecmcAsynPortDriver *asynPort_; - kissfft* fftDouble_; - double* rawDataBuffer_; // Input data (real) - double* prepProcDataBuffer_; // Preprocessed data (real) - std::complex* fftBufferInput_; // Result (complex) - std::complex* fftBufferResult_; // Result (complex) - double* fftBufferResultAmp_; // Resulting amplitude (abs of fftBufferResult_) - double* fftBufferXAxis_; // FFT x axis with freqs - size_t elementsInBuffer_; - double ecmcSampleRateHz_; - int dataSourceLinked_; // To avoid link several times - // ecmc callback handle for use when deregister at unload - int callbackHandle_; - int fftWaitingForCalc_; - int destructs_; - int objectId_; // Unique object id - int triggOnce_; - int cycleCounter_; - int ignoreCycles_; - double scale_; // Config: Data set size - FFT_STATUS status_; // Status/state (NO_STAT, IDLE, ACQ, CALC) - - // 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) - int cfgLinRemove_; // Config: remove linear componet (by least square) - size_t cfgNfft_; // Config: Data set size - int cfgEnable_; // Config: Enable data acq./calc. - FFT_MODE cfgMode_; // Config: Mode continous or triggered. - double cfgFFTSampleRateHz_; // Config: Sample rate (defaults to ecmc rate) - double cfgScale_; - double cfgDataSampleRateHz_; // Config: Sample for data - - // Asyn - int asynEnableId_; // Enable/disable acq./calcs - int asynRawDataId_; // Raw data (input) array (double) - int asynPPDataId_; // Pre-processed data array (double) - int asynFFTAmpId_; // FFT amplitude array (double) - int asynFFTModeId_; // FFT mode (cont/trigg) - int asynFFTStatId_; // FFT status (no_stat/idle/acq/calc) - int asynSourceId_; // SOURCE - int asynTriggId_; // Trigg new measurement - int asynFFTXAxisId_; // FFT X-axis frequencies - int asynNfftId_; // NFFT - int asynSRateId_; // Sample rate - int asynElementsInBuffer_; // Current buffer index - - // Thread related - epicsEvent doCalcEvent_; - - - // 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); - static int16_t getInt16(uint8_t* data); - static uint32_t getUint32(uint8_t* data); - static int32_t getInt32(uint8_t* data); - static uint64_t getUint64(uint8_t* data); - static int64_t getInt64(uint8_t* data); - static float getFloat32(uint8_t* data); - static double getFloat64(uint8_t* data); - static size_t getEcDataTypeByteSize(ecmcEcDataType dt); - static void printEcDataArray(uint8_t* data, - size_t size, - ecmcEcDataType dt, - int objId); - static void printComplexArray(std::complex* fftBuff, - size_t elements, - int objId); - static std::string to_string(int value); - static int leastSquare(int n, - const double y[], - double* k, - double* m); // y=kx+m -}; - -#endif /* ECMC_FFT_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTDefs.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTDefs.h deleted file mode 100644 index 8e6d4cd..0000000 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTDefs.h +++ /dev/null @@ -1,54 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2019 European Spallation Source ERIC -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcFFTDefs.h -* -* Created on: Mar 22, 2020 -* Author: anderssandstrom -* Credits to https://github.com/sgreg/dynamic-loading -* -\*************************************************************************/ - -#ifndef ECMC_FFT_DEFS_H_ -#define ECMC_FFT_DEFS_H_ - -// Options -#define ECMC_PLUGIN_DBG_PRINT_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_RM_DC_OPTION_CMD "RM_DC=" -#define ECMC_PLUGIN_ENABLE_OPTION_CMD "ENABLE=" -#define ECMC_PLUGIN_RATE_OPTION_CMD "RATE=" -#define ECMC_PLUGIN_RM_LIN_OPTION_CMD "RM_LIN=" -#define ECMC_PLUGIN_SCALE_OPTION_CMD "SCALE=" - - -// CONT, TRIGG -#define ECMC_PLUGIN_MODE_OPTION_CMD "MODE=" -#define ECMC_PLUGIN_MODE_CONT_OPTION "CONT" -#define ECMC_PLUGIN_MODE_TRIGG_OPTION "TRIGG" - -typedef enum FFT_MODE{ - NO_MODE = 0, - CONT = 1, - TRIGG = 2, -} FFT_MODE; - -typedef enum FFT_STATUS{ - NO_STAT = 0, - IDLE = 1, // Doing nothing, waiting for trigg - ACQ = 2, // Acquireing data - CALC = 3, // Calc FFT -} FFT_STATUS; - -/** 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 4096 - -#endif /* ECMC_FFT_DEFS_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.cpp deleted file mode 100644 index bf07ab0..0000000 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2019 European Spallation Source ERIC -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcFFTWrap.cpp -* -* Created on: Mar 22, 2020 -* Author: anderssandstrom -* Credits to https://github.com/sgreg/dynamic-loading -* -\*************************************************************************/ - -// Needed to get headers in ecmc right... -#define ECMC_IS_PLUGIN - -#include -#include -#include -#include "ecmcFFTWrap.h" -#include "ecmcFFT.h" -#include "ecmcFFTDefs.h" - -#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64 -#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.FFT" - -static std::vector ffts; -static int fftObjCounter = 0; -static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; - -int createFFT(char* configStr) { - - // create new ecmcFFT object - ecmcFFT* fft = NULL; - - // create asynport name for new object () - memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS); - snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS, - ECMC_PLUGIN_PORTNAME_PREFIX "%d", fftObjCounter); - try { - fft = new ecmcFFT(fftObjCounter, configStr, portNameBuffer); - } - catch(std::exception& e) { - if(fft) { - delete fft; - } - printf("Exception: %s. Plugin will unload.\n",e.what()); - return ECMC_PLUGIN_FFT_ERROR_CODE; - } - - ffts.push_back(fft); - fftObjCounter++; - - return 0; -} - -void deleteAllFFTs() { - for(std::vector::iterator pfft = ffts.begin(); pfft != ffts.end(); ++pfft) { - if(*pfft) { - delete (*pfft); - } - } -} - -int linkDataToFFTs() { - for(std::vector::iterator pfft = ffts.begin(); pfft != ffts.end(); ++pfft) { - if(*pfft) { - try { - (*pfft)->connectToDataSource(); - } - catch(std::exception& e) { - printf("Exception: %s. Plugin will unload.\n",e.what()); - return ECMC_PLUGIN_FFT_ERROR_CODE; - } - } - } - return 0; -} - -int enableFFT(int fftIndex, int enable) { - try { - ffts.at(fftIndex)->setEnable(enable); - } - catch(std::exception& e) { - printf("Exception: %s. FFT index out of range.\n",e.what()); - return ECMC_PLUGIN_FFT_ERROR_CODE; - } - return 0; -} - -int clearFFT(int fftIndex) { - try { - ffts.at(fftIndex)->clearBuffers(); - } - catch(std::exception& e) { - printf("Exception: %s. FFT index out of range.\n",e.what()); - return ECMC_PLUGIN_FFT_ERROR_CODE; - } - 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 modeFFT(int fftIndex, FFT_MODE mode) { - try { - ffts.at(fftIndex)->setModeFFT(mode); - } - catch(std::exception& e) { - printf("Exception: %s. FFT index out of range.\n",e.what()); - return ECMC_PLUGIN_FFT_ERROR_CODE; - } - return 0; -} - -FFT_STATUS statFFT(int fftIndex) { - try { - return ffts.at(fftIndex)->getStatusFFT(); - } - catch(std::exception& e) { - printf("Exception: %s. FFT index out of range.\n",e.what()); - return NO_STAT; - } - return NO_STAT; -} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.h deleted file mode 100644 index fe4650c..0000000 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcFFTWrap.h +++ /dev/null @@ -1,112 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2019 European Spallation Source ERIC -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcFFTWrap.h -* -* Created on: Mar 22, 2020 -* Author: anderssandstrom -* -\*************************************************************************/ -#ifndef ECMC_FFT_WRAP_H_ -#define ECMC_FFT_WRAP_H_ -#include "ecmcFFTDefs.h" - -# ifdef __cplusplus -extern "C" { -# endif // ifdef __cplusplus - -/** \brief Create new FFT object - * - * The plugin supports creation of multiple FFT objects\n - * (if loaded several times).\n - * The different fft are adressed by fftindex (in other functions below).\n - * The first loaded fft get index 0 and then increases for each load.\n - * This function call will create the custom asynparameters dedicated for this plugin.\ - * The configuration string needs to define a data source by:\n - * "SOURCE=;"\n - * Example:\n - * "SOURCE=ec0.s1.AI_1";\n - * \param[in] configStr Configuration string.\n - * - * \return 0 if success or otherwise an error code.\n - */ -int createFFT(char *configStr); - -/** \brief Enable/disable FFT object - * - * Enable/disable FFT object. If disabled no data will be acquired\n - * and no calculations will be made.\n - * \param[in] fftIndex Index of fft (first loaded fft have index 0 then increases)\n - * \param[in] enable enable/disable (1/0).\n - * - * \return 0 if success or otherwise an error code.\n - */ -int enableFFT(int fftIndex, int enable); - -/** \brief Clear FFT object\n - * - * Clears buffers. After this command the acquistion can start from scratch.\n - * \param[in] fftIndex Index of fft (first loaded fft have index 0 then increases)\n - * - * \return 0 if success or otherwise an error code.\n - */ -int clearFFT(int fftIndex); - -/** \brief Set mode of FFT object - * - * The FFT object can measure in two differnt modes:\n - * CONT(1) : Continious measurement (Acq data, calc, then Acq data ..)\n - * TRIGG(2): Measurements are triggered from plc or over asyn and is only done once (untill next trigger)\n - * \param[in] fftIndex Index of fft (first loaded fft have index 0 then increases)\n - * \param[in] mode Mode CONT(1) or TRIGG(2)\n - * - * \return 0 if success or otherwise an error code.\n - */ -int modeFFT(int fftIndex, FFT_MODE mode); - -/** \brief Trigger FFT object\n - * - * If in triggered mode a new measurment cycle is initiated (fft will be cleared first).\n - * \param[in] fftIndex Index of fft (first loaded fft have index 0 then increases)\n - * - * \return 0 if success or otherwise an error code.\n - */ -int triggFFT(int fftIndex); - -/** \brief Get status of FFT object - * - * The FFT object can be in different states:\n - * NO_STAT(0): Invalid state (something is most likely wrong)\n - * IDLE(1) : Waiting for trigger in triggered mode\n - * ACQ(2) : Acquiring data (filling data buffer)\n - * CALC(3) : Calculating FFT results\n - * \param[in] fftIndex Index of fft (first loaded fft have index 0 then increases)\n - * - * \return Status of fft (if index is out of range NO_STAT will be returned).\n - */ -FFT_STATUS statFFT(int fftIndex); - -/** \brief Link data to _all_ fft objects - * - * This tells the FFT lib to connect to ecmc to find it's data source.\n - * This function should be called just before entering realtime since then all\n - * data sources in ecmc will be definded (plc sources are compiled just before runtime\n - * so are only fist accesible now).\n - * \return 0 if success or otherwise an error code.\n - */ -int linkDataToFFTs(); - -/** \brief Deletes all created fft objects\n - * - * Should be called when destructs.\n - */ - -void deleteAllFFTs(); - -# ifdef __cplusplus -} -# endif // ifdef __cplusplus - -#endif /* ECMC_FFT_WRAP_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginFFT.c b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c similarity index 91% rename from ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginFFT.c rename to ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c index f0efe90..d06eba4 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginFFT.c +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c @@ -299,3 +299,65 @@ ecmc_plugin_register(pluginDataDef); # ifdef __cplusplus } # endif // ifdef __cplusplus + + + + + + + +/**/ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +int +main(void) +{ + int s; + int nbytes; + struct sockaddr_can addr; + struct can_frame frame; + struct ifreq ifr; + + const char *ifname = "vcan0"; + + if((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) == -1) { + perror("Error while opening socket"); + return -1; + } + + strcpy(ifr.ifr_name, ifname); + ioctl(s, SIOCGIFINDEX, &ifr); + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + printf("%s at index %d\n", ifname, ifr.ifr_ifindex); + + if(bind(s, (struct sockaddr *)&addr, sizeof(addr)) == -1) { + perror("Error in socket bind"); + return -2; + } + + frame.can_id = 0x123; + frame.can_dlc = 2; + frame.data[0] = 0x11; + frame.data[1] = 0x22; + + nbytes = write(s, &frame, sizeof(struct can_frame)); + + printf("Wrote %d bytes\n", nbytes); + + return 0; +}