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-31 12:18:02 +01:00
parent bd5141b9fe
commit 108ce125a9
7 changed files with 214 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
iocsh/el7031.script
View File

@@ -37,7 +37,6 @@ ${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
#
@@ -45,8 +44,8 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT
require ecmc_plugin_safety sandst_a "PLUGIN_ID=0"
ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE",500)
ecmcAddAxisToSafetyGroup("first",1)
ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.1",500)
ecmcAddAxisToSafetyGroup("first",1,1,0)
##############################################################################
############# Configure diagnostics:

7
scripts/startup.cmd
View File

@@ -15,6 +15,13 @@
#-
#################################################################################
#- Print discalimer
#
# IMPORTANT!!!
# This plugin has _NO_ safety rated functionalities.
# The intended use of this plugin is only to handle interfacing with a safety PLC.
#
#- Load plugin: Safety
# Might need differet paths for PSI and ESS.. must check

6
src/ecmcPluginSafety.c
View File

@@ -28,6 +28,7 @@ extern "C" {
static int lastEcmcError = 0;
static int alreadyLoaded = 0;
static int destructs_ = 0;
/** Optional.
* Will be called once after successfull load into ecmc.
@@ -51,6 +52,7 @@ int safetyConstruct(char *configStr)
**/
void safetyDestruct(void)
{
destructs_ = 1;
deleteAllSafetyGroups();
}
@@ -62,6 +64,8 @@ void safetyDestruct(void)
**/
int safetyRealtime(int ecmcError)
{
if(destructs_) return;
executeSafetyGroups();
lastEcmcError = ecmcError;
return 0;
@@ -71,7 +75,7 @@ int safetyRealtime(int ecmcError)
* (for example ecmc PLC variables are defined only at enter of realtime)
**/
int safetyEnterRT(){
return linkDataToSafetyGroups();
return validate();
}
/** Optional function.

175
src/ecmcSafetyGroup.cpp
View File

@@ -56,8 +56,10 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
0) /* Default stack size */
{
sName_ = strdup(name);
sEcRampDownCmdName_ = strdup(ec_rampdown_cmd);
sEcAxesStandStillStat_= strdup(ec_standstill_status);
sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd);
sEcAxesStandStillStatOrg_= strdup(ec_standstill_status);
sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd);
sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status);
sConfig_ = strdup(cfg_string);
delayMs_ = time_delay_ms;
status_ = 0;
@@ -72,27 +74,35 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
bitRampDown_ = 0;
slaveIdStandStill_ = 0;
bitStandStill_ = 0;
aliasRampDown_[0] = 0;
aliasStandStill_[0] = 0;
// Config defaults
cfgDbgMode_ = 0;
parseConfigStr(cfg_string);
initAsyn();
if(cfgDbgMode_) {
printf("Safety group created:\n"
"Name: %s\n"
"I/O for rampdown command from saftey PLC: %s\n"
"I/O for axes standstill status: %s\n"
"STO delay [ms]: %d\n"
"Configuration string: %s\n",
sName_,sEcRampDownCmdName_,sEcAxesStandStillStat_,
printf("Safety: Safety group created:\n"
" Name: %s\n"
" I/O for rampdown command from saftey PLC: %s\n"
" I/O for axes standstill status: %s\n"
" STO delay [ms]: %d\n"
" Configuration string: %s\n",
sName_,sEcRampDownCmdNameOrg_,sEcAxesStandStillStatOrg_,
delayMs_,sConfig_);
}
}
ecmcSafetyGroup::~ecmcSafetyGroup() {
if(cfgDbgMode_) {
printf("Safety: Cleanup\n");
}
free(sName_);
free(sEcRampDownCmdName_);
free(sEcAxesStandStillStat_);
free(sEcRampDownCmdNameOrg_);
free(sEcAxesStandStillStatOrg_);
free(sEcRampDownCmdNameStrip_);
free(sEcAxesStandStillStatStrip_);
free(sConfig_);
}
@@ -123,62 +133,138 @@ void ecmcSafetyGroup::parseConfigStr(const char *configStr) {
}
}
void ecmcSafetyGroup::validate() {
validateCfgs();
connectToDataSources();
validateAxes();
}
void ecmcSafetyGroup::validateCfgs() {
// Validate data sources
char* alias;
int masterIdRampDown=-1;
if(parseEcPath(dataItemRampDownCmd_,
*masterIdRampDown,
*slaveIdRampDown_,
alias,
*bitRampDown_)) {
throw std::runtime_error( "Parse error: Data source for rampdown cmd.");
if(parseEcPath(sEcRampDownCmdNameOrg_,
&masterIdRampDown,
&slaveIdRampDown_,
aliasRampDown_,
&bitRampDown_)) {
throw std::runtime_error( "Safety: Parse error: Data source for rampdown cmd.");
}
int masterIdStandStill=-1;
if(parseEcPath(dataItemStandStillStat_,
*masterIdStandStill,
*slaveIdStandStill_,
alias,
*bitStandStill_)) {
throw std::runtime_error( "Parse error: Data source for standstill status.");
if(parseEcPath(sEcAxesStandStillStatOrg_,
&masterIdStandStill,
&slaveIdStandStill_,
aliasStandStill_,
&bitStandStill_)) {
throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
}
if(masterIdStandStill != masterIdRampDown ) {
throw std::runtime_error( "Parse error: Master id for datasources different.");
throw std::runtime_error( "Safety: Parse error: Master id for datasources different.");
}
masterId_ = masterIdStandStill;
if(bitRampDown_==<0) {
throw std::runtime_error( "Parse error: Rampdown cmd, bit invalid.");
if(bitRampDown_ < 0) {
throw std::runtime_error( "Safety: Parse error: Rampdown cmd, bit invalid.");
}
if(bitStandStill_==<0) {
throw std::runtime_error( "Parse error: Standstill status, bit invalid.");
if(bitStandStill_ < 0) {
throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid.");
}
if(cfgDbgMode_) {
printf("Safety: I/O link parsed:\n"
" name: %s\n"
" masterid: %d\n"
" slaveid: %d\n"
" alias: %s\n"
" bit: %d\n",
sEcRampDownCmdNameOrg_,
masterIdRampDown,
slaveIdRampDown_,
aliasRampDown_,
bitRampDown_);
printf("\n");
printf("Safety: I/O link parsed:\n"
" name: %s\n"
" masterid: %d\n"
" slaveid: %d\n"
" alias: %s\n"
" bit: %d\n",
sEcAxesStandStillStatOrg_,
masterIdStandStill,
slaveIdStandStill_,
aliasStandStill_,
bitStandStill_);
}
// Now bits are idenfied. Now bits need to be removed in order to find data item.
stripBits();
}
void ecmcSafetyGroup::connectToDataSource() {
void ecmcSafetyGroup::stripBits() {
char * lastdot = strrchr(sEcRampDownCmdNameStrip_,'.');
if(lastdot) {
lastdot[0] = 0;
}
else {
throw std::runtime_error( "Safety: Failed trim bit from ramp down I/O link.");
}
if(cfgDbgMode_) {
printf("Safety: I/O bit removed:\n"
" before: %s\n"
" after: %s\n",
sEcRampDownCmdNameOrg_,
sEcRampDownCmdNameStrip_);
}
lastdot = strrchr(sEcAxesStandStillStatStrip_,'.');
if(lastdot) {
lastdot[0] = 0;
}
else {
throw std::runtime_error( "Safety: Failed trim bit from stand still I/O link.");
}
if(cfgDbgMode_) {
printf("Safety: I/O bit removed:\n"
" before: %s\n"
" after: %s\n",
sEcAxesStandStillStatOrg_,
sEcAxesStandStillStatStrip_);
}
}
void ecmcSafetyGroup::validateAxes() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
throw std::runtime_error( "Safety: Axis object NULL.");
}
}
}
void ecmcSafetyGroup::connectToDataSources() {
if( dataSourcesLinked_ ) {
return;
}
// Get dataItem for rampdown command
dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdName_);
dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_);
if(!dataItemRampDownCmd_) {
throw std::runtime_error( "Data item for ramp down command NULL.");
throw std::runtime_error( "Safety: Data item for ramp down command NULL.");
}
// Get dataItem for axes standstill status
dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStat_);
dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_);
if(!dataItemStandStillStat_) {
throw std::runtime_error( "Data item for axes standstill status NULL.");
throw std::runtime_error( "Safety: Data item for axes standstill status NULL.");
}
if(cfgDbgMode_) {
printf("Safety group \"%s\"\": Data sources linked.\n",sName_);
printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_);
}
dataSourcesLinked_ = 1;
@@ -191,7 +277,7 @@ void ecmcSafetyGroup::initAsyn() {
"." + ECMC_PLUGIN_ASYN_SAFETY_STAT;
int *paramId = &asynStatusId_;
if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter mode");
throw std::runtime_error("Safety: Failed create asyn parameter mode");
}
setIntegerParam(*paramId, (epicsInt32)status_);
@@ -221,17 +307,22 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
return asynError;
}
void ecmcSafetyGroup::addAxis(int axisId) {
ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId);
void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId);
if(axis) {
axes_.push_back(axis);
safetyAxis* saxis = new safetyAxis(axis, axisId, veloLimit, standStillTimeMs);
axes_.push_back(saxis);
axesCounter_++;
} else {
throw std::out_of_range("Invalid axis id");
throw std::out_of_range("Safety: Invalid axis id");
}
if(cfgDbgMode_) {
printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_);
}
printf("Added axis %d to safety group \"%s\"\n",axisId,sName_);
return;
}

