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

Merge branch 'master' of git.psi.ch:epics_ioc_modules/ecmc_plugin_safety

Browse Source
This commit is contained in:
sandst_a
2024-02-15 16:51:58 +01:00
parent 5923babd63 b091c6e247
commit 2ffa6e36ff
4 changed files with 206 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
GNUmakefile
View File

@@ -42,5 +42,4 @@ SOURCES += $(SRC_DIR)/ecmcSafetyGroup.cpp
HEADERS += $(foreach d,${SRC_DIR}, $(wildcard $d/*.h))
DBDS += $(foreach d,${SRC_DIR}, $(wildcard $d/*.dbd))
SCRIPTS += $(BASE_DIR)/scripts/startup.cmd
#SCRIPTS += $(BASE_DIR)/scripts/addSafetyAxisToGroup.cmd
TEMPLATES += $(wildcard $(DB_DIR)/*.template)

2
src/ecmcPluginSafety.c
View File

@@ -64,7 +64,7 @@ void safetyDestruct(void)
**/
int safetyRealtime(int ecmcError)
{
if(destructs_) return;
if(destructs_) return 0;
executeSafetyGroups();
lastEcmcError = ecmcError;

220
src/ecmcSafetyGroup.cpp
View File

@@ -67,8 +67,9 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
axesCounter_ = 0;
ecmcSampleRateHz_ = getEcmcSampleRate();
dataSourcesLinked_ = 0;
dataItemRampDownCmd_ = NULL;
dataItemStandStillStat_ = NULL;
ecMaster_ = NULL;
ecEntryRampDown_ = NULL;
ecEntryStandstill_ = NULL;
masterId_ = 0;
slaveIdRampDown_ = 0;
bitRampDown_ = 0;
@@ -76,26 +77,31 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
bitStandStill_ = 0;
aliasRampDown_[0] = 0;
aliasStandStill_[0] = 0;
rampDownCmd_ = 0;
axesAreStandstill_ = 0;
rampDownCmdOld_ = 0;
axesAreStandstillOld_ = 0;
printEnableStatus_ = 1;
// Config defaults
cfgDbgMode_ = 0;
parseConfigStr(cfg_string);
initAsyn();
if(cfgDbgMode_) {
printf("Safety: Safety group created:\n"
printf("Safety %s: 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_);
sName_,sName_,sEcRampDownCmdNameOrg_,
sEcAxesStandStillStatOrg_,delayMs_,sConfig_);
}
}
ecmcSafetyGroup::~ecmcSafetyGroup() {
if(cfgDbgMode_) {
printf("Safety: Cleanup\n");
printf("Safety %s: Cleanup\n",sName_);
}
free(sName_);
@@ -174,24 +180,26 @@ void ecmcSafetyGroup::validateCfgs() {
}
if(cfgDbgMode_) {
printf("Safety: I/O link parsed:\n"
printf("Safety %s: I/O link parsed:\n"
" name: %s\n"
" masterid: %d\n"
" slaveid: %d\n"
" alias: %s\n"
" bit: %d\n",
sName_,
sEcRampDownCmdNameOrg_,
masterIdRampDown,
slaveIdRampDown_,
aliasRampDown_,
bitRampDown_);
printf("Safety: I/O link parsed:\n"
printf("Safety %s: I/O link parsed:\n"
" name: %s\n"
" masterid: %d\n"
" slaveid: %d\n"
" alias: %s\n"
" bit: %d\n",
sName_,
sEcAxesStandStillStatOrg_,
masterIdStandStill,
slaveIdStandStill_,
@@ -213,9 +221,10 @@ void ecmcSafetyGroup::stripBits() {
}
if(cfgDbgMode_) {
printf("Safety: I/O bit removed:\n"
printf("Safety %s: I/O bit removed:\n"
" before: %s\n"
" after: %s\n",
sName_,
sEcRampDownCmdNameOrg_,
sEcRampDownCmdNameStrip_);
}
@@ -229,9 +238,10 @@ void ecmcSafetyGroup::stripBits() {
}
if(cfgDbgMode_) {
printf("Safety: I/O bit removed:\n"
printf("Safety %s: I/O bit removed:\n"
" before: %s\n"
" after: %s\n",
sName_,
sEcAxesStandStillStatOrg_,
sEcAxesStandStillStatStrip_);
}
@@ -247,27 +257,56 @@ void ecmcSafetyGroup::validateAxes() {
}
void ecmcSafetyGroup::connectToDataSources() {
if( dataSourcesLinked_ ) {
return;
}
// Get dataItem for rampdown command
dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_);
if(!dataItemRampDownCmd_) {
throw std::runtime_error( "Safety: Data item for ramp down command NULL.");
}
// Get dataItem for axes standstill status
dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_);
if(!dataItemStandStillStat_) {
throw std::runtime_error( "Safety: Data item for axes standstill status NULL.");
}
if(cfgDbgMode_) {
printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_);
ecMaster_ = (ecmcEc*)getEcMaster();
if(!ecMaster_) {
throw std::runtime_error( "Safety: EtherCAT master object NULL.");
}
// rampdown
ecmcEcSlave *slave = ecMaster_->findSlave(slaveIdRampDown_);
if(!slave) {
throw std::runtime_error( "Safety: EtherCAT slave for rampdown I/O NULL.");
}
ecEntryRampDown_ = slave->findEntry(aliasRampDown_);
if(!ecEntryRampDown_) {
throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
}
// standstill
slave = ecMaster_->findSlave(slaveIdStandStill_);
if(!slave) {
throw std::runtime_error( "Safety: EtherCAT slave for standstill I/O NULL.");
}
ecEntryStandstill_ = slave->findEntry(aliasStandStill_);
if(!ecEntryStandstill_) {
throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
};
dataSourcesLinked_ = 1;
return;
//// Get dataItem for rampdown command
//dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_);
//if(!dataItemRampDownCmd_) {
// throw std::runtime_error( "Safety: Data item for ramp down command NULL.");
//}
//
//// Get dataItem for axes standstill status
//dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_);
//if(!dataItemStandStillStat_) {
// throw std::runtime_error( "Safety: Data item for axes standstill status NULL.");
//}
//
//if(cfgDbgMode_) {
// printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_);
//}
//
//dataSourcesLinked_ = 1;
}
void ecmcSafetyGroup::initAsyn() {
@@ -294,7 +333,115 @@ std::string ecmcSafetyGroup::to_string(int value) {
// Executed by ecmc rt thread.
void ecmcSafetyGroup::execute() {
// xxx
uint64_t data = 0;
// Read ramp down command from safety plc
if(ecEntryRampDown_->readBit(bitRampDown_,
&data)) {
// Disable all axes
setAxesDisable(); // disable
setAxesSafetyInterlocks(0); // stop
setAxesStandstillStatus(0); // set output
throw std::out_of_range("Safety: Read rampdown cmd failed");
}
rampDownCmdOld_ = rampDownCmd_;
rampDownCmd_ = data == 0;
if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) {
if(rampDownCmd_) {
printf("Safety %s: Ramp down cmd active\n",sName_);
} else {
printf("Safety %s: Ramp down cmd not active\n",sName_);
}
}
// set safety interlock in ecmc
setAxesSafetyInterlocks(rampDownCmd_);
// check if axes are standstill to safety PLC
axesAreStandstill_ = checkAxesStandstill();
setAxesStandstillStatus(axesAreStandstill_);
// Disable
if(axesAreStandstill_ && rampDownCmd_) {
setAxesDisable();
} else {
printEnableStatus_ = true;
}
/*
//Needed functions in motion.h
int getAxisBusy(int axisIndex,
int *value);
int getAxisEncVelo(int axisIndex,
double *velo);
int getAxisTrajVelo(int axisIndex,
double *velo);
int getAxisEnabled(int axisIndex,
int *value);
int setAxisEnable(int axisIndex,
int value);
*/
}
void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
if(axesAreStandstillOld_ != standstill) {
if(standstill) {
printf("Safety %s: Axes are not moving\n",sName_);
} else {
printf("Safety %s: Axes are moving\n",sName_);
}
}
if(ecEntryStandstill_->writeBit(bitStandStill_,
standstill > 0)) {
throw std::out_of_range("Safety: Read rampdown cmd failed");
}
axesAreStandstillOld_ = standstill;
}
// Check standstill axes
bool ecmcSafetyGroup::checkAxesStandstill() {
bool standstill = 1;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
standstill= standstill && checkAxisStandstill((*saxis)->axisIndex_,(*saxis)->veloLimit_);
}
return standstill;
}
// Check standstill axis
bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) {
double velo = 1;
int err = getAxisTrajVelo(axisId, &velo);
if(err) {
return 0;
}
return std::abs(velo) <= veloLimit;
}
// Set safety interlock
void ecmcSafetyGroup::setAxesDisable() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
throw std::runtime_error("Safety: Axis object NULL.");
}
setAxisEnable((*saxis)->axisIndex_,0);
}
if(printEnableStatus_ && cfgDbgMode_) {
printf("Safety %s: Axes are beeing disabled\n",sName_);
}
printEnableStatus_ = 0;
}
// Set safety interlock
void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
throw std::runtime_error( "Safety: Axis object NULL.");
}
setAxisEmergencyStopInterlock((*saxis)->axisIndex_,stop, 1000);
}
}
asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
@@ -309,18 +456,15 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId);
if(axis) {
safetyAxis* saxis = new safetyAxis(axis, axisId, veloLimit, standStillTimeMs);
axes_.push_back(saxis);
axesCounter_++;
} else {
throw std::out_of_range("Safety: Invalid axis id");
if(!getAxisValid(axisId)) {
throw std::out_of_range("Safety: Invalid axis id");
}
axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));
axesCounter_++;
if(cfgDbgMode_) {
printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_);
printf("Safety %s: Added axis %d to safety group\n",sName_,axisId);
}
return;

