diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c index e240feb..25cdffb 100644 --- a/src/ecmcPluginSafety.c +++ b/src/ecmcPluginSafety.c @@ -3,16 +3,16 @@ * ecmc is distributed subject to a Software License Agreement found * in file LICENSE that is included with this distribution. * -* ecmcPluginExample.cpp +* ecmcPluginSafety.c * -* Created on: july 10, 2023 +* Created on: jan 29, 2024 * Author: anderssandstrom * \*************************************************************************/ // Needed to get headers in ecmc right... #define ECMC_IS_PLUGIN -#define ECMC_EXAMPLE_PLUGIN_VERSION 2 +#define ECMC_EXAMPLE_PLUGIN_VERSION 0 #ifdef __cplusplus extern "C" { @@ -23,8 +23,8 @@ extern "C" { #include #include "ecmcPluginDefs.h" -#include "ecmcMotionPlgDefs.h" -#include "ecmcMotionPlgWrap.h" +#include "ecmcSafetyPlgDefs.h" +#include "ecmcSafetyPlgWrap.h" static int lastEcmcError = 0; static char* lastConfStr = NULL; @@ -34,21 +34,21 @@ static char* lastConfStr = NULL; * Return value other than 0 will be considered error. * configStr can be used for configuration parameters. **/ -int motionConstruct(char *configStr) +int safetyConstruct(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 createMotionObj(configStr); + return createSafetyObj(configStr); } /** Optional function. * Will be called once at unload. **/ -void motionDestruct(void) +void safetyDestruct(void) { - deleteAllMotionObjs(); + deleteAllSafetyObjs(); if(lastConfStr){ free(lastConfStr); } @@ -60,9 +60,9 @@ void motionDestruct(void) * this plugin to react on ecmc errors * Return value other than 0 will be considered to be an error code in ecmc. **/ -int motionRealtime(int ecmcError) +int safetyRealtime(int ecmcError) { - executeMotionObjs(); + executeSafetyObjs(); lastEcmcError = ecmcError; return 0; } @@ -70,15 +70,15 @@ int motionRealtime(int ecmcError) /** Link to data source here since all sources should be availabe at this stage * (for example ecmc PLC variables are defined only at enter of realtime) **/ -int motionEnterRT(){ - return linkDataTomotionObjs(); +int safetyEnterRT(){ + return linkDataToSafetyObjs(); } /** Optional function. * Will be called once just before leaving realtime mode * Return value other than 0 will be considered error. **/ -int motionExitRT(void){ +int safetyExitRT(void){ return 0; } @@ -112,28 +112,26 @@ struct ecmcPluginData pluginDataDef = { // Allways use ECMC_PLUG_VERSION_MAGIC .ifVersion = ECMC_PLUG_VERSION_MAGIC, // Name - .name = "ecmcPlugin_Motion", + .name = "ecmc_plugin_safety", // Description - .desc = "Motion plugin for commissioning of ecmc motion axes.", + .desc = "Safety plugin.", // Option description .optionDesc = "\n "ECMC_PLUGIN_DBG_PRINT_OPTION_CMD"<1/0> : Enables/disables printouts from plugin, default = disabled.\n" - " "ECMC_PLUGIN_AXIS_OPTION_CMD" : Sets default source axis id.\n" - " "ECMC_PLUGIN_BUFFER_SIZE_OPTION_CMD" : Data points to collect, default = 4096.\n" - " "ECMC_PLUGIN_RATE_OPTION_CMD" : Sampling rate in Hz" - " "ECMC_PLUGIN_MODE_OPTION_CMD" : Sampling rate in Hz" + " "ECMC_PLUGIN_RAMP_DOWN_INPUT_CMD_ENTRY_OPTION_CMD" : EtherCat entry input for ramp down cmd (bit).\n" + " "ECMC_PLUGIN_AXES_STANDSTILL_OUTPUT_STAT_ENTRY_OPTION_CMD" : EtherCat entry output for all axes standstill status (bit).\n" , // Plugin version .version = ECMC_EXAMPLE_PLUGIN_VERSION, // Optional construct func, called once at load. NULL if not definded. - .constructFnc = motionConstruct, + .constructFnc = safetyConstruct, // Optional destruct func, called once at unload. NULL if not definded. - .destructFnc = motionDestruct, + .destructFnc = safetyDestruct, // Optional func that will be called each rt cycle. NULL if not definded. - .realtimeFnc = motionRealtime, + .realtimeFnc = safetyRealtime, // Optional func that will be called once just before enter realtime mode - .realtimeEnterFnc = motionEnterRT, + .realtimeEnterFnc = safetyEnterRT, // Optional func that will be called once just before exit realtime mode - .realtimeExitFnc = motionExitRT, + .realtimeExitFnc = safetyExitRT, // PLC funcs // .funcs[0] = // { /*----fft_clear----*/ diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp new file mode 100644 index 0000000..b81b444 --- /dev/null +++ b/src/ecmcSafetyGroup.cpp @@ -0,0 +1,302 @@ +/*************************************************************************\ +* Copyright (c) 2023 Paul Scherrer Institute +* ecmc is distributed subject to a Software License Agreement found +* in file LICENSE that is included with this distribution. +* +* ecmcSafetyGroup.cpp +* +* Created on: July 10, 2023 +* 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_SAFETY_STAT "status" + +#include +#include "ecmcSafetyPlg.h" +#include "ecmcPluginClient.h" +#include "ecmcAsynPortDriver.h" +#include "ecmcAsynPortDriverUtils.h" +#include "ecmcMotion.h" + +#ifdef BASE_VERSION +#define EPICS_3_13 +extern DBBASE *pdbbase; +#endif + +/** ecmc Safety interface class + * This object can throw: + * - bad_alloc + * - invalid_argument + * - runtime_error +*/ +ecmcSafetyGroup::ecmcSafetyGroup(int objIndex, // 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 */ + { + + status_ = 0; + asynStatusId_ = -1; + axesCounter_ = 0; + ecmcSampleRateHz_ = getEcmcSampleRate(); + dataSourcesLinked_ = 0; + // Config defaults + cfgDbgMode_ = 0; + + parseConfigStr(configStr); // Assigns all configs + initAsyn(); +} + +ecmcSafetyGroup::~ecmcSafetyGroup() { +} + +void ecmcSafetyGroup::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); + } + + pThisOption = pNextOption; + } + free(pOptions); + } +} + +void ecmcSafetyGroup::connectToDataSource() { + ///* Check if already linked (one call to enterRT per loaded FFT lib (FFT object)) + // But link should only happen once!!*/ + //if( dataSourcesLinked_ ) { + // 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_ = cfgSampleRateHz_ * dataItem_->getEcmcDataSize()/dataItem_->getEcmcDataElementSize(); + //setDoubleParam(asynSRateId_, cfgDataSampleRateHz_); + //callParamCallbacks(); +// + // dataSourcesLinked_ = 1; + // updateStatus(IDLE); + + dataSourcesLinked_ = 1; +} + +void ecmcSafetyGroup::initAsyn() { + + // Add motion "plugin.motion%d.status" + paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_SAFETY_STAT; + paramId = &asynStatusId_; + if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { + throw std::runtime_error("Failed create asyn parameter mode"); + } + setIntegerParam(*paramId, (epicsInt32)status_); + + // Update integers + callParamCallbacks(); +} + +// Avoid issues with std:to_string() +std::string ecmcSafetyGroup::to_string(int value) { + std::ostringstream os; + os << value; + return os.str(); +} + +// Executed by ecmc rt thread. +void ecmcSafetyGroup::execute() { + // xxx +} + +asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { + int function = pasynUser->reason; + if( function == asynStatusId_ ){ + *value = (epicsInt32)status_; + return asynSuccess; + } + + return asynError; +} + +int ecmcSafetyGroup::addAxis(int axisId) { + ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId); + + if(temp) { + axes_->push_back(axis); + axesCounter_++; + } else { + throw std::out_of_range("Invalid axis id"); + } + + return 0; +} + +uint8_t ecmcSafetyGroup::getUint8(uint8_t* data) { + return *data; +} + +int8_t ecmcSafetyGroup::getInt8(uint8_t* data) { + int8_t* p=(int8_t*)data; + return *p; +} + +uint16_t ecmcSafetyGroup::getUint16(uint8_t* data) { + uint16_t* p=(uint16_t*)data; + return *p; +} + +int16_t ecmcSafetyGroup::getInt16(uint8_t* data) { + int16_t* p=(int16_t*)data; + return *p; +} + +uint32_t ecmcSafetyGroup::getUint32(uint8_t* data) { + uint32_t* p=(uint32_t*)data; + return *p; +} + +int32_t ecmcSafetyGroup::getInt32(uint8_t* data) { + int32_t* p=(int32_t*)data; + return *p; +} + +uint64_t ecmcSafetyGroup::getUint64(uint8_t* data) { + uint64_t* p=(uint64_t*)data; + return *p; +} + +int64_t ecmcSafetyGroup::getInt64(uint8_t* data) { + int64_t* p=(int64_t*)data; + return *p; +} + +float ecmcSafetyGroup::getFloat32(uint8_t* data) { + float* p=(float*)data; + return *p; +} + +double ecmcSafetyGroup::getFloat64(uint8_t* data) { + double* p=(double*)data; + return *p; +} + +size_t ecmcSafetyGroup::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; +} diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h new file mode 100644 index 0000000..87a6a61 --- /dev/null +++ b/src/ecmcSafetyGroup.h @@ -0,0 +1,78 @@ +/*************************************************************************\ +* Copyright (c) 2023 Paul Scherrer Institute +* ecmc is distributed subject to a Software License Agreement found +* in file LICENSE that is included with this distribution. +* +* ecmcSafetyPlg.h +* +* Created on: jan 29, 2024 +* Author: anderssandstrom +* +\*************************************************************************/ +#ifndef ECMC_SAFETY_GROUP_PLG_H_ +#define ECMC_SAFETY_GROUP_PLG_H_ + +#include +#include "ecmcDataItem.h" +#include "ecmcAsynPortDriver.h" +#include "ecmcSafetyGroupDefs.h" +#include +#include "ecmcAxisBase.h" +#include + +class ecmcSafetyGroup : public asynPortDriver { + public: + + /** ecmc Safty Plg class + * This object can throw: + * - bad_alloc + * - out_of_range + */ + ecmcSafetyGroup(int objIndex, // index of this object + char* configStr, + char* portName); + ~ecmcSafetyGroup(); + + // Call just before realtime because then all data sources should be available + void connectToDataSource(); + void addAxis(int axisId); + void execute(); + virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + + + private: + void parseConfigStr(char *configStr); + void initAsyn(); + double ecmcSampleRateHz_ + int dataSourceLinked_; // To avoid link several times + int objectId_; // Unique object id + int cycleCounter_; + + // Config options + int cfgDbgMode_; // Config: allow dbg printouts + + // Asyn + int asynStatusId_; + + // + int status_; + static std::vector axes_; + int axesCounter_; + + // 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); + int objId); + static std::string to_string(int value); +}; + +#endif /* ECMC_SAFETY_GROUP_PLG_H_ */ diff --git a/src/ecmcSafetyPlg.cpp b/src/ecmcSafetyPlg.cpp deleted file mode 100644 index 5cf7fd7..0000000 --- a/src/ecmcSafetyPlg.cpp +++ /dev/null @@ -1,1064 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2023 Paul Scherrer Institute -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcMotionPlg.cpp -* -* Created on: July 10, 2023 -* 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_ENABLE "enable" -#define ECMC_PLUGIN_ASYN_MOTION_COMMAND "cmd" -#define ECMC_PLUGIN_ASYN_MOTION_STAT "status" -#define ECMC_PLUGIN_ASYN_MOTION_TRIGG "trigg" -#define ECMC_PLUGIN_ASYN_MOTION_MODE "mode" -#define ECMC_PLUGIN_ASYN_AXIS_ID "axis_id" -#define ECMC_PLUGIN_ASYN_RATE "samplerate" -#define ECMC_PLUGIN_ASYN_X_DATA "x_arr" -#define ECMC_PLUGIN_ASYN_Y_ACTPOS_DATA "actpos_arr" -#define ECMC_PLUGIN_ASYN_Y_SETPOS_DATA "setpos_arr" -#define ECMC_PLUGIN_ASYN_Y_DIFFPOS_DATA "diffpos_arr" -#define ECMC_PLUGIN_ASYN_ENABLE_DATA "enable_arr" -#define ECMC_PLUGIN_ASYN_ENABLED_DATA "enabled_arr" -#define ECMC_PLUGIN_ASYN_BUSY_DATA "busy_arr" -#define ECMC_PLUGIN_ASYN_EXECUTE_DATA "execute_arr" -#define ECMC_PLUGIN_ASYN_TRAJSOURCE_DATA "trajsrc_arr" -#define ECMC_PLUGIN_ASYN_ENCSOURCE_DATA "encsrc_arr" -#define ECMC_PLUGIN_ASYN_ATTARGET_DATA "attarget_arr" -#define ECMC_PLUGIN_ASYN_ERROR_DATA "error_arr" -#define ECMC_PLUGIN_ASYN_BUFF_SIZE "buff_size" -#define ECMC_PLUGIN_ASYN_ELEMENTS_IN_BUFF "elem_count" -#define ECMC_PLUGIN_ASYN_STATUSWD_DATA "statuswd_arr" - -#include - -#include "ecmcMotionPlg.h" -#include "ecmcPluginClient.h" -#include "ecmcAsynPortDriver.h" -#include "ecmcAsynPortDriverUtils.h" -#include "epicsThread.h" -#include "ecmcMotion.h" - -// Breaktable -#include "ellLib.h" -#include "dbStaticLib.h" -#include "dbAccess.h" -#include "epicsVersion.h" -#include "cvtTable.h" -#ifdef BASE_VERSION -#define EPICS_3_13 -extern DBBASE *pdbbase; -#endif - -// Start worker for asyn write -void f_worker_write(void *obj) { - if(!obj) { - printf("%s/%s:%d: Error: Worker write thread ecmcMotionPlg object NULL..\n", - __FILE__, __FUNCTION__, __LINE__); - return; - } - ecmcMotionPlg *mtnobj = (ecmcMotionPlg*)obj; - mtnobj->doWriteWorker(); -} - -/** ecmc Motion Commissioning class - * This object can throw: - * - bad_alloc - * - invalid_argument - * - runtime_error -*/ -ecmcMotionPlg::ecmcMotionPlg(int objIndex, // 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 */ - { - - axis_ = NULL; - - command_ = 0; - status_ = 0; - - destructs_ = 0; - callbackHandle_ = -1; - objectId_ = objIndex; - triggOnce_ = 0; - cycleCounter_ = 0; - ignoreCycles_ = 0; - dataSourceLinked_ = 0; - - bTriggInProgress_ = false; - - asynEnableId_ = -1; // Enable/disable acq./calcs - asynYActPosArrayId_ = -1; - asynYSetPosArrayId_ = -1; // FFT amplitude array (double) - asynYDiffPosArrayId_ = -1; // FFT mode (cont/trigg) - asynTriggId_ = -1; // Trigg new measurement - asynXAxisArrayId_ = -1; // FFT X-axis frequencies - asynSRateId_ = -1; // Sample rate Hz - asynElementsInBufferId_ = -1; - asynCommandId_ = -1; - asynStatusId_ = -1; - asynBufferSizeId_ = -1; - asynModeId_ = -1; - - writeBusy_ = 0; - writePauseTime_.tv_sec = 0; - writePauseTime_.tv_nsec = 0.5e6; // 0.5ms - - axisMutex_ = epicsMutexCreate(); - - ecmcSampleRateHz_ = getEcmcSampleRate(); - cfgSampleRateHz_ = ecmcSampleRateHz_; - cfgDataSampleRateHz_ = ecmcSampleRateHz_; - cfgAxisIndex_ = 1; - - // Config defaults - cfgDbgMode_ = 0; - cfgArraySize_ = ECMC_PLUGIN_DEFAULT_ARRAY_SIZE; // samples in fft (must be n^2) - cfgEnable_ = 0; // start disabled (enable over asyn) - - parseConfigStr(configStr); // Assigns all configs - // Check valid nfft - if(cfgArraySize_ <= 0) { - throw std::out_of_range("Array size must be > 0 (default 4096)."); - } - - // Check valid sample rate - if(cfgSampleRateHz_ <= 0) { - throw std::out_of_range("Invalid sample rate"); - } - if(cfgSampleRateHz_ > ecmcSampleRateHz_) { - printf("Warning selected sample rate faster than ecmc rate, rate will be set to ecmc rate.\n"); - cfgSampleRateHz_ = 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_ / cfgSampleRateHz_ -1; - - // Allocate buffers - actPosBuffer_ = new ecmcDataBuffer(objectId_,0, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_Y_ACTPOS_DATA, - this); - setPosBuffer_ = new ecmcDataBuffer(objectId_,1, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_Y_SETPOS_DATA, - this); - diffPosBuffer_ = new ecmcDataBuffer(objectId_,2, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_Y_DIFFPOS_DATA, - this); - xPosBuffer_ = new ecmcDataBuffer(objectId_,3, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_X_DATA, - this); - enableBuffer_ = new ecmcDataBuffer(objectId_,4, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_ENABLE_DATA, - this); - enabledBuffer_ = new ecmcDataBuffer(objectId_,5, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_ENABLED_DATA, - this); - busyBuffer_ = new ecmcDataBuffer(objectId_,6, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_BUSY_DATA, - this); - executeBuffer_ = new ecmcDataBuffer(objectId_,7, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_EXECUTE_DATA, - this); - trajSourceBuffer_ = new ecmcDataBuffer(objectId_,8, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_TRAJSOURCE_DATA, - this); - encSourceBuffer_ = new ecmcDataBuffer(objectId_,9, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_ENCSOURCE_DATA, - this); - atTargetBuffer_ = new ecmcDataBuffer(objectId_,10, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_ATTARGET_DATA, - this); - errorIdBuffer_ = new ecmcDataBuffer(objectId_,11, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_ERROR_DATA, - this); - statusWdBuffer_ = new ecmcDataBuffer(objectId_,12, - cfgArraySize_, - cfgDbgMode_, - ECMC_PLUGIN_ASYN_STATUSWD_DATA, - this); - - axis_ = (ecmcAxisBase*) getAxisPointer(cfgAxisIndex_); - clearBuffers(); - - // init x axis (after clear) - xdt_ = 1 / cfgSampleRateHz_; - xTime_ = 0; - for(size_t i=0;iaddData(xTime_); - xTime_+=xdt_; - } - - xTime_ = 0; - initAsyn(); - - // Create worker thread for writing socket - std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX ".writer"; - if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) { - throw std::runtime_error("Error: Failed create worker thread for ecmc." ECMC_PLUGIN_ASYN_PREFIX ".writer"); - } - doWriteEvent_.signal(); -} - -ecmcMotionPlg::~ecmcMotionPlg() { - // kill worker - destructs_ = 1; // maybe need todo in other way.. - delete actPosBuffer_; - - // De register callback when unload - //if(callbackHandle_ >= 0) { - // dataItem_->deregDataUpdatedCallback(callbackHandle_); - //} -} - -void ecmcMotionPlg::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_AXIS_OPTION_CMD (number) - else if (!strncmp(pThisOption, ECMC_PLUGIN_AXIS_OPTION_CMD, strlen(ECMC_PLUGIN_AXIS_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_AXIS_OPTION_CMD); - cfgAxisIndex_ = 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_BUFFER_SIZE_OPTION_CMD (number) - else if (!strncmp(pThisOption, ECMC_PLUGIN_BUFFER_SIZE_OPTION_CMD, strlen(ECMC_PLUGIN_BUFFER_SIZE_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_BUFFER_SIZE_OPTION_CMD); - cfgArraySize_ = atoi(pThisOption); - } - - // ECMC_PLUGIN_RATE_OPTION_CMD (number) - else if (!strncmp(pThisOption, ECMC_PLUGIN_RATE_OPTION_CMD, strlen(ECMC_PLUGIN_RATE_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_RATE_OPTION_CMD); - cfgSampleRateHz_ = atof(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); - } -} - -void ecmcMotionPlg::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_ = cfgSampleRateHz_ * dataItem_->getEcmcDataSize()/dataItem_->getEcmcDataElementSize(); - //setDoubleParam(asynSRateId_, cfgDataSampleRateHz_); - //callParamCallbacks(); -// - // dataSourceLinked_ = 1; - // updateStatus(IDLE); -} - -void ecmcMotionPlg::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_ == cfgArraySize_) { - // printf("Buffer full (%zu elements appended).\n",elementsInBuffer_); - // } - //} - // - //if(elementsInBuffer_ >= cfgArraySize_) { - // //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 ecmcMotionPlg::addDataToBuffer(double data) { -// -// if(rawDataBuffer_ && (elementsInBuffer_ < cfgArraySize_) ) { -// -// if( cfgBreakTableStr_ && interruptAccept ) { -// double breakData = data; -// // Supply a breaktable (init=0, LINR must be > 1 but only used if init > 0) -// if (cvtRawToEngBpt(&breakData, 2, 0, &breakTable_, &lastBreakPoint_)!=0) { -// //TODO: What does status here mean.. -// //throw std::runtime_error("Breaktable conversion failed.\n"); -// } -// //printf("Index %d: Before %lf, after %lf\n",objectId_,data,breakData); -// data = breakData * cfgScale_; -// } else { -// data = data * cfgScale_; -// } -// -// rawDataBuffer_[elementsInBuffer_] = data; -// prepProcDataBuffer_[elementsInBuffer_] = data; -// } -// elementsInBuffer_ ++; -//} - -void ecmcMotionPlg::clearBuffers() { - actPosBuffer_->clear(); - setPosBuffer_->clear(); - diffPosBuffer_->clear(); - enableBuffer_->clear(); - enabledBuffer_->clear(); - busyBuffer_->clear(); - executeBuffer_->clear(); - trajSourceBuffer_->clear(); - encSourceBuffer_->clear(); - atTargetBuffer_->clear(); - errorIdBuffer_->clear(); - xPosBuffer_->clear(); - statusWdBuffer_->clear(); -} - -void ecmcMotionPlg::printEcDataArray(uint8_t* data, - size_t size, - ecmcEcDataType dt, - int objId) { - printf("motion obj 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; - } -} - -int ecmcMotionPlg::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 ecmcMotionPlg::getUint8(uint8_t* data) { - return *data; -} - -int8_t ecmcMotionPlg::getInt8(uint8_t* data) { - int8_t* p=(int8_t*)data; - return *p; -} - -uint16_t ecmcMotionPlg::getUint16(uint8_t* data) { - uint16_t* p=(uint16_t*)data; - return *p; -} - -int16_t ecmcMotionPlg::getInt16(uint8_t* data) { - int16_t* p=(int16_t*)data; - return *p; -} - -uint32_t ecmcMotionPlg::getUint32(uint8_t* data) { - uint32_t* p=(uint32_t*)data; - return *p; -} - -int32_t ecmcMotionPlg::getInt32(uint8_t* data) { - int32_t* p=(int32_t*)data; - return *p; -} - -uint64_t ecmcMotionPlg::getUint64(uint8_t* data) { - uint64_t* p=(uint64_t*)data; - return *p; -} - -int64_t ecmcMotionPlg::getInt64(uint8_t* data) { - int64_t* p=(int64_t*)data; - return *p; -} - -float ecmcMotionPlg::getFloat32(uint8_t* data) { - float* p=(float*)data; - return *p; -} - -double ecmcMotionPlg::getFloat64(uint8_t* data) { - double* p=(double*)data; - return *p; -} - -size_t ecmcMotionPlg::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 ecmcMotionPlg::initAsyn() { - - // Add enable "plugin.motion%d.enable" - std::string paramName =ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_ENABLE; - int *paramId = &asynEnableId_; - if( createParam(0, paramName.c_str(), asynParamInt32, paramId) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter enable"); - } - setIntegerParam(*paramId, cfgEnable_); - -// Add xdata "plugin.motion%d.x_arr" -// paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + -// "." + ECMC_PLUGIN_ASYN_X_DATA; -// paramId = &asynXAxisArrayId_; -// if( createParam(0, paramName.c_str(), asynParamFloat64Array, paramId ) != asynSuccess ) { -// throw std::runtime_error("Failed create asyn parameter rawdata"); -// } -// doCallbacksFloat64Array(xAxisArray_, cfgArraySize_, *paramId,0); -// -// // Add actpos "plugin.motion%d.actpos_arr" -// paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + -// "." + ECMC_PLUGIN_ASYN_Y_ACTPOS_DATA; -// paramId = &asynYActPosArrayId_; -// if( createParam(0, paramName.c_str(), asynParamFloat64Array, paramId ) != asynSuccess ) { -// throw std::runtime_error("Failed create asyn parameter rawdata"); -// } -// doCallbacksFloat64Array(yActPosArray_, cfgArraySize_, *paramId,0); -// -// // Add actpos "plugin.motion%d.setpos_arr" -// paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + -// "." + ECMC_PLUGIN_ASYN_Y_SETPOS_DATA; -// paramId = &asynYSetPosArrayId_; -// if( createParam(0, paramName.c_str(), asynParamFloat64Array, paramId ) != asynSuccess ) { -// throw std::runtime_error("Failed create asyn parameter rawdata"); -// } -// doCallbacksFloat64Array(ySetPosArray_, cfgArraySize_, *paramId,0); -// -// // Add diffpos "plugin.motion%d.diffpos_arr" -// paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + -// "." + ECMC_PLUGIN_ASYN_Y_DIFFPOS_DATA; -// paramId = &asynYDiffPosArrayId_; -// if( createParam(0, paramName.c_str(), asynParamFloat64Array, paramId ) != asynSuccess ) { -// throw std::runtime_error("Failed create asyn parameter rawdata"); -// } -// doCallbacksFloat64Array(yDiffPosArray_, cfgArraySize_, *paramId,0); -// - // Add motion "plugin.motion%d.command" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_MOTION_COMMAND; - paramId = &asynCommandId_; - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter mode"); - } - setIntegerParam(*paramId, (epicsInt32)command_); - - // Add motion "plugin.motion%d.status" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_MOTION_STAT; - paramId = &asynStatusId_; - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter mode"); - } - setIntegerParam(*paramId, (epicsInt32)status_); - - // Add motion "plugin.motion%d.trigg" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_MOTION_TRIGG; - paramId = &asynTriggId_; - - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter trigg"); - } - setIntegerParam(*paramId, (epicsInt32)triggOnce_); - - // Add motion plugin.motion%d.buff_size" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_BUFF_SIZE; - paramId = &asynBufferSizeId_; - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter nfft"); - } - setIntegerParam(*paramId, (epicsInt32)cfgArraySize_); - - // Add motion "plugin.motion%d.rate" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_RATE; - paramId = &asynSRateId_; - - if( createParam(0, paramName.c_str(), asynParamFloat64, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter rate"); - } - setDoubleParam(*paramId, cfgSampleRateHz_); - - // Add motion "plugin.motion%d.elem_count" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_ELEMENTS_IN_BUFF; - paramId = &asynElementsInBufferId_; - - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter trigg"); - } - setIntegerParam(*paramId, (epicsInt32)elementsInBuffer_); - - // Add motion "plugin.motion%d.axis" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_AXIS_ID; - paramId = &asynAxisId_; - - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter axis id"); - } - setIntegerParam(*paramId, (epicsInt32)cfgAxisIndex_); - - // Add motion "plugin.motion%d.mode" - paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + - "." + ECMC_PLUGIN_ASYN_MOTION_MODE; - paramId = &asynModeId_; - - if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter mode"); - } - setIntegerParam(*paramId, (epicsInt32)cfgMode_); - - // Update integers - callParamCallbacks(); -} - -// Avoid issues with std:to_string() -std::string ecmcMotionPlg::to_string(int value) { - std::ostringstream os; - os << value; - return os.str(); -} - -void ecmcMotionPlg::setEnable(int enable) { - cfgEnable_ = enable; - setIntegerParam(asynEnableId_, enable); - actPosBuffer_->setEnable(enable); - setPosBuffer_->setEnable(enable); - diffPosBuffer_->setEnable(enable); - xPosBuffer_->setEnable(enable); -} - -void ecmcMotionPlg::triggMotionObject() { - clearBuffers(); - triggOnce_ = 1; - setIntegerParam(asynTriggId_,0); -} - - -// Executed by ecmc rt thread. -void ecmcMotionPlg::executeMotionObject() { - - if(!axis_) { - return; - } - - if(cfgMode_==TRIGG && !bTriggInProgress_) { - return; - } - - // protect axis_ if axis object id is changed over asyn - epicsMutexLock(axisMutex_); - ecmcAxisStatusType *tempAxisStat = axis_->getDebugInfoDataPointer(); - - // Fill the buffers - actPosBuffer_->addData(tempAxisStat->onChangeData.positionActual); - setPosBuffer_->addData(tempAxisStat->onChangeData.positionSetpoint); - diffPosBuffer_->addData(tempAxisStat->onChangeData.positionError); - enableBuffer_->addData(tempAxisStat->onChangeData.statusWd.enable); - enabledBuffer_->addData(tempAxisStat->onChangeData.statusWd.enabled); - busyBuffer_->addData(tempAxisStat->onChangeData.statusWd.busy); - executeBuffer_->addData(tempAxisStat->onChangeData.statusWd.execute); - trajSourceBuffer_->addData(tempAxisStat->onChangeData.statusWd.trajsource); - encSourceBuffer_->addData(tempAxisStat->onChangeData.statusWd.encsource); - atTargetBuffer_->addData(tempAxisStat->onChangeData.statusWd.attarget); - errorIdBuffer_->addData(tempAxisStat->onChangeData.error); - - epicsInt32 *temp=(epicsInt32*)&(tempAxisStat->onChangeData.statusWd); // A bit nasty - statusWdBuffer_->addData(*temp); - - xTime_+=xdt_; - xPosBuffer_->addData(xTime_); // Always relative within one buffer - - // If buffers full the (all buffers syncronous so all should be full if one is full) - if(actPosBuffer_->getBufferFull()) { - bTriggInProgress_ = false; - xTime_ = 0; //reset x axis - switchBuffers(); // unfortenately needed here - doWriteEvent_.signal(); - } - epicsMutexUnlock(axisMutex_); -} - -//void ecmcMotionPlg::setModeFFT(FFT_MODE mode) { -// cfgMode_ = mode; -// setIntegerParam(asynFFTModeId_,(epicsInt32)mode); -//} -// -//FFT_STATUS ecmcMotionPlg::getStatusFFT() { -// return status_; -//} - -//void ecmcMotionPlg::updateStatus(FFT_STATUS status) { -// status_ = status; -// setIntegerParam(asynFFTStatId_,(epicsInt32) status); -// -// setIntegerParam(asynElementsInBuffer_, (epicsInt32)elementsInBuffer_); -// -// callParamCallbacks(); -//} - -asynStatus ecmcMotionPlg::writeInt32(asynUser *pasynUser, epicsInt32 value) { - int function = pasynUser->reason; - if( function == asynEnableId_ ) { - cfgEnable_ = value; - return asynSuccess; - } else if( function == asynTriggId_){ - triggOnce_ = value > 0; - return asynSuccess; - } else if( function == asynCommandId_){ - command_ = value > 0; - //also exe something? - return asynSuccess; - } else if( function == asynEnableId_){ - setEnable(value > 0); - return asynSuccess; - } else if( function == asynAxisId_){ - return setAxis(value) > 0 ? asynSuccess : asynError; - } else if( function == asynModeId_){ - return setMode((TRIGG_MODE)value) > 0 ? asynSuccess : asynError; - } else if( function == asynModeId_){ - return setTrigg(value) ? asynSuccess :asynError; - } - - return asynError; -} - -asynStatus ecmcMotionPlg::readInt32(asynUser *pasynUser, epicsInt32 *value) { - int function = pasynUser->reason; - if( function == asynEnableId_ ) { - *value = cfgEnable_; - return asynSuccess; - } else if( function == asynAxisId_){ - *value = cfgAxisIndex_; - return asynSuccess; - } else if( function == asynTriggId_ ){ - *value = triggOnce_; - return asynSuccess; - }else if( function == asynStatusId_ ){ - *value = (epicsInt32)status_; - return asynSuccess; - }else if( function == asynBufferSizeId_ ){ - *value = (epicsInt32)cfgArraySize_; - return asynSuccess; - }else if( function == asynElementsInBufferId_){ - *value = (epicsInt32)elementsInBuffer_; - return asynSuccess; - } else if( function == asynEnableId_){ - *value =cfgEnable_; - return asynSuccess; - } - - return asynError; -} - -// These callbacks are moved to ecmcdataBuffer class -asynStatus ecmcMotionPlg::readFloat64Array(asynUser *pasynUser, epicsFloat64 *value, - size_t nElements, size_t *nIn) { - *nIn = 0; - return asynError; -} - -asynStatus ecmcMotionPlg::readFloat64(asynUser *pasynUser, epicsFloat64 *value) { - int function = pasynUser->reason; - if( function == asynSRateId_ ) { - *value = cfgDataSampleRateHz_; - return asynSuccess; - } - - return asynError; -} - -int ecmcMotionPlg::setAxis(int axisId) { - ecmcAxisBase *temp= (ecmcAxisBase*) getAxisPointer(axisId); - if(!temp) { - printf("Warning selected axis index out of range.\n"); - //set old value again - setParamAlarmStatus(asynAxisId_,1); - setParamAlarmSeverity(asynAxisId_,1); - setIntegerParam(asynAxisId_, (epicsInt32)cfgAxisIndex_); - callParamCallbacks(); - return ECMC_PLUGIN_MOTION_ERROR_AXIS_OUT_OF_RANGE; - } - - epicsMutexLock(axisMutex_); - clearBuffers(); - axis_ = temp; - cfgAxisIndex_ = axisId; - epicsMutexUnlock(axisMutex_); - - setParamAlarmStatus(asynAxisId_,0); - setParamAlarmSeverity(asynAxisId_,0); - setIntegerParam(asynAxisId_, (epicsInt32)cfgAxisIndex_); - callParamCallbacks(); - return 0; -} - -int ecmcMotionPlg::setTrigg(int trigg) { - if(cfgMode_==TRIGG) { - epicsMutexLock(axisMutex_); - clearBuffers(); - bTriggInProgress_ = true; - epicsMutexUnlock(axisMutex_); - } - return 0; -} - -int ecmcMotionPlg::setMode(TRIGG_MODE mode) { - cfgMode_ = mode; - return 0; -} - -// Write socket worker thread (switch between two buffers) -void ecmcMotionPlg::doWriteWorker() { - while(true) { - if(destructs_) { - return; - } - - nanosleep(&writePauseTime_,NULL); - - if(writeBusy_ || !cfgEnable_ ) { - continue; - } - - if(destructs_) { - return; - } - - doWriteEvent_.wait(); - writeBusy_ = 1; - writeBuffers(); - writeBusy_ = 0; - } -} - -// triggered by low prio work thread only -void ecmcMotionPlg::writeBuffers() { - - //Write all buffers - actPosBuffer_->writeBuffer(); - setPosBuffer_->writeBuffer(); - diffPosBuffer_->writeBuffer(); - enableBuffer_->writeBuffer(); - enabledBuffer_->writeBuffer(); - busyBuffer_->writeBuffer(); - executeBuffer_->writeBuffer(); - trajSourceBuffer_->writeBuffer(); - encSourceBuffer_->writeBuffer(); - atTargetBuffer_->writeBuffer(); - errorIdBuffer_->writeBuffer(); - statusWdBuffer_->writeBuffer(); - - // Always write x last if triggering is needed - xPosBuffer_->writeBuffer(); -} - -// triggered by ecmc RT thread -void ecmcMotionPlg::switchBuffers() { - // switch in new empty buffer while data is stored in the other. - actPosBuffer_->switchBuffer(); - setPosBuffer_->switchBuffer(); - diffPosBuffer_->switchBuffer(); - enableBuffer_->switchBuffer(); - enabledBuffer_->switchBuffer(); - busyBuffer_->switchBuffer(); - executeBuffer_->switchBuffer(); - trajSourceBuffer_->switchBuffer(); - encSourceBuffer_->switchBuffer(); - atTargetBuffer_->switchBuffer(); - errorIdBuffer_->switchBuffer(); - statusWdBuffer_->switchBuffer(); - xPosBuffer_->switchBuffer(); -} - diff --git a/src/ecmcSafetyPlg.h b/src/ecmcSafetyPlg.h deleted file mode 100644 index f13e1e8..0000000 --- a/src/ecmcSafetyPlg.h +++ /dev/null @@ -1,168 +0,0 @@ -/*************************************************************************\ -* Copyright (c) 2023 Paul Scherrer Institute -* ecmc is distributed subject to a Software License Agreement found -* in file LICENSE that is included with this distribution. -* -* ecmcMotionPlg.h -* -* Created on: july 10, 2023 -* Author: anderssandstrom -* -\*************************************************************************/ -#ifndef ECMC_MOTION_PLG_H_ -#define ECMC_MOTION_PLG_H_ - -#include -#include "ecmcDataItem.h" -#include "ecmcAsynPortDriver.h" -#include "ecmcMotionPlgDefs.h" -#include "inttypes.h" -#include -#include "dbBase.h" -#include "ecmcAxisBase.h" -#include "ecmcDataBuffer.h" -#include "epicsMutex.h" -#include "epicsEvent.h" - -#define ECMC_PLUGIN_MOTION_ERROR_AXIS_OUT_OF_RANGE 1 - -typedef enum TRIGG_MODE{ - NO_MODE = 0, - CONT = 1, - TRIGG = 2, -} TRIGG_MODE; - -class ecmcMotionPlg : public asynPortDriver { - public: - - /** ecmc Motion Plg class - * This object can throw: - * - bad_alloc - * - invalid_argument - * - runtime_error - * - out_of_range - */ - ecmcMotionPlg(int objIndex, // index of this object - char* configStr, - char* portName); - ~ecmcMotionPlg(); - - // 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 doWriteWorker(); // low prio worker thread - void setEnable(int enable); - void clearBuffers(); - void triggMotionObject(); - void executeMotionObject(); - 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 readFloat64(asynUser *pasynUser, epicsFloat64 *value); - - - private: - void parseConfigStr(char *configStr); - void addDataToBuffer(double data); - void initAsyn(); - static int dataTypeSupported(ecmcEcDataType dt); - int setAxis(int axisId); - int setMode(TRIGG_MODE mode); - int setTrigg(int trigg); - void writeBuffers(); - void switchBuffers(); - - - epicsMutexId axisMutex_; - - double* xAxisArray_; // FFT x axis with freqs - size_t elementsInBuffer_; - double ecmcSampleRateHz_; - int dataSourceLinked_; // To avoid link several times - int command_; - int status_; - // ecmc callback handle for use when deregister at unload - int callbackHandle_; - int destructs_; - int objectId_; // Unique object id - int triggOnce_; - int cycleCounter_; - int ignoreCycles_; - - // Config options - int cfgDbgMode_; // Config: allow dbg printouts - size_t cfgArraySize_; // Config: Data set size - int cfgEnable_; // Config: Enable data acq./calc. - double cfgSampleRateHz_; // Config: Sample rate (defaults to ecmc rate) - double cfgDataSampleRateHz_;// Config: Sample for data - int cfgAxisIndex_; // Config: Enable data acq./calc. - TRIGG_MODE cfgMode_; // Config: Mode continous or triggered. - - // Asyn - int asynEnableId_; // Enable/disable acq./calcs - int asynYActPosArrayId_; - int asynYSetPosArrayId_; - int asynYDiffPosArrayId_; - int asynXAxisArrayId_; - int asynTriggId_; // Trigg new measurement - int asynAxisId_; // motion axis id - int asynSRateId_; // Sample rate - int asynElementsInBufferId_; // Current buffer index - int asynBufferSizeId_; // Current buffer index - int asynCommandId_; - int asynStatusId_; - int asynModeId_; - - ecmcAxisBase *axis_; - // 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); - - ecmcDataBuffer *actPosBuffer_; - ecmcDataBuffer *setPosBuffer_; - ecmcDataBuffer *diffPosBuffer_; - ecmcDataBuffer *xPosBuffer_; - ecmcDataBuffer *enableBuffer_; - ecmcDataBuffer *enabledBuffer_; - ecmcDataBuffer *busyBuffer_; - ecmcDataBuffer *executeBuffer_; - ecmcDataBuffer *trajSourceBuffer_; - ecmcDataBuffer *encSourceBuffer_; - ecmcDataBuffer *atTargetBuffer_; - ecmcDataBuffer *errorIdBuffer_; - ecmcDataBuffer *statusWdBuffer_; - - bool bTriggInProgress_; - double xdt_; - double xTime_; - epicsEvent doWriteEvent_; - int writeBusy_; - timespec writePauseTime_; -}; - -#endif /* ECMC_MOTION_PLG_H_ */ diff --git a/src/ecmcSafetyPlgDefs.h b/src/ecmcSafetyPlgDefs.h index 1adcac1..4dfa472 100644 --- a/src/ecmcSafetyPlgDefs.h +++ b/src/ecmcSafetyPlgDefs.h @@ -3,9 +3,9 @@ * ecmc is distributed subject to a Software License Agreement found * in file LICENSE that is included with this distribution. * -* ecmcMotionPlgDefs.h +* ecmcSafetyPlgDefs.h * -* Created on: july 10, 2023 +* Created on: Jan 29 2024 * Author: anderssandstrom * Credits to https://github.com/sgreg/dynamic-loading * @@ -14,26 +14,15 @@ #ifndef ECMC_MOTION_PLG_DEFS_H_ #define ECMC_MOTION_PLG_DEFS_H_ -#define ECMC_PLUGIN_ASYN_PREFIX "plugin.motion" +#define ECMC_PLUGIN_ASYN_PREFIX "plugin.safety" // Options -#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT=" -#define ECMC_PLUGIN_AXIS_OPTION_CMD "AXIS=" -#define ECMC_PLUGIN_BUFFER_SIZE_OPTION_CMD "BUFFER_SIZE=" -#define ECMC_PLUGIN_RATE_OPTION_CMD "RATE=" -#define ECMC_PLUGIN_ENABLE_OPTION_CMD "ENABLE=" -#define ECMC_PLUGIN_MODE_OPTION_CMD "MODE=" +#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT=" +#define ECMC_PLUGIN_RAMP_DOWN_INPUT_CMD_ENTRY_OPTION_CMD "RAMP_DOWN_INPUT_CMD_ENTRY=" +#define ECMC_PLUGIN_AXES_STANDSTILL_OUTPUT_STAT_ENTRY_OPTION_CMD "AXES_STANDSTILL_OUTPUT_STAT_ENTRY=" -// CONT, TRIGG -#define ECMC_PLUGIN_MODE_CONT_OPTION "CONT" -#define ECMC_PLUGIN_MODE_TRIGG_OPTION "TRIGG" - - /** Just one error code in "c" part of plugin (error handled with exceptions i c++ part) */ -#define ECMC_PLUGIN_MOTION_ERROR_CODE 1 - -// Default size (must be n²) -#define ECMC_PLUGIN_DEFAULT_ARRAY_SIZE 4096 +#define ECMC_PLUGIN_SAFETY_ERROR_CODE 1 #endif /* ECMC_MOTION_PLG_DEFS_H_ */ diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp index 96571f8..56b450e 100644 --- a/src/ecmcSafetyPlgWrap.cpp +++ b/src/ecmcSafetyPlgWrap.cpp @@ -3,9 +3,9 @@ * ecmc is distributed subject to a Software License Agreement found * in file LICENSE that is included with this distribution. * -* ecmcMotionPlgWrap.cpp +* ecmcSafetyPlgWrap.cpp * -* Created on: july 10, 2023 +* Created on: jan 29, 2024 * Author: anderssandstrom * Credits to https://github.com/sgreg/dynamic-loading * @@ -17,104 +17,82 @@ #include #include #include -#include "ecmcMotionPlgWrap.h" -#include "ecmcMotionPlg.h" +#include "ecmcSafetyGroupPlgWrap.h" +#include "ecmcSafetyGroupPlg.h" #define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64 -#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.MOTION" +#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.SAFETY" -static std::vector motionObjs; -static int motionObjCounter = 0; -static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; +static std::vector safetyGroups; +static int safetyGroupsCounter = 0; +static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; -int createMotionObj(char* configStr) { +int createSafetyGroup(char* configStr) { - // create new ecmcMotionPlg object - ecmcMotionPlg* motionObj = NULL; + // create new ecmcSafetyGroupPlg object + ecmcSafetyGroupPlg* safetyGroup = 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", motionObjCounter); + ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter); try { - motionObj = new ecmcMotionPlg(motionObjCounter, configStr, portNameBuffer); + safetyGroup = new ecmcSafetyGroupPlg(safetyGroupsCounter, configStr, portNameBuffer); } catch(std::exception& e) { - if(motionObj) { - delete motionObj; + if(safetyGroup) { + delete safetyGroup; } printf("Exception: %s. Plugin will unload.\n",e.what()); - return ECMC_PLUGIN_MOTION_ERROR_CODE; + return ECMC_PLUGIN_SAFETY_ERROR_CODE; } - motionObjs.push_back(motionObj); - motionObjCounter++; + safetyGroups.push_back(safetyGroup); + safetyGroupsCounter++; return 0; } -void deleteAllMotionObjs() { - for(std::vector::iterator pMotionObj = motionObjs.begin(); pMotionObj != motionObjs.end(); ++pMotionObj) { - if(*pMotionObj) { - delete (*pMotionObj); +void deleteAllsafetyGroups() { + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + if(*psafetyGroup) { + delete (*psafetyGroup); } } } -int linkDataTomotionObjs() { - for(std::vector::iterator pMotionObj = motionObjs.begin(); pMotionObj != motionObjs.end(); ++pMotionObj) { - if(*pMotionObj) { +int addMotionAxis(int safetyGroupIndex, int axisIndex) { + try { + safetyGroups.at(safetyGroupIndex)->addAxis(axisIndex); + } + catch(std::exception& e) { + printf("Exception: %s. Safety object index out of range.\n",e.what()); + return ECMC_PLUGIN_SAFETY_ERROR_CODE; + } + return 0; +} + +int linkDataToSafetyGroups() { + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + if(*psafetyGroup) { try { - (*pMotionObj)->connectToDataSource(); - } - catch(std::exception& e) { - printf("Exception: %s. Plugin will unload.\n",e.what()); - return ECMC_PLUGIN_MOTION_ERROR_CODE; - } - } - } - return 0; -} - -int enableMotionObj(int objIndex, int enable) { - try { - motionObjs.at(objIndex)->setEnable(enable); - } - catch(std::exception& e) { - printf("Exception: %s. Motion object index out of range.\n",e.what()); - return ECMC_PLUGIN_MOTION_ERROR_CODE; - } - return 0; -} - -int clearMotionObj(int objIndex) { - try { - motionObjs.at(objIndex)->clearBuffers(); - } - catch(std::exception& e) { - printf("Exception: %s. Motion object index out of range.\n",e.what()); - return ECMC_PLUGIN_MOTION_ERROR_CODE; - } - return 0; -} - -int triggMotionObj(int objIndex) { - try { - motionObjs.at(objIndex)->triggMotionObject(); - } - catch(std::exception& e) { - printf("Exception: %s. Motion object index out of range.\n",e.what()); - return ECMC_PLUGIN_MOTION_ERROR_CODE; - } - return 0; -} - -int executeMotionObjs() { - - for(std::vector::iterator pMotionObj = motionObjs.begin(); pMotionObj != motionObjs.end(); ++pMotionObj) { - if(*pMotionObj) { - try { - (*pMotionObj)->executeMotionObject(); + (*psafetyGroup)->connectToDataSource(); + } + catch(std::exception& e) { + printf("Exception: %s. Plugin will unload.\n",e.what()); + return ECMC_PLUGIN_SAFETY_ERROR_CODE; + } + } + } + return 0; +} + +int executeSafetyGroups() { + + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + if(*psafetyGroup) { + try { + (*psafetyGroup)->execute(); } catch(std::exception& e) { printf("Exception: %s. Plugin will unload.\n",e.what()); diff --git a/src/ecmcSafetyPlgWrap.h b/src/ecmcSafetyPlgWrap.h index 240be88..648bb7a 100644 --- a/src/ecmcSafetyPlgWrap.h +++ b/src/ecmcSafetyPlgWrap.h @@ -3,107 +3,61 @@ * ecmc is distributed subject to a Software License Agreement found * in file LICENSE that is included with this distribution. * -* ecmcMotionPlgWrap.h +* ecmcSafetyPlgWrap.h * -* Created on: july 10, 2023 +* Created on: jan 29, 2024 * Author: anderssandstrom * \*************************************************************************/ -#ifndef ECMC_MOTION_PLG_WRAP_H_ -#define ECMC_MOTION_PLG_WRAP_H_ -#include "ecmcMotionPlgDefs.h" +#ifndef ECMC_SAFETY_PLG_WRAP_H_ +#define ECMC_SAFETY_PLG_WRAP_H_ +#include "ecmcSafetyPlgDefs.h" # ifdef __cplusplus extern "C" { # endif // ifdef __cplusplus -/** \brief Create new Motion object +/** \brief Create new Safetry group * - * The plugin supports creation of multiple Motion 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 + * \param[in] configStr Configuration string from load plugin.\n * * \return 0 if success or otherwise an error code.\n */ -int createMotionObj(char *configStr); +int createSafetyGroup(char *configStr); -/** \brief Enable/disable Motion object +/** \brief Add motion axis to safety group * - * Enable/disable Motion 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 + * \param[in] safetyGroupIndex Safty group index + * \param[in] axisIndex Axis index to add to safety group * * \return 0 if success or otherwise an error code.\n */ -int enableMotionObj(int objIndex, int enable); +int addMotionAxis(int safetyGroupIndex, int axisIndex); -/** \brief Clear FFT object\n +/** \brief Deletes all created objects\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 + * Should be called when destructs.\n */ -int clearMotionObj(int objIndex); +void deleteAll(); -/** \brief Set mode of FFT object +/** \brief Link data to _all_ safety objects * - * 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 modeMotionObj(int objIndex, 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 triggMotionObj(int objIndex); - - -/** \brief execute 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 executeMotionObjs(); - -/** \brief Link data to _all_ fft objects - * - * This tells the FFT lib to connect to ecmc to find it's data source.\n + * This tells the safety 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 linkDataTomotionObjs(); +int linkDataToSafetyObjs(); -/** \brief Deletes all created fft objects\n +/** \brief Execute all safety groups * - * Should be called when destructs.\n + * \return 0 if success or otherwise an error code.\n */ - -void deleteAllMotionObjs(); +int executeSafetyGroups() { # ifdef __cplusplus } # endif // ifdef __cplusplus -#endif /* ECMC_MOTION_PLG_WRAP_H_ */ +#endif /* ECMC_SAFETY_PLG_WRAP_H_ */