38
src/ecmcSafetyGroup.h
View File

@@ -20,6 +20,23 @@
#include "ecmcAxisBase.h"
#include <vector>
class safetyAxis {
public:
safetyAxis(ecmcAxisBase* axis,
int axisIndex,
double veloLimit,
int standStillTimeMs) {
veloLimit_ = veloLimit;
axisIndex_ = axisIndex;
axis_ = axis;
standStillTimeMs_ = standStillTimeMs;
}
double veloLimit_;
int axisIndex_;
int standStillTimeMs_;
ecmcAxisBase* axis_;
};
class ecmcSafetyGroup : public asynPortDriver {
public:
@@ -38,13 +55,17 @@ class ecmcSafetyGroup : public asynPortDriver {
~ecmcSafetyGroup();
// Call just before realtime because then all data sources should be available
void connectToDataSource();
void addAxis(int axisId);
void validate();
void addAxis(int axisId, double veloLimit,int standStillTimeMs);
void execute();
virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
std::string getName();
private:
void validateCfgs();
void validateAxes();
void stripBits();
void connectToDataSources();
void parseConfigStr(const char *configStr);
void initAsyn();
double ecmcSampleRateHz_;
@@ -60,8 +81,10 @@ class ecmcSafetyGroup : public asynPortDriver {
int status_;
int axesCounter_;
char* sName_;
char* sEcRampDownCmdName_;
char* sEcAxesStandStillStat_;
char* sEcRampDownCmdNameOrg_;
char* sEcAxesStandStillStatOrg_;
char* sEcRampDownCmdNameStrip_;
char* sEcAxesStandStillStatStrip_;
char* sConfig_;
ecmcDataItem *dataItemRampDownCmd_;
ecmcDataItem *dataItemStandStillStat_;
@@ -71,8 +94,11 @@ class ecmcSafetyGroup : public asynPortDriver {
int bitRampDown_;
int slaveIdStandStill_;
int bitStandStill_;
std::vector<ecmcAxisBase*> axes_;
char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
std::vector<safetyAxis*> axes_;
// Some generic utility functions
static uint8_t getUint8(uint8_t* data);
static int8_t getInt8(uint8_t* data);

43
src/ecmcSafetyPlgWrap.cpp
View File

@@ -71,12 +71,14 @@ int createSafetyGroup(const char *name,
}
int addAxisToSafetyGroup(const char *groupName,
int axisId) {
int axisId,
double veloLimit,
int standStillTimeMs) {
// 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);
(*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs);
return asynSuccess;
}
}
@@ -84,8 +86,10 @@ int addAxisToSafetyGroup(const char *groupName,
return asynError;
}
void deleteAllSafetyGroups() {
return; // The delete process results in seg fault.. need to investigate..
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
if(*psafetyGroup) {
delete (*psafetyGroup);
@@ -93,11 +97,11 @@ void deleteAllSafetyGroups() {
}
}
int linkDataToSafetyGroups() {
int validate() {
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
if(*psafetyGroup) {
try {
(*psafetyGroup)->connectToDataSource();
(*psafetyGroup)->validate();
}
catch(std::exception& e) {
printf("Exception: %s. Plugin will unload.\n",e.what());
@@ -203,10 +207,12 @@ void ecmcAddAxisToSafetyGroupPrintHelp() {
printf(" Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
printf(" <name> : Name of safety group.\n");
printf(" <Axis id> : Axis index to add.\n");
printf(" <velo limit> : Axis standstill velo limit [unit of axis].\n");
printf(" <time> : Time for axis to be below velo limit [ms].\n");
printf("\n");
}
int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int stand_still_time) {
if(!name) {
printf("Error: name.\n");
ecmcAddAxisToSafetyGroupPrintHelp();
@@ -223,8 +229,17 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
exit (1);
}
if(velo_lim < 0) {
printf("Error: Invalid velocity limit.\n");
exit (1);
}
if(stand_still_time < 0) {
printf("Error: Invalid stand still filter time.\n");
exit (1);
}
try {
return addAxisToSafetyGroup(name,axis_id);
return addAxisToSafetyGroup(name,axis_id, velo_lim, stand_still_time);
}
catch(std::exception& e) {
printf("Exception: %s. Add axis to safety group failed.\n",e.what());
@@ -237,14 +252,20 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
static const iocshArg initArg0_2 =
{ "Group name", iocshArgString };
static const iocshArg initArg1_2 =
{ "Axis id", iocshArgInt };
{ "Axis id []", iocshArgInt };
static const iocshArg initArg2_2 =
{ "Velo limit [unit same as axis cfg]", iocshArgDouble };
static const iocshArg initArg3_2 =
{ "Velo stand still filter time [ms]", iocshArgInt };
static const iocshArg *const initArgs_2[] = { &initArg0_2,
&initArg1_2};
&initArg1_2,
&initArg2_2,
&initArg3_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 2, initArgs_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 4, initArgs_2};
static void initCallFunc_2(const iocshArgBuf *args) {
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival);
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival);
}
void ecmcSafetyPlgRegister(void) {

15
src/ecmcSafetyPlgWrap.h
View File

@@ -31,30 +31,21 @@ int setCfgString(const char* cfgString);
*/
//int createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
/** \brief Add motion axis to safety group
*
* \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 addAxisToSafetyGroup(int safetyGroupIndex, int axisIndex);
/** \brief Deletes all created objects\n
*
* Should be called when destructs.\n
*/
void deleteAllSafetyGroups();
/** \brief Link data to _all_ safety objects
/** \brief validate cfgs and link data to _all_ safety groups
*
* This tells the safety 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 sources.\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 linkDataToSafetyGroups();
int validate();
/** \brief Execute all safety groups
*