diff --git a/GNUmakefile b/GNUmakefile index 6f29149..c3eda64 100644 --- a/GNUmakefile +++ b/GNUmakefile @@ -36,7 +36,7 @@ DB_DIR = $(BASE_DIR)/Db SOURCES += $(SRC_DIR)/ecmcPluginSafety.c SOURCES += $(SRC_DIR)/ecmcSafetyPlgWrap.cpp -SOURCES += $(SRC_DIR)/ecmcSafetyGroup.cpp +SOURCES += $(SRC_DIR)/ecmcSS1SafetyGroup.cpp #SOURCES += $(foreach d,${SRC_DIR}, $(wildcard $d/*.c) $(wildcard $d/*.cpp)) HEADERS += $(foreach d,${SRC_DIR}, $(wildcard $d/*.h)) diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSS1SafetyGroup.cpp similarity index 82% rename from src/ecmcSafetyGroup.cpp rename to src/ecmcSS1SafetyGroup.cpp index 479f1c2..302df03 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSS1SafetyGroup.cpp @@ -3,7 +3,7 @@ * ecmc is distributed subject to a Software License Agreement found * in file LICENSE that is included with this distribution. * -* ecmcSafetyGroup.cpp +* ecmcSS1SafetyGroup.cpp * * Created on: July 10, 2023 * Author: anderssandstrom @@ -17,7 +17,7 @@ #define ECMC_PLUGIN_ASYN_SAFETY_STAT "status" #include -#include "ecmcSafetyGroup.h" +#include "ecmcSS1SafetyGroup.h" #include "ecmcPluginClient.h" #include "ecmcAsynPortDriver.h" #include "ecmcAsynPortDriverUtils.h" @@ -34,7 +34,7 @@ extern DBBASE *pdbbase; * - invalid_argument * - runtime_error */ -ecmcSafetyGroup::ecmcSafetyGroup(const char *name, +ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name, const char *ec_rampdown_cmd, const char *ec_standstill_status, int time_delay_ms, @@ -89,6 +89,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, initAsyn(); if(cfgDbgMode_) { printf("Safety %s: Safety group created:\n" + " Type: SS1\n" " Name: %s\n" " I/O for rampdown command from saftey PLC: %s\n" " I/O for axes standstill status: %s\n" @@ -99,7 +100,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, } } -ecmcSafetyGroup::~ecmcSafetyGroup() { +ecmcSS1SafetyGroup::~ecmcSS1SafetyGroup() { if(cfgDbgMode_) { printf("Safety %s: Cleanup\n",sName_); } @@ -112,7 +113,7 @@ ecmcSafetyGroup::~ecmcSafetyGroup() { free(sConfig_); } -void ecmcSafetyGroup::parseConfigStr(const char *configStr) { +void ecmcSS1SafetyGroup::parseConfigStr(const char *configStr) { // check config parameters if (configStr && configStr[0]) { @@ -139,13 +140,13 @@ void ecmcSafetyGroup::parseConfigStr(const char *configStr) { } } -void ecmcSafetyGroup::validate() { +void ecmcSS1SafetyGroup::validate() { validateCfgs(); connectToDataSources(); validateAxes(); } -void ecmcSafetyGroup::validateCfgs() { +void ecmcSS1SafetyGroup::validateCfgs() { // Validate data sources int masterIdRampDown=-1; @@ -179,39 +180,41 @@ void ecmcSafetyGroup::validateCfgs() { throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid."); } - if(cfgDbgMode_) { - 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 %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_, - aliasStandStill_, - bitStandStill_); - } + //if(cfgDbgMode_) { + // printf("Safety %s: I/O link parsed:\n" + // " type: SS1\n" + // " name: %s\n" + // " masterid: %d\n" + // " slaveid: %d\n" + // " alias: %s\n" + // " bit: %d\n", + // sName_, + // sEcRampDownCmdNameOrg_, + // masterIdRampDown, + // slaveIdRampDown_, + // aliasRampDown_, + // bitRampDown_); +// + // printf("Safety %s: I/O link parsed:\n" + // " type: SS1\n" + // " name: %s\n" + // " masterid: %d\n" + // " slaveid: %d\n" + // " alias: %s\n" + // " bit: %d\n", + // sName_, + // sEcAxesStandStillStatOrg_, + // masterIdStandStill, + // slaveIdStandStill_, + // aliasStandStill_, + // bitStandStill_); + //} // Now bits are idenfied. Now bits need to be removed in order to find data item. - stripBits(); + //stripBits(); } -void ecmcSafetyGroup::stripBits() { +void ecmcSS1SafetyGroup::stripBits() { char * lastdot = strrchr(sEcRampDownCmdNameStrip_,'.'); if(lastdot) { lastdot[0] = 0; @@ -247,16 +250,19 @@ void ecmcSafetyGroup::stripBits() { } } -void ecmcSafetyGroup::validateAxes() { +void ecmcSS1SafetyGroup::validateAxes() { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { - if(!*saxis) { + if(!(*saxis)) { throw std::runtime_error( "Safety: Axis object NULL."); } + if(!getAxisValid((*saxis)->axisId_)) { + throw std::runtime_error( "Safety: Ecmc Axis object not valid."); + } } } -void ecmcSafetyGroup::connectToDataSources() { +void ecmcSS1SafetyGroup::connectToDataSources() { ecMaster_ = (ecmcEc*)getEcMaster(); @@ -309,7 +315,7 @@ void ecmcSafetyGroup::connectToDataSources() { //dataSourcesLinked_ = 1; } -void ecmcSafetyGroup::initAsyn() { +void ecmcSS1SafetyGroup::initAsyn() { // Add motion "plugin.motion%d.status" std::string paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) + @@ -325,14 +331,14 @@ void ecmcSafetyGroup::initAsyn() { } // Avoid issues with std:to_string() -std::string ecmcSafetyGroup::to_string(int value) { +std::string ecmcSS1SafetyGroup::to_string(int value) { std::ostringstream os; os << value; return os.str(); } // Executed by ecmc rt thread. -void ecmcSafetyGroup::execute() { +void ecmcSS1SafetyGroup::execute() { uint64_t data = 0; // Read ramp down command from safety plc @@ -373,13 +379,13 @@ void ecmcSafetyGroup::execute() { } } -void ecmcSafetyGroup::resetPrintoutStatus() { +void ecmcSS1SafetyGroup::resetPrintoutStatus() { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { (*saxis)->printEnableStat_ = 1; } } -void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) { +void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) { if(axesAreStandstillOld_ != standstill) { if(standstill) { @@ -397,7 +403,7 @@ void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) { } // Check standstill axes -bool ecmcSafetyGroup::checkAxesStandstill() { +bool ecmcSS1SafetyGroup::checkAxesStandstill() { bool standstill = 1; for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { standstill= standstill && checkAxisStandstill((*saxis)); @@ -406,7 +412,7 @@ bool ecmcSafetyGroup::checkAxesStandstill() { } // Check standstill axis -bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) { +bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) { double velo = 1; int err = getAxisTrajVelo(axis->axisId_, &velo); if(err) { @@ -416,7 +422,7 @@ bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) { } // Check standstill and disable -bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() { +bool ecmcSS1SafetyGroup::checkAxesStandstillAndDisableIfNeeded() { bool standstill = 1; for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { standstill = standstill && checkAxisStandstillAndDisableIfNeeded((*saxis)); @@ -425,7 +431,7 @@ bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() { } // Check standstill and disable -bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) { +bool ecmcSS1SafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) { bool standstill = checkAxisStandstill(axis); if( standstill && rampDownCmd_) { setAxisEnable(axis->axisId_,0); @@ -438,7 +444,7 @@ bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) { } // Disable all axes -void ecmcSafetyGroup::setAxesDisable() { +void ecmcSS1SafetyGroup::setAxesDisable() { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { if(!*saxis) { throw std::runtime_error("Safety: Axis object NULL."); @@ -453,7 +459,7 @@ void ecmcSafetyGroup::setAxesDisable() { } // Set safety interlock -void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) { +void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { if(!*saxis) { throw std::runtime_error( "Safety: Axis object NULL."); @@ -462,7 +468,7 @@ void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) { } } -asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { +asynStatus ecmcSS1SafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { int function = pasynUser->reason; if( function == asynStatusId_ ){ *value = (epicsInt32)status_; @@ -472,7 +478,7 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { return asynError; } -void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) { +void ecmcSS1SafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) { if(!getAxisValid(axisId)) { throw std::out_of_range("Safety: Invalid axis id"); @@ -489,60 +495,60 @@ void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) return; } -std::string ecmcSafetyGroup::getName() { +std::string ecmcSS1SafetyGroup::getName() { return sName_; } -uint8_t ecmcSafetyGroup::getUint8(uint8_t* data) { +uint8_t ecmcSS1SafetyGroup::getUint8(uint8_t* data) { return *data; } -int8_t ecmcSafetyGroup::getInt8(uint8_t* data) { +int8_t ecmcSS1SafetyGroup::getInt8(uint8_t* data) { int8_t* p=(int8_t*)data; return *p; } -uint16_t ecmcSafetyGroup::getUint16(uint8_t* data) { +uint16_t ecmcSS1SafetyGroup::getUint16(uint8_t* data) { uint16_t* p=(uint16_t*)data; return *p; } -int16_t ecmcSafetyGroup::getInt16(uint8_t* data) { +int16_t ecmcSS1SafetyGroup::getInt16(uint8_t* data) { int16_t* p=(int16_t*)data; return *p; } -uint32_t ecmcSafetyGroup::getUint32(uint8_t* data) { +uint32_t ecmcSS1SafetyGroup::getUint32(uint8_t* data) { uint32_t* p=(uint32_t*)data; return *p; } -int32_t ecmcSafetyGroup::getInt32(uint8_t* data) { +int32_t ecmcSS1SafetyGroup::getInt32(uint8_t* data) { int32_t* p=(int32_t*)data; return *p; } -uint64_t ecmcSafetyGroup::getUint64(uint8_t* data) { +uint64_t ecmcSS1SafetyGroup::getUint64(uint8_t* data) { uint64_t* p=(uint64_t*)data; return *p; } -int64_t ecmcSafetyGroup::getInt64(uint8_t* data) { +int64_t ecmcSS1SafetyGroup::getInt64(uint8_t* data) { int64_t* p=(int64_t*)data; return *p; } -float ecmcSafetyGroup::getFloat32(uint8_t* data) { +float ecmcSS1SafetyGroup::getFloat32(uint8_t* data) { float* p=(float*)data; return *p; } -double ecmcSafetyGroup::getFloat64(uint8_t* data) { +double ecmcSS1SafetyGroup::getFloat64(uint8_t* data) { double* p=(double*)data; return *p; } -size_t ecmcSafetyGroup::getEcDataTypeByteSize(ecmcEcDataType dt){ +size_t ecmcSS1SafetyGroup::getEcDataTypeByteSize(ecmcEcDataType dt){ switch(dt) { case ECMC_EC_NONE: return 0; diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSS1SafetyGroup.h similarity index 96% rename from src/ecmcSafetyGroup.h rename to src/ecmcSS1SafetyGroup.h index cb4cecb..3e8dfae 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSS1SafetyGroup.h @@ -23,11 +23,11 @@ class safetyAxis { public: - safetyAxis(int axisIndex, + safetyAxis(int axisId, double veloLimit, int standStillTimeMs) { veloLimit_ = veloLimit; - axisId_ = axisIndex; + axisId_ = axisId; standStillTimeMs_ = standStillTimeMs; printEnableStat_ = 1; } @@ -38,7 +38,7 @@ class safetyAxis { int printEnableStat_; }; -class ecmcSafetyGroup : public asynPortDriver { +class ecmcSS1SafetyGroup : public asynPortDriver { public: /** ecmc Safty Plg class @@ -46,14 +46,14 @@ class ecmcSafetyGroup : public asynPortDriver { * - bad_alloc * - out_of_range */ - ecmcSafetyGroup(const char *name, + ecmcSS1SafetyGroup(const char *name, const char *ec_rampdown_cmd, const char *ec_standstill_status, int time_delay_ms, const char *cfg_string, char* portName); - ~ecmcSafetyGroup(); + ~ecmcSS1SafetyGroup(); // Call just before realtime because then all data sources should be available void validate(); diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp index 39e3370..f7feabc 100644 --- a/src/ecmcSafetyPlgWrap.cpp +++ b/src/ecmcSafetyPlgWrap.cpp @@ -21,12 +21,12 @@ #include #include "ecmcSafetyPlgWrap.h" -#include "ecmcSafetyGroup.h" +#include "ecmcSS1SafetyGroup.h" #define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64 #define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.SAFETY" -static std::vector safetyGroups; +static std::vector safetyGroups; static int safetyGroupsCounter = 0; static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; @@ -42,15 +42,15 @@ int createSafetyGroup(const char *name, const char *ec_standstill_status, int time_delay_ms) { - // create new ecmcSafetyGroup object - ecmcSafetyGroup* safetyGroup = NULL; + // create new ecmcSS1SafetyGroup object + ecmcSS1SafetyGroup* safetyGroup = NULL; // create asynport name for new object () memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS); snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS, ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter); try { - safetyGroup = new ecmcSafetyGroup(name, + safetyGroup = new ecmcSS1SafetyGroup(name, ec_rampdown_cmd, ec_standstill_status, time_delay_ms, @@ -75,7 +75,7 @@ int addAxisToSafetyGroup(const char *groupName, double veloLimit, int standStillTimeMs) { // Find group by name - for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0; if(found) { (*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs); @@ -90,7 +90,7 @@ void deleteAllSafetyGroups() { return; // The delete process results in seg fault.. need to investigate.. - for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { if(*psafetyGroup) { delete (*psafetyGroup); } @@ -98,7 +98,7 @@ void deleteAllSafetyGroups() { } int validate() { - for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { if(*psafetyGroup) { try { (*psafetyGroup)->validate(); @@ -114,7 +114,7 @@ int validate() { int executeSafetyGroups() { - for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { if(*psafetyGroup) { try { (*psafetyGroup)->execute(); @@ -129,11 +129,11 @@ int executeSafetyGroups() { } /** - * EPICS iocsh shell command: ecmcAddSafetyGroup + * EPICS iocsh shell command: ecmcAddSS1SafetyGroup */ -void ecmcAddSafetyGroupPrintHelp() { +void ecmcAddSS1SafetyGroupPrintHelp() { printf("\n"); - printf(" Use ecmcAddSafetyGroup(, , ,)\n"); + printf(" Use ecmcAddSS1SafetyGroup(, , ,)\n"); printf(" : Name of group.\n"); printf(" : Ethercat entry input for rampdown cmd.\n"); printf(" : Ethercat entry output for group standstill status.\n"); @@ -141,27 +141,27 @@ void ecmcAddSafetyGroupPrintHelp() { printf("\n"); } -int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) { +int ecmcAddSS1SafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) { if(!name) { printf("Error: name.\n"); - ecmcAddSafetyGroupPrintHelp(); + ecmcAddSS1SafetyGroupPrintHelp(); return asynError; } if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) { - ecmcAddSafetyGroupPrintHelp(); + ecmcAddSS1SafetyGroupPrintHelp(); return asynSuccess; } if(!ec_rampdown_cmd) { printf("Error: ec_rampdown_cmd ethercat entry not defined.\n"); - ecmcAddSafetyGroupPrintHelp(); + ecmcAddSS1SafetyGroupPrintHelp(); return asynError; } if(!ec_standstill_status) { printf("Error: ec_standstill_status ethercat entry not defined.\n"); - ecmcAddSafetyGroupPrintHelp(); + ecmcAddSS1SafetyGroupPrintHelp(); return asynError; } if(time_delay_ms <= 0) { @@ -194,9 +194,9 @@ static const iocshArg *const initArgs_1[] = { &initArg0_1, &initArg2_1, &initArg3_1}; -static const iocshFuncDef initFuncDef_1 = { "ecmcAddSafetyGroup", 4, initArgs_1 }; +static const iocshFuncDef initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 4, initArgs_1 }; static void initCallFunc_1(const iocshArgBuf *args) { - ecmcAddSafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival); + ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival); } /**