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

WIP

Browse Source
This commit is contained in:
sandst_a
2024-01-29 16:23:58 +01:00
parent 51f4b7dfed
commit 7f92b81965
10 changed files with 264 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
iocsh/el7031.script
View File

@@ -37,6 +37,19 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml,LIMIT=1
#
${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT=5000,TYPE=1"
##############################################################################
## Load safety plugin
#
#- PLUGIN_ID = Plugin instansiation index, must be unique for each call
require ecmc_plugin_safety sandst_a "PLUGIN_ID=0"
ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0",500)
ecmcAddAxisToSafetyGroup("first",-10)
ecmcAddAxisToSafetyGroup("first",0)
ecmcAddAxisToSafetyGroup("first",1)
##############################################################################
############# Configure diagnostics:

19
scripts/addSafetyAxisToGroup.cmd
View File

@@ -1,19 +0,0 @@
#==============================================================================
# addSafetyAxisToGroup.cmd
#-------------- Information:
#- Description: ecmc_plugin_safety addSafetyAxisToGroup
#-
#- by Anders Sandström, Paul Scherrer Institute, 2024
#- email: anders.sandstroem@psi.ch
#-
#-###############################################################################
#-
#- Arguments
#- [mandatory]
#- GROUP = Safety group index
#- AXIS = Axis index
#-
#################################################################################
xxxx

20
scripts/startup.cmd
View File

@@ -11,22 +11,14 @@
#-
#- Arguments
#- [mandatory]
#- PLUGIN_ID = Plugin instansiation index, must be unique for each call
#- PLUGIN_ID = Plugin instansiation index, must be unique for each call
#-
#- [optional]
#- AX = Axis id, default 1
#- BUFF_SIZE = Buffer size, default 1000
#- DBG = Debug mode, default 1
#- ENA = Enable operation, default 1
#- REPORT = Printout plugin details, default 1
#################################################################################
#- Load plugin: MOTION
#- Load plugin: Safety
# Only allow call startup.cmd once. if more objects are needed then use addMotionObj.cmd directlly.
# Might need differet paths for PSI and ESS.. must check
epicsEnvSet(ECMC_PLUGIN_FILNAME,"$(ecmc_plugin_safety_DIR)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_safety.so")
epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;")
${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=${PLUGIN_ID=0},FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG=""}', REPORT=${REPORT=1}"
#- add One motion plugin object, only run startup once
${ECMC_PLG_MOTION_INIT=""}${SCRIPTEXEC} $(ecmc_plugin_motion_DIR)addMotionObj.cmd "PLUGIN_ID=${PLUGIN_ID},AX=${AX=1},BUFF_SIZE=${BUFF_SIZE=1000},DBG=${DBG=1},ENA=${ENA=1},REPORT=${REPORT=1}"
epicsEnvSet("ECMC_PLG_MOTION_INIT" ,"#")

20
src/ecmcPluginSafety.c
View File

@@ -29,18 +29,22 @@ extern "C" {
static int lastEcmcError = 0;
static char* lastConfStr = NULL;
static alreadyLoaded = 0;
/** Optional.
* Will be called once after successfull load into ecmc.
* Return value other than 0 will be considered error.
* configStr can be used for configuration parameters.
**/
int safetyConstruct(char *configStr)
int safetyConstruct(const 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 createSafetyGroup(configStr);
if(alreadyLoaded) {
return ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE;
}
lastConfStr = strdup(configStr);
alreadyLoaded = 1;
return 0;
}
/** Optional function.
@@ -71,7 +75,7 @@ int safetyRealtime(int ecmcError)
* (for example ecmc PLC variables are defined only at enter of realtime)
**/
int safetyEnterRT(){
return linkDataToSafetyObjs();
return linkDataToSafetyGroups();
}
/** Optional function.
@@ -117,8 +121,6 @@ struct ecmcPluginData pluginDataDef = {
.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_RAMP_DOWN_ENTRY_OPTION_CMD"<ec entry> : EtherCat entry input for ramp down cmd (bit).\n"
" "ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD"<ec entry> : EtherCat entry output for all axes standstill status (bit).\n"
,
// Plugin version
.version = ECMC_PLUGIN_VERSION,

98
src/ecmcSafetyGroup.cpp
View File

@@ -34,10 +34,12 @@ extern DBBASE *pdbbase;
* - invalid_argument
* - runtime_error
*/
ecmcSafetyGroup::ecmcSafetyGroup(int objIndex, // index of this object (if several is created)
char* configStr,
char* portName)
: asynPortDriver(portName,
ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
int time_delay_ms,
char* portName)
: asynPortDriver(portName,
1, /* maxAddr */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
@@ -52,62 +54,50 @@ ecmcSafetyGroup::ecmcSafetyGroup(int objIndex, // index of this object (
0, /* Default priority */
0) /* Default stack size */
{
status_ = 0;
asynStatusId_ = -1;
axesCounter_ = 0;
ecmcSampleRateHz_ = getEcmcSampleRate();
dataSourcesLinked_ = 0;
name_ = name;
ecRampDownCmdName_ = ec_rampdown_cmd;
ecAxesStandStillStat_ = ec_standstill_status;
delayMs_ = time_delay_ms;
status_ = 0;
asynStatusId_ = -1;
axesCounter_ = 0;
ecmcSampleRateHz_ = getEcmcSampleRate();
dataSourcesLinked_ = 0;
// Config defaults
cfgDbgMode_ = 0;
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);
}
// ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD (string)
if (!strncmp(pThisOption, ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD, strlen(ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD);
printf("RAMP_DOWN_CMD=%s",pThisOption);
//cfgDbgMode_ = atoi(pThisOption);
}
// ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD (string)
if (!strncmp(pThisOption, ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD, strlen(ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD))) {
pThisOption += strlen(ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD);
printf("STANDSTILL_STAT=%s",pThisOption);
//cfgDbgMode_ = atoi(pThisOption);
}
pThisOption = pNextOption;
}
free(pOptions);
}
}
//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))
@@ -196,6 +186,10 @@ void ecmcSafetyGroup::addAxis(int axisId) {
return;
}
std::string ecmcSafetyGroup::getName() {
return name_;
}
uint8_t ecmcSafetyGroup::getUint8(uint8_t* data) {
return *data;
}

