880 lines
25 KiB
C++
880 lines
25 KiB
C++
/*************************************************************************\
|
|
* 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_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"
|
|
|
|
#include <sstream>
|
|
#include "ecmcFFT.h"
|
|
#include "ecmcPluginClient.h"
|
|
#include "ecmcAsynPortDriver.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;
|
|
|
|
// Asyn
|
|
asynEnableId_ = -1; // Enable/disable acq./calcs
|
|
asynRawDataId_ = -1; // Raw data (input) 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
|
|
|
|
ecmcSampleRateHz_ = getEcmcSampleRate();
|
|
cfgFFTSampleRateHz_ = ecmcSampleRateHz_;
|
|
|
|
// 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;
|
|
|
|
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)
|
|
fftBufferInput_ = new std::complex<double>[cfgNfft_]; // FFT input (complex)
|
|
fftBufferResult_ = new std::complex<double>[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<double>(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_;
|
|
}
|
|
// 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_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 (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);
|
|
}
|
|
|
|
pThisOption = pNextOption;
|
|
}
|
|
free(pOptions);
|
|
}
|
|
|
|
// Data source must be defined...
|
|
if(!cfgDataSourceStr_) {
|
|
throw std::invalid_argument( "Data source not defined.");
|
|
}
|
|
}
|
|
|
|
void ecmcFFT::connectToDataSource() {
|
|
// 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." );
|
|
}
|
|
|
|
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) {
|
|
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;
|
|
fftBufferInput_[elementsInBuffer_].real(data);
|
|
fftBufferInput_[elementsInBuffer_].imag(0);
|
|
}
|
|
elementsInBuffer_ ++;
|
|
}
|
|
|
|
void ecmcFFT::clearBuffers() {
|
|
memset(rawDataBuffer_, 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() {
|
|
fftDouble_->transform(fftBufferInput_, fftBufferResult_);
|
|
}
|
|
|
|
void ecmcFFT::scaleFFT() {
|
|
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;
|
|
double deltaFreq = ecmcSampleRateHz_* ((double)dataItemInfo_->dataSize /
|
|
(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 raw data
|
|
double sum = 0;
|
|
for(unsigned int i = 0; i < cfgNfft_; ++i ) {
|
|
sum += fftBufferInput_[i].real();
|
|
}
|
|
double avg = sum / ((double)cfgNfft_);
|
|
for(unsigned int i = 0; i < cfgNfft_; ++i ) {
|
|
fftBufferInput_[i].real(fftBufferInput_[i].real()-avg);
|
|
}
|
|
}
|
|
|
|
|
|
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<double>* 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 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, asynFFTXAxisId_,0);
|
|
|
|
// Add fft mode "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 mode "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 mode "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 mode "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 mode "plugin.fft%d.xaxisfreqs"
|
|
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);
|
|
|
|
// 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;
|
|
}
|
|
|
|
void ecmcFFT::triggFFT() {
|
|
clearBuffers();
|
|
triggOnce_ = 1;
|
|
}
|
|
|
|
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);
|
|
callParamCallbacks();
|
|
}
|
|
|
|
// Called from worker thread. Makes the hard work
|
|
void ecmcFFT::doCalcWorker() {
|
|
|
|
while(true) {
|
|
doCalcEvent_.wait();
|
|
if(destructs_) {
|
|
break;
|
|
}
|
|
|
|
removeDCOffset(); // Remove dc on rawdata
|
|
calcFFT(); // FFT cacluation
|
|
scaleFFT(); // Scale FFT
|
|
calcFFTAmp(); // Calculate amplitude from complex
|
|
calcFFTXAxis(); // Calculate x axis
|
|
|
|
setIntegerParam(asynTriggId_,triggOnce_);
|
|
doCallbacksFloat64Array(rawDataBuffer_, cfgNfft_, asynRawDataId_, 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_);
|
|
}
|
|
triggOnce_ = 0; // Wait for next trigger if in trigg mode
|
|
clearBuffers();
|
|
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;
|
|
}
|
|
|
|
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 == 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;
|
|
}
|