diff --git a/GNUmakefile b/GNUmakefile index 3a1f7fb..6f29149 100644 --- a/GNUmakefile +++ b/GNUmakefile @@ -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) diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c index a33f432..c24349d 100644 --- a/src/ecmcPluginSafety.c +++ b/src/ecmcPluginSafety.c @@ -64,7 +64,7 @@ void safetyDestruct(void) **/ int safetyRealtime(int ecmcError) { - if(destructs_) return; + if(destructs_) return 0; executeSafetyGroups(); lastEcmcError = ecmcError; diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index 3ed9290..d3a12a8 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -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::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::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::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; diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index b33d3f8..c51bc59 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -18,23 +18,21 @@ #include "ecmcSafetyPlgDefs.h" #include #include "ecmcAxisBase.h" +#include "ecmcEc.h" #include 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 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 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);