WIP
This commit is contained in:
+23
-25
@@ -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 <string.h>
|
||||
|
||||
#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"<axis id> : Sets default source axis id.\n"
|
||||
" "ECMC_PLUGIN_BUFFER_SIZE_OPTION_CMD"<size> : Data points to collect, default = 4096.\n"
|
||||
" "ECMC_PLUGIN_RATE_OPTION_CMD"<rate hz> : Sampling rate in Hz"
|
||||
" "ECMC_PLUGIN_MODE_OPTION_CMD"<TRIGG/CONT> : Sampling rate in Hz"
|
||||
" "ECMC_PLUGIN_RAMP_DOWN_INPUT_CMD_ENTRY_OPTION_CMD"<ec entry> : EtherCat entry input for ramp down cmd (bit).\n"
|
||||
" "ECMC_PLUGIN_AXES_STANDSTILL_OUTPUT_STAT_ENTRY_OPTION_CMD"<ec entry> : 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----*/
|
||||
|
||||
@@ -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 <sstream>
|
||||
#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;
|
||||
}
|
||||
@@ -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 <stdexcept>
|
||||
#include "ecmcDataItem.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcSafetyGroupDefs.h"
|
||||
#include <string>
|
||||
#include "ecmcAxisBase.h"
|
||||
#include <vector>
|
||||
|
||||
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<ecmcAxisBase*> 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_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 <stdexcept>
|
||||
#include "ecmcDataItem.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcMotionPlgDefs.h"
|
||||
#include "inttypes.h"
|
||||
#include <string>
|
||||
#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<double>* fftBuff,
|
||||
size_t elements,
|
||||
int objId);
|
||||
static std::string to_string(int value);
|
||||
|
||||
ecmcDataBuffer<epicsFloat64> *actPosBuffer_;
|
||||
ecmcDataBuffer<epicsFloat64> *setPosBuffer_;
|
||||
ecmcDataBuffer<epicsFloat64> *diffPosBuffer_;
|
||||
ecmcDataBuffer<epicsFloat64> *xPosBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *enableBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *enabledBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *busyBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *executeBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *trajSourceBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *encSourceBuffer_;
|
||||
ecmcDataBuffer<epicsInt8> *atTargetBuffer_;
|
||||
ecmcDataBuffer<epicsInt32> *errorIdBuffer_;
|
||||
ecmcDataBuffer<epicsInt32> *statusWdBuffer_;
|
||||
|
||||
bool bTriggInProgress_;
|
||||
double xdt_;
|
||||
double xTime_;
|
||||
epicsEvent doWriteEvent_;
|
||||
int writeBusy_;
|
||||
timespec writePauseTime_;
|
||||
};
|
||||
|
||||
#endif /* ECMC_MOTION_PLG_H_ */
|
||||
+7
-18
@@ -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_ */
|
||||
|
||||
+53
-75
@@ -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 <vector>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#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<ecmcMotionPlg*> motionObjs;
|
||||
static int motionObjCounter = 0;
|
||||
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
|
||||
static std::vector<ecmcSafetyGroupPlg*> 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<ecmcMotionPlg*>::iterator pMotionObj = motionObjs.begin(); pMotionObj != motionObjs.end(); ++pMotionObj) {
|
||||
if(*pMotionObj) {
|
||||
delete (*pMotionObj);
|
||||
void deleteAllsafetyGroups() {
|
||||
for(std::vector<ecmcSafetyGroupPlg*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
||||
if(*psafetyGroup) {
|
||||
delete (*psafetyGroup);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int linkDataTomotionObjs() {
|
||||
for(std::vector<ecmcMotionPlg*>::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<ecmcSafetyGroupPlg*>::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<ecmcMotionPlg*>::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<ecmcSafetyGroupPlg*>::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());
|
||||
|
||||
+22
-68
@@ -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=<data 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_ */
|
||||
|
||||
Reference in New Issue
Block a user