18
src/ecmcSafetyGroup.h
View File

@@ -28,9 +28,12 @@ class ecmcSafetyGroup : public asynPortDriver {
* - bad_alloc
* - out_of_range
*/
ecmcSafetyGroup(int objIndex, // index of this object
char* configStr,
ecmcSafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
int time_delay_ms,
char* portName);
~ecmcSafetyGroup();
// Call just before realtime because then all data sources should be available
@@ -38,9 +41,10 @@ class ecmcSafetyGroup : public asynPortDriver {
void addAxis(int axisId);
void execute();
virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
std::string getName();
private:
void parseConfigStr(char *configStr);
//void parseConfigStr(char *configStr);
void initAsyn();
double ecmcSampleRateHz_;
int dataSourcesLinked_; // To avoid link several times
@@ -56,8 +60,12 @@ class ecmcSafetyGroup : public asynPortDriver {
//
int status_;
int axesCounter_;
static std::vector<ecmcAxisBase*> axes_;
std::vector<ecmcAxisBase*> axes_;
std::string name_;
std::string ecRampDownCmdName_;
std::string ecAxesStandStillStat_;
int delayMs_;
// Some generic utility functions
static uint8_t getUint8(uint8_t* data);
static int8_t getInt8(uint8_t* data);

1
src/ecmcSafetyPlg.dbd Normal file
View File

@@ -0,0 +1 @@
registrar("ecmcSafetyPlgRegister")

7
src/ecmcSafetyPlgDefs.h
View File

@@ -14,15 +14,14 @@
#ifndef ECMC_MOTION_PLG_DEFS_H_
#define ECMC_MOTION_PLG_DEFS_H_
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.safety"
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.safety"
// Options
#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT="
#define ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD "RAMP_DOWN_ENTRY="
#define ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD "AXES_STANDSTILL_ENTRY="
#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT="
/** Just one error code in "c" part of plugin
(error handled with exceptions i c++ part) */
#define ECMC_PLUGIN_SAFETY_ERROR_CODE 1
#define ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE 2
#endif /* ECMC_MOTION_PLG_DEFS_H_ */

177
src/ecmcSafetyPlgWrap.cpp
View File

@@ -17,6 +17,9 @@
#include <vector>
#include <stdexcept>
#include <string>
#include <epicsExport.h>
#include <iocsh.h>
#include "ecmcSafetyPlgWrap.h"
#include "ecmcSafetyGroup.h"
@@ -27,7 +30,10 @@ static std::vector<ecmcSafetyGroup*> safetyGroups;
static int safetyGroupsCounter = 0;
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
int createSafetyGroup(char* configStr) {
int createSafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
int time_delay_ms) {
// create new ecmcSafetyGroup object
ecmcSafetyGroup* safetyGroup = NULL;
@@ -37,13 +43,12 @@ int createSafetyGroup(char* configStr) {
snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter);
try {
safetyGroup = new ecmcSafetyGroup(safetyGroupsCounter, configStr, portNameBuffer);
safetyGroup = new ecmcSafetyGroup(name, ec_rampdown_cmd,ec_standstill_status,time_delay_ms,portNameBuffer);
}
catch(std::exception& e) {
if(safetyGroup) {
delete safetyGroup;
}
printf("Exception: %s. Plugin will unload.\n",e.what());
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
}
@@ -53,6 +58,21 @@ int createSafetyGroup(char* configStr) {
return 0;
}
int addAxisToSafetyGroup(const char *groupName,
int axisId) {
// Find group by name
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0;
if(found) {
(*psafetyGroup)->addAxis(axisId);
return asynSuccess;
}
}
return asynError;
}
void deleteAllSafetyGroups() {
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
if(*psafetyGroup) {
@@ -61,16 +81,16 @@ void deleteAllSafetyGroups() {
}
}
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 addAxisToSafetyGroup(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<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
@@ -102,3 +122,134 @@ int executeSafetyGroups() {
}
return 0;
}
/**
* EPICS iocsh shell command: ecmcAddSafetyGroup
*/
void ecmcAddSafetyGroupPrintHelp() {
printf("\n");
printf(" Use ecmcAddSafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)\n");
printf(" <name> : Name of group.\n");
printf(" <ec_rampdown_cmd> : Ethercat entry input for rampdown cmd.\n");
printf(" <ec_standstill_status> : Ethercat entry output for group standstill status.\n");
printf(" <time_delay_ms> : Time delay of STO [ms].\n");
printf("\n");
}
int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
if(!name) {
printf("Error: name.\n");
ecmcAddSafetyGroupPrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcAddSafetyGroupPrintHelp();
return asynSuccess;
}
if(!ec_rampdown_cmd) {
printf("Error: ec_rampdown_cmd ethercat entry not defined.\n");
ecmcAddSafetyGroupPrintHelp();
return asynError;
}
if(!ec_standstill_status) {
printf("Error: ec_standstill_status ethercat entry not defined.\n");
ecmcAddSafetyGroupPrintHelp();
return asynError;
}
if(time_delay_ms <= 0) {
printf("Error: time_delay invalid.\n");
return asynError;
}
try {
createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
}
catch(std::exception& e) {
printf("Exception: %s. Add safety group failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_1 =
{ "Name", iocshArgString };
static const iocshArg initArg1_1 =
{ "ec entry input ramp down cmd", iocshArgString };
static const iocshArg initArg2_1 =
{ "ec entry output axes standstill status", iocshArgString };
static const iocshArg initArg3_1 =
{ "STO delay [ms]", iocshArgInt };
static const iocshArg *const initArgs_1[] = { &initArg0_1,
&initArg1_1,
&initArg2_1,
&initArg3_1};
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSafetyGroup", 4, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
ecmcAddSafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival);
}
/**
* EPICS iocsh shell command: ecmcAddAxisToSafetyGroup
*/
void ecmcAddAxisToSafetyGroupPrintHelp() {
printf("\n");
printf(" Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
printf(" <name> : Name of safety group.\n");
printf(" <Axis id> : Axis index to add.\n");
printf("\n");
}
int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
if(!name) {
printf("Error: name.\n");
ecmcAddAxisToSafetyGroupPrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcAddAxisToSafetyGroupPrintHelp();
return asynSuccess;
}
if(axis_id <= 0) {
printf("Error: Invalid axis id.\n");
return asynError;
}
try {
return addAxisToSafetyGroup(name,axis_id);
}
catch(std::exception& e) {
printf("Exception: %s. Add axis to safety group failed.\n",e.what());
return asynError;
}
return asynSuccess;
}
static const iocshArg initArg0_2 =
{ "Group name", iocshArgString };
static const iocshArg initArg1_2 =
{ "Axis id", iocshArgInt };
static const iocshArg *const initArgs_2[] = { &initArg0_2,
&initArg1_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 2, initArgs_2};
static void initCallFunc_2(const iocshArgBuf *args) {
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival);
}
void ecmcSafetyPlgRegister(void) {
iocshRegister(&initFuncDef_1, initCallFunc_1);
iocshRegister(&initFuncDef_2, initCallFunc_2);
}
epicsExportRegistrar(ecmcSafetyPlgRegister);

11
src/ecmcSafetyPlgWrap.h
View File

@@ -19,11 +19,14 @@ extern "C" {
/** \brief Create new Safetry group
*
* \param[in] configStr Configuration string from load plugin.\n
* \param[in] name Name of safety group.\n
* \param[in] ec_rampdown_cmd Name of ethercat entry for ramp down command\n
* \param[in] ec_standstill_status Name of ethercat entry all axis standstill status\n
* \param[in] time_delay_ms Timedelay between ec_rampdown_cmd going high untill STO is triggered by safety relay.\n
*
* \return 0 if success or otherwise an error code.\n
*/
int createSafetyGroup(char *configStr);
//int createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
/** \brief Add motion axis to safety group
*
@@ -32,7 +35,7 @@ int createSafetyGroup(char *configStr);
*
* \return 0 if success or otherwise an error code.\n
*/
int addMotionAxis(int safetyGroupIndex, int axisIndex);
int addAxisToSafetyGroup(int safetyGroupIndex, int axisIndex);
/** \brief Deletes all created objects\n
*
@@ -48,7 +51,7 @@ void deleteAllSafetyGroups();
* so are only fist accesible now).\n
* \return 0 if success or otherwise an error code.\n
*/
int linkDataToSafetyObjs();
int linkDataToSafetyGroups();
/** \brief Execute all safety groups
*