From 0d2c06a84642df7c7f948cdedeb76225d63552c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Fri, 2 Feb 2024 15:46:12 +0100 Subject: [PATCH] WIP --- src/ecmcPluginSafety.c | 2 +- src/ecmcSafetyGroup.cpp | 129 +++++++++++++++++++++++++++++++--------- src/ecmcSafetyGroup.h | 25 ++++---- 3 files changed, 114 insertions(+), 42 deletions(-) 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..ce6c855 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -67,8 +67,11 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, axesCounter_ = 0; ecmcSampleRateHz_ = getEcmcSampleRate(); dataSourcesLinked_ = 0; - dataItemRampDownCmd_ = NULL; - dataItemStandStillStat_ = NULL; + //dataItemRampDownCmd_ = NULL; + //dataItemStandStillStat_ = NULL; + ecMaster_ = NULL; + ecEntryRampDown_ = NULL; + ecEntryStandstill_ = NULL; masterId_ = 0; slaveIdRampDown_ = 0; bitRampDown_ = 0; @@ -76,6 +79,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, bitStandStill_ = 0; aliasRampDown_[0] = 0; aliasStandStill_[0] = 0; + rampDownCmd_ = 0; // Config defaults cfgDbgMode_ = 0; @@ -247,27 +251,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 +327,50 @@ std::string ecmcSafetyGroup::to_string(int value) { // Executed by ecmc rt thread. void ecmcSafetyGroup::execute() { - // xxx + + uint64_t data = 1; + + if(!ecEntryRampDown_) { + return; + } + + if(ecEntryRampDown_->readBit(bitRampDown_, + &data)) { + throw std::out_of_range("Safety: Read rampdown cmd failed"); + } + rampDownCmd_ = data > 0; + + if(rampDownCmd_) { + // set safety interlock in ecmc + setSafetyInterlocks(); + } + +/* + //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); + int setAxisClearEmergencyInterlock(int axisIndex); + int setAxisEmergencyStopInterlock(int axisIndex, + double deceleration); + */ +} + +// Set safety interlock +void ecmcSafetyGroup::setSafetyInterlocks() { + for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { + if(!*saxis) { + throw std::runtime_error( "Safety: Axis object NULL."); + } + setAxisEmergencyStopInterlock((*saxis)->axisIndex_,1000); + } } asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { @@ -309,16 +385,13 @@ 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_); } diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index f0ed774..bf9f3d7 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,13 +64,14 @@ class ecmcSafetyGroup : public asynPortDriver { void validateAxes(); void stripBits(); void connectToDataSources(); + void setSafetyInterlocks(); 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_; // Config options int cfgDbgMode_; // Config: allow dbg printouts @@ -80,7 +79,7 @@ class ecmcSafetyGroup : public asynPortDriver { int asynStatusId_; int status_; int axesCounter_; - std::vector axes_; + std::vector axes_; char* sName_; char* sEcRampDownCmdNameOrg_; @@ -88,8 +87,8 @@ class ecmcSafetyGroup : public asynPortDriver { char* sEcRampDownCmdNameStrip_; char* sEcAxesStandStillStatStrip_; char* sConfig_; - ecmcDataItem *dataItemRampDownCmd_; - ecmcDataItem *dataItemStandStillStat_; + //ecmcDataItem *dataItemRampDownCmd_; + //ecmcDataItem *dataItemStandStillStat_; int delayMs_; int masterId_; int slaveIdRampDown_; @@ -98,9 +97,9 @@ class ecmcSafetyGroup : public asynPortDriver { int bitStandStill_; 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);