From d572233c504f93176ac8008cc0e2e9c9707478e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Mon, 29 Jan 2024 17:07:17 +0100 Subject: [PATCH] WIP --- GNUmakefile | 2 +- src/ecmcPluginSafety.c | 14 +++++--------- src/ecmcSafetyGroup.cpp | 8 ++++++-- src/ecmcSafetyGroup.h | 3 +++ src/ecmcSafetyPlgWrap.cpp | 16 ++++++++++++++-- src/ecmcSafetyPlgWrap.h | 3 +++ 6 files changed, 32 insertions(+), 14 deletions(-) diff --git a/GNUmakefile b/GNUmakefile index 3787072..fde5f30 100644 --- a/GNUmakefile +++ b/GNUmakefile @@ -41,5 +41,5 @@ 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 +#SCRIPTS += $(BASE_DIR)/scripts/addSafetyAxisToGroup.cmd TEMPLATES += $(wildcard $(DB_DIR)/*.template) diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c index 810554d..d69d763 100644 --- a/src/ecmcPluginSafety.c +++ b/src/ecmcPluginSafety.c @@ -26,25 +26,24 @@ extern "C" { #include "ecmcSafetyPlgDefs.h" #include "ecmcSafetyPlgWrap.h" -static int lastEcmcError = 0; -static char* lastConfStr = NULL; +static int lastEcmcError = 0; +static int alreadyLoaded = 0; -static alreadyLoaded = 0; /** Optional. * Will be called once after successfull load into ecmc. * Return value other than 0 will be considered error. * configStr can be used for configuration parameters. **/ -int safetyConstruct(const char *configStr) +int safetyConstruct(char *configStr) { if(alreadyLoaded) { return ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE; } - lastConfStr = strdup(configStr); alreadyLoaded = 1; - return 0; + + return setCfgString(configStr); } /** Optional function. @@ -53,9 +52,6 @@ int safetyConstruct(const char *configStr) void safetyDestruct(void) { deleteAllSafetyGroups(); - if(lastConfStr){ - free(lastConfStr); - } } /** Optional function. diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index 276ebc1..b060472 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -38,6 +38,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, const char *ec_rampdown_cmd, const char *ec_standstill_status, int time_delay_ms, + const char *cfg_string, char* portName) : asynPortDriver(portName, 1, /* maxAddr */ @@ -57,6 +58,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, name_ = name; ecRampDownCmdName_ = ec_rampdown_cmd; ecAxesStandStillStat_ = ec_standstill_status; + cfgString_ = cfg_string; delayMs_ = time_delay_ms; status_ = 0; asynStatusId_ = -1; @@ -65,11 +67,12 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, dataSourcesLinked_ = 0; // Config defaults cfgDbgMode_ = 0; - initAsyn(); + printf("Safety group %s created: rampdown cmd=%s, axis at standstill=%s,cfg str=%s\n", + name_.c_str(),ecRampDownCmdName_.c_str(),ecAxesStandStillStat_.c_str(),cfgString_.c_str()); } -ecmcSafetyGroup::~ecmcSafetyGroup() { +ecmcSafetyGroup::~ecmcSafetyGroup() { } //void ecmcSafetyGroup::parseConfigStr(char *configStr) { @@ -183,6 +186,7 @@ void ecmcSafetyGroup::addAxis(int axisId) { throw std::out_of_range("Invalid axis id"); } + printf("Added axis %d to safety group \"%s\"\n",axisId,name_.c_str()); return; } diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index 40ab426..832e233 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -32,6 +32,7 @@ class ecmcSafetyGroup : public asynPortDriver { const char *ec_rampdown_cmd, const char *ec_standstill_status, int time_delay_ms, + const char *cfg_string, char* portName); ~ecmcSafetyGroup(); @@ -65,6 +66,8 @@ class ecmcSafetyGroup : public asynPortDriver { std::string name_; std::string ecRampDownCmdName_; std::string ecAxesStandStillStat_; + std::string cfgString_; + int delayMs_; // Some generic utility functions static uint8_t getUint8(uint8_t* data); diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp index f0afeff..ef7baf6 100644 --- a/src/ecmcSafetyPlgWrap.cpp +++ b/src/ecmcSafetyPlgWrap.cpp @@ -30,6 +30,13 @@ static std::vector safetyGroups; static int safetyGroupsCounter = 0; static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; +static const char *configString = NULL; + +int setCfgString(const char* cfgString) { + configString = cfgString; + return 0; +} + int createSafetyGroup(const char *name, const char *ec_rampdown_cmd, const char *ec_standstill_status, @@ -43,7 +50,12 @@ int createSafetyGroup(const char *name, snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS, ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter); try { - safetyGroup = new ecmcSafetyGroup(name, ec_rampdown_cmd,ec_standstill_status,time_delay_ms,portNameBuffer); + safetyGroup = new ecmcSafetyGroup(name, + ec_rampdown_cmd, + ec_standstill_status, + time_delay_ms, + configString, + portNameBuffer); } catch(std::exception& e) { if(safetyGroup) { @@ -165,8 +177,8 @@ int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* } try { + printf("############ 1:\n"); createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms); - } catch(std::exception& e) { printf("Exception: %s. Add safety group failed.\n",e.what()); diff --git a/src/ecmcSafetyPlgWrap.h b/src/ecmcSafetyPlgWrap.h index e6b05a6..e159003 100644 --- a/src/ecmcSafetyPlgWrap.h +++ b/src/ecmcSafetyPlgWrap.h @@ -17,6 +17,9 @@ extern "C" { # endif // ifdef __cplusplus +// Set config string from plugin load +int setCfgString(const char* cfgString); + /** \brief Create new Safetry group * * \param[in] name Name of safety group.\n