34
src/ecmcSafetyGroup.h
View File

@@ -18,23 +18,21 @@
#include "ecmcSafetyPlgDefs.h"
#include <string>
#include "ecmcAxisBase.h"
#include "ecmcEc.h"
#include <vector>
class safetyAxis {
public:
safetyAxis(ecmcAxisBase* axis,
int axisIndex,
safetyAxis(int axisIndex,
double veloLimit,
int standStillTimeMs) {
veloLimit_ = veloLimit;
axisIndex_ = axisIndex;
axis_ = axis;
axisIndex_ = axisIndex;
standStillTimeMs_ = standStillTimeMs;
}
double veloLimit_;
int axisIndex_;
int standStillTimeMs_;
ecmcAxisBase* axis_;
int standStillTimeMs_;
};
class ecmcSafetyGroup : public asynPortDriver {
@@ -66,12 +64,19 @@ class ecmcSafetyGroup : public asynPortDriver {
void validateAxes();
void stripBits();
void connectToDataSources();
void setAxesSafetyInterlocks(int stop);
void setAxesDisable();
void setAxesStandstillStatus(int standstill);
bool checkAxesStandstill();
bool checkAxisStandstill(int axisId, double veloLimit);
void parseConfigStr(const char *configStr);
void initAsyn();
double ecmcSampleRateHz_;
int dataSourcesLinked_; // To avoid link several times
int objectId_; // Unique object id
int cycleCounter_;
int rampDownCmd_;
int rampDownCmdOld_;
// Config options
int cfgDbgMode_; // Config: allow dbg printouts
@@ -80,25 +85,32 @@ class ecmcSafetyGroup : public asynPortDriver {
int asynStatusId_;
int status_;
int axesCounter_;
std::vector<safetyAxis*> axes_;
char* sName_;
char* sEcRampDownCmdNameOrg_;
char* sEcAxesStandStillStatOrg_;
char* sEcRampDownCmdNameStrip_;
char* sEcAxesStandStillStatStrip_;
char* sConfig_;
ecmcDataItem *dataItemRampDownCmd_;
ecmcDataItem *dataItemStandStillStat_;
int delayMs_;
int masterId_;
int slaveIdRampDown_;
int bitRampDown_;
int slaveIdStandStill_;
int bitStandStill_;
int axesAreStandstill_;
int axesAreStandstillOld_;
int printEnableStatus_;
char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
std::vector<safetyAxis*> axes_;
ecmcEc *ecMaster_;
ecmcEcEntry *ecEntryRampDown_;
ecmcEcEntry *ecEntryStandstill_;
// Some generic utility functions
static uint8_t getUint8(uint8_t* data);
static int8_t getInt8(uint8_t* data);