From 7f92b819652fc67098dba79f2aa4ed43f65d82c1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Mon, 29 Jan 2024 16:23:58 +0100
Subject: [PATCH 1/5] WIP

---
 iocsh/el7031.script              |  13 +++
 scripts/addSafetyAxisToGroup.cmd |  19 ----
 scripts/startup.cmd              |  20 ++--
 src/ecmcPluginSafety.c           |  20 ++--
 src/ecmcSafetyGroup.cpp          |  98 ++++++++---------
 src/ecmcSafetyGroup.h            |  18 +++-
 src/ecmcSafetyPlg.dbd            |   1 +
 src/ecmcSafetyPlgDefs.h          |   7 +-
 src/ecmcSafetyPlgWrap.cpp        | 177 ++++++++++++++++++++++++++++---
 src/ecmcSafetyPlgWrap.h          |  11 +-
 10 files changed, 264 insertions(+), 120 deletions(-)
 delete mode 100644 scripts/addSafetyAxisToGroup.cmd
 create mode 100644 src/ecmcSafetyPlg.dbd

diff --git a/iocsh/el7031.script b/iocsh/el7031.script
index 3c63d15..8424603 100644
--- a/iocsh/el7031.script
+++ b/iocsh/el7031.script
@@ -37,6 +37,19 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml,LIMIT=1
 #
 ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT=5000,TYPE=1"
 
+
+##############################################################################
+## Load safety plugin
+#
+#- PLUGIN_ID             = Plugin instansiation index, must be unique for each call
+
+require ecmc_plugin_safety sandst_a "PLUGIN_ID=0"
+
+ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0",500)
+ecmcAddAxisToSafetyGroup("first",-10)
+ecmcAddAxisToSafetyGroup("first",0)
+ecmcAddAxisToSafetyGroup("first",1)
+
 ##############################################################################
 ############# Configure diagnostics:
 
diff --git a/scripts/addSafetyAxisToGroup.cmd b/scripts/addSafetyAxisToGroup.cmd
deleted file mode 100644
index 2b2f190..0000000
--- a/scripts/addSafetyAxisToGroup.cmd
+++ /dev/null
@@ -1,19 +0,0 @@
-
-#==============================================================================
-# addSafetyAxisToGroup.cmd
-#-------------- Information:
-#- Description: ecmc_plugin_safety addSafetyAxisToGroup
-#-
-#- by Anders Sandström, Paul Scherrer Institute, 2024
-#- email: anders.sandstroem@psi.ch
-#-
-#-###############################################################################
-#-
-#- Arguments
-#- [mandatory]
-#- GROUP             = Safety group index
-#- AXIS              = Axis index
-#-
-#################################################################################
-
-xxxx
\ No newline at end of file
diff --git a/scripts/startup.cmd b/scripts/startup.cmd
index 37fb6f6..93036d9 100644
--- a/scripts/startup.cmd
+++ b/scripts/startup.cmd
@@ -11,22 +11,14 @@
 #-
 #- Arguments
 #- [mandatory]
-#- PLUGIN_ID         = Plugin instansiation index, must be unique for each call
+#- PLUGIN_ID             = Plugin instansiation index, must be unique for each call
 #-
-#- [optional]
-#- AX                = Axis id, default 1
-#- BUFF_SIZE         = Buffer size, default 1000
-#- DBG               = Debug mode, default 1
-#- ENA               = Enable operation, default 1
-#- REPORT            = Printout plugin details, default 1
-
 #################################################################################
 
-#- Load plugin: MOTION
+#- Load plugin: Safety
 
-# Only allow call startup.cmd once. if more objects are needed then use addMotionObj.cmd directlly.
+# Might need differet paths for PSI and ESS.. must check
+epicsEnvSet(ECMC_PLUGIN_FILNAME,"$(ecmc_plugin_safety_DIR)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_safety.so")
+epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;")
+${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=${PLUGIN_ID=0},FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG=""}', REPORT=${REPORT=1}"
 
-#- add One motion plugin object, only run startup once
-${ECMC_PLG_MOTION_INIT=""}${SCRIPTEXEC} $(ecmc_plugin_motion_DIR)addMotionObj.cmd "PLUGIN_ID=${PLUGIN_ID},AX=${AX=1},BUFF_SIZE=${BUFF_SIZE=1000},DBG=${DBG=1},ENA=${ENA=1},REPORT=${REPORT=1}"
- 
-epicsEnvSet("ECMC_PLG_MOTION_INIT" ,"#")
diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c
index 024642e..810554d 100644
--- a/src/ecmcPluginSafety.c
+++ b/src/ecmcPluginSafety.c
@@ -29,18 +29,22 @@ extern "C" {
 static int    lastEcmcError   = 0;
 static char*  lastConfStr         = NULL;
 
+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(char *configStr)
+int safetyConstruct(const char *configStr)
 {
-  //This module is allowed to load several times so no need to check if loaded
-
-  // create FFT object and register data callback
-  lastConfStr = strdup(configStr);
-  return createSafetyGroup(configStr);
+  
+  if(alreadyLoaded) {
+    return  ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE;
+  }
+  
+  lastConfStr = strdup(configStr);  
+  alreadyLoaded = 1;
+  return 0;
 }
 
 /** Optional function.
@@ -71,7 +75,7 @@ int safetyRealtime(int ecmcError)
  *  (for example ecmc PLC variables are defined only at enter of realtime)
  **/
 int safetyEnterRT(){
-  return linkDataToSafetyObjs();
+  return linkDataToSafetyGroups();
 }
 
 /** Optional function.
@@ -117,8 +121,6 @@ struct ecmcPluginData pluginDataDef = {
   .desc = "Safety plugin.",
   // Option description
   .optionDesc = "\n    "ECMC_PLUGIN_DBG_PRINT_OPTION_CMD"<1/0>                : Enables/disables printouts from plugin, default = disabled.\n"
-                "    "ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD"<ec entry>       : EtherCat entry input for ramp down cmd (bit).\n"
-                "    "ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD"<ec entry> : EtherCat entry output for all axes standstill status (bit).\n"
                 ,
   // Plugin version
   .version = ECMC_PLUGIN_VERSION,
diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp
index 383de2d..276ebc1 100644
--- a/src/ecmcSafetyGroup.cpp
+++ b/src/ecmcSafetyGroup.cpp
@@ -34,10 +34,12 @@ extern DBBASE *pdbbase;
  *    - invalid_argument
  *    - runtime_error
 */
-ecmcSafetyGroup::ecmcSafetyGroup(int   objIndex,       // index of this object (if several is created)
-                 char* configStr,
-                 char* portName) 
-                  : asynPortDriver(portName,
+ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
+                                 const char *ec_rampdown_cmd,
+                                 const char *ec_standstill_status,
+                                 int time_delay_ms,
+                                 char* portName) 
+                : asynPortDriver(portName,
                    1, /* maxAddr */
                    asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
                    asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
@@ -52,62 +54,50 @@ ecmcSafetyGroup::ecmcSafetyGroup(int   objIndex,       // index of this object (
                    0, /* Default priority */
                    0) /* Default stack size */
                    {
-    
-  status_              = 0;
-  asynStatusId_        = -1;
-  axesCounter_         = 0;
-  ecmcSampleRateHz_    = getEcmcSampleRate();
-  dataSourcesLinked_   = 0;
+  name_                 = name;
+  ecRampDownCmdName_    = ec_rampdown_cmd;
+  ecAxesStandStillStat_ = ec_standstill_status;
+  delayMs_              = time_delay_ms;
+  status_               = 0;
+  asynStatusId_         = -1;
+  axesCounter_          = 0;
+  ecmcSampleRateHz_     = getEcmcSampleRate();
+  dataSourcesLinked_    = 0;
   // Config defaults
-  cfgDbgMode_          = 0;
+  cfgDbgMode_           = 0;
 
-  parseConfigStr(configStr); // Assigns all configs
   initAsyn();
 }
 
 ecmcSafetyGroup::~ecmcSafetyGroup() {   
 }
 
-void ecmcSafetyGroup::parseConfigStr(char *configStr) {
-
-  // check config parameters
-  if (configStr && configStr[0]) {    
-    char *pOptions = strdup(configStr);
-    char *pThisOption = pOptions;
-    char *pNextOption = pOptions;
-    
-    while (pNextOption && pNextOption[0]) {
-      pNextOption = strchr(pNextOption, ';');
-      if (pNextOption) {
-        *pNextOption = '\0'; /* Terminate */
-        pNextOption++;       /* Jump to (possible) next */
-      }
-      
-      // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
-      if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
-        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
-        cfgDbgMode_ = atoi(pThisOption);
-      } 
-
-      // ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD (string)
-      if (!strncmp(pThisOption, ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD, strlen(ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD))) {
-        pThisOption += strlen(ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD);
-        printf("RAMP_DOWN_CMD=%s",pThisOption);
-        //cfgDbgMode_ = atoi(pThisOption);
-      } 
-
-      // ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD (string)
-      if (!strncmp(pThisOption, ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD, strlen(ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD))) {
-        pThisOption += strlen(ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD);
-        printf("STANDSTILL_STAT=%s",pThisOption);
-        //cfgDbgMode_ = atoi(pThisOption);
-      } 
-
-      pThisOption = pNextOption;
-    }
-    free(pOptions);
-  }
-}
+//void ecmcSafetyGroup::parseConfigStr(char *configStr) {
+//
+//  // check config parameters
+//  if (configStr && configStr[0]) {    
+//    char *pOptions = strdup(configStr);
+//    char *pThisOption = pOptions;
+//    char *pNextOption = pOptions;
+//    
+//    while (pNextOption && pNextOption[0]) {
+//      pNextOption = strchr(pNextOption, ';');
+//      if (pNextOption) {
+//        *pNextOption = '\0'; /* Terminate */
+//        pNextOption++;       /* Jump to (possible) next */
+//      }
+//      
+//      // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
+//      if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
+//        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
+//        cfgDbgMode_ = atoi(pThisOption);
+//      } 
+//
+//      pThisOption = pNextOption;
+//    }
+//    free(pOptions);
+//  }
+//}
 
 void ecmcSafetyGroup::connectToDataSource() {
   ///* Check if already linked (one call to enterRT per loaded FFT lib (FFT object))
@@ -196,6 +186,10 @@ void ecmcSafetyGroup::addAxis(int axisId) {
   return;
 }
 
+std::string ecmcSafetyGroup::getName() {
+  return name_;
+}
+
 uint8_t   ecmcSafetyGroup::getUint8(uint8_t* data) {
   return *data;
 }
diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h
index e106dc0..40ab426 100644
--- a/src/ecmcSafetyGroup.h
+++ b/src/ecmcSafetyGroup.h
@@ -28,9 +28,12 @@ class ecmcSafetyGroup : public asynPortDriver {
    *    - bad_alloc
    *    - out_of_range
   */
-  ecmcSafetyGroup(int   objIndex,    // index of this object  
-                  char* configStr,
+  ecmcSafetyGroup(const char *name,
+                  const char *ec_rampdown_cmd,
+                  const char *ec_standstill_status,
+                  int time_delay_ms,
                   char* portName);
+                      
   ~ecmcSafetyGroup();  
 
   // Call just before realtime because then all data sources should be available
@@ -38,9 +41,10 @@ class ecmcSafetyGroup : public asynPortDriver {
   void                  addAxis(int axisId);
   void                  execute();
   virtual asynStatus    readInt32(asynUser *pasynUser, epicsInt32 *value);
+  std::string           getName();
 
  private:
-  void                  parseConfigStr(char *configStr);
+  //void                  parseConfigStr(char *configStr);
   void                  initAsyn();
   double                ecmcSampleRateHz_;
   int                   dataSourcesLinked_;  // To avoid link several times
@@ -56,8 +60,12 @@ class ecmcSafetyGroup : public asynPortDriver {
   //
   int                   status_;
   int                   axesCounter_;
-  static std::vector<ecmcAxisBase*>  axes_;
-  
+
+  std::vector<ecmcAxisBase*>  axes_;
+  std::string           name_;
+  std::string           ecRampDownCmdName_;
+  std::string           ecAxesStandStillStat_;
+  int                   delayMs_;
   // Some generic utility functions
   static uint8_t        getUint8(uint8_t* data);
   static int8_t         getInt8(uint8_t* data);
diff --git a/src/ecmcSafetyPlg.dbd b/src/ecmcSafetyPlg.dbd
new file mode 100644
index 0000000..6c96784
--- /dev/null
+++ b/src/ecmcSafetyPlg.dbd
@@ -0,0 +1 @@
+registrar("ecmcSafetyPlgRegister")
diff --git a/src/ecmcSafetyPlgDefs.h b/src/ecmcSafetyPlgDefs.h
index f120f22..552e90d 100644
--- a/src/ecmcSafetyPlgDefs.h
+++ b/src/ecmcSafetyPlgDefs.h
@@ -14,15 +14,14 @@
 #ifndef ECMC_MOTION_PLG_DEFS_H_
 #define ECMC_MOTION_PLG_DEFS_H_
 
-#define ECMC_PLUGIN_ASYN_PREFIX         "plugin.safety"
+#define ECMC_PLUGIN_ASYN_PREFIX          "plugin.safety"
 
 // Options
-#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD                "DBG_PRINT="
-#define ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD          "RAMP_DOWN_ENTRY="
-#define ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD    "AXES_STANDSTILL_ENTRY="
+#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT="
 
 /** Just one error code in "c" part of plugin 
 (error handled with exceptions i c++ part) */
 #define ECMC_PLUGIN_SAFETY_ERROR_CODE 1
+#define ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE 2
 
 #endif  /* ECMC_MOTION_PLG_DEFS_H_ */
diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp
index 52ef567..f0afeff 100644
--- a/src/ecmcSafetyPlgWrap.cpp
+++ b/src/ecmcSafetyPlgWrap.cpp
@@ -17,6 +17,9 @@
 #include <vector>
 #include <stdexcept>
 #include <string>
+#include <epicsExport.h>
+
+#include <iocsh.h>
 #include "ecmcSafetyPlgWrap.h"
 #include "ecmcSafetyGroup.h"
 
@@ -27,7 +30,10 @@ static std::vector<ecmcSafetyGroup*>  safetyGroups;
 static int safetyGroupsCounter = 0;
 static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
 
-int createSafetyGroup(char* configStr) {
+int createSafetyGroup(const char *name,
+                      const char *ec_rampdown_cmd,
+                      const char *ec_standstill_status,
+                      int time_delay_ms) {
 
   // create new ecmcSafetyGroup object
   ecmcSafetyGroup* safetyGroup = NULL;
@@ -37,13 +43,12 @@ int createSafetyGroup(char* configStr) {
   snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
             ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter);
   try {
-    safetyGroup = new ecmcSafetyGroup(safetyGroupsCounter, configStr, portNameBuffer);
+    safetyGroup = new ecmcSafetyGroup(name, ec_rampdown_cmd,ec_standstill_status,time_delay_ms,portNameBuffer);
   }
   catch(std::exception& e) {
     if(safetyGroup) {
       delete safetyGroup;
     }
-    printf("Exception: %s. Plugin will unload.\n",e.what());
     return ECMC_PLUGIN_SAFETY_ERROR_CODE;
   }
   
@@ -53,6 +58,21 @@ int createSafetyGroup(char* configStr) {
   return 0;
 }
 
+int addAxisToSafetyGroup(const char *groupName, 
+                         int axisId) {
+  // Find group by name
+  for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
+    bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0;   
+    if(found) {
+      (*psafetyGroup)->addAxis(axisId);
+      return asynSuccess;
+    }
+  }
+
+  return asynError;
+}
+
+
 void deleteAllSafetyGroups() {
   for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
     if(*psafetyGroup) {
@@ -61,16 +81,16 @@ void deleteAllSafetyGroups() {
   }
 }
 
-int addMotionAxis(int safetyGroupIndex, int axisIndex) {
-  try {
-    safetyGroups.at(safetyGroupIndex)->addAxis(axisIndex);
-  }
-  catch(std::exception& e) {
-    printf("Exception: %s. Safety object index out of range.\n",e.what());
-    return ECMC_PLUGIN_SAFETY_ERROR_CODE;
-  }
-  return 0;
-}
+//int addAxisToSafetyGroup(int safetyGroupIndex, int axisIndex) {
+//  try {
+//    safetyGroups.at(safetyGroupIndex)->addAxis(axisIndex);
+//  }
+//  catch(std::exception& e) {
+//    printf("Exception: %s. Safety object index out of range.\n",e.what());
+//    return ECMC_PLUGIN_SAFETY_ERROR_CODE;
+//  }
+//  return 0;
+//}
 
 int  linkDataToSafetyGroups() {
   for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
@@ -102,3 +122,134 @@ int executeSafetyGroups() {
   }
   return 0;
 }
+
+/** 
+ * EPICS iocsh shell command: ecmcAddSafetyGroup
+*/
+void ecmcAddSafetyGroupPrintHelp() {
+  printf("\n");
+  printf("       Use ecmcAddSafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)\n");
+  printf("          <name>                     : Name of group.\n");
+  printf("          <ec_rampdown_cmd>          : Ethercat entry input for rampdown cmd.\n");
+  printf("          <ec_standstill_status>     : Ethercat entry output for group standstill status.\n");
+  printf("          <time_delay_ms>            : Time delay of STO [ms].\n");
+  printf("\n");
+}
+
+int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
+  if(!name) {
+    printf("Error: name.\n");
+    ecmcAddSafetyGroupPrintHelp();
+    return asynError;
+  }
+
+  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
+    ecmcAddSafetyGroupPrintHelp();
+    return asynSuccess;
+  }
+
+  if(!ec_rampdown_cmd) {
+    printf("Error: ec_rampdown_cmd ethercat entry not defined.\n");
+    ecmcAddSafetyGroupPrintHelp();
+    return asynError;
+  }
+
+  if(!ec_standstill_status) {
+    printf("Error: ec_standstill_status ethercat entry not defined.\n");
+    ecmcAddSafetyGroupPrintHelp();
+    return asynError;
+  }
+  if(time_delay_ms <= 0) {
+    printf("Error: time_delay invalid.\n");
+    return asynError;
+  }
+
+  try {
+    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());
+    return asynError;
+  }
+
+  return asynSuccess;
+}
+
+static const iocshArg initArg0_1 =
+{ "Name", iocshArgString };
+static const iocshArg initArg1_1 =
+{ "ec entry input ramp down cmd", iocshArgString };
+static const iocshArg initArg2_1 =
+{ "ec entry output axes standstill status", iocshArgString };
+static const iocshArg initArg3_1 =
+{ "STO delay [ms]", iocshArgInt };
+
+static const iocshArg *const initArgs_1[]  = { &initArg0_1, 
+                                               &initArg1_1,
+                                               &initArg2_1,
+                                               &initArg3_1};
+
+static const iocshFuncDef    initFuncDef_1 = { "ecmcAddSafetyGroup", 4, initArgs_1 };
+static void initCallFunc_1(const iocshArgBuf *args) {
+  ecmcAddSafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival);
+}
+
+/** 
+ * EPICS iocsh shell command: ecmcAddAxisToSafetyGroup
+*/
+void ecmcAddAxisToSafetyGroupPrintHelp() {
+  printf("\n");
+  printf("       Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
+  printf("          <name>         : Name of safety group.\n");
+  printf("          <Axis id>      : Axis index to add.\n");
+  printf("\n");
+}
+
+int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
+  if(!name) {
+    printf("Error: name.\n");
+    ecmcAddAxisToSafetyGroupPrintHelp();
+    return asynError;
+  }
+
+  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
+    ecmcAddAxisToSafetyGroupPrintHelp();
+    return asynSuccess;
+  }
+
+  if(axis_id <= 0) {
+    printf("Error: Invalid axis id.\n");
+    return asynError;
+  }
+
+  try {
+    return addAxisToSafetyGroup(name,axis_id);
+  }
+  catch(std::exception& e) {
+    printf("Exception: %s. Add axis to safety group failed.\n",e.what());
+    return asynError;
+  }
+
+  return asynSuccess;
+}
+
+static const iocshArg initArg0_2 =
+{ "Group name", iocshArgString };
+static const iocshArg initArg1_2 =
+{ "Axis id", iocshArgInt };
+
+static const iocshArg *const initArgs_2[]  = { &initArg0_2, 
+                                               &initArg1_2};
+
+static const iocshFuncDef    initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 2, initArgs_2};
+static void initCallFunc_2(const iocshArgBuf *args) {
+  ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival);
+}
+
+void ecmcSafetyPlgRegister(void) {
+  iocshRegister(&initFuncDef_1,  initCallFunc_1);
+  iocshRegister(&initFuncDef_2,  initCallFunc_2);
+}
+
+epicsExportRegistrar(ecmcSafetyPlgRegister);
diff --git a/src/ecmcSafetyPlgWrap.h b/src/ecmcSafetyPlgWrap.h
index 52f60ee..e6b05a6 100644
--- a/src/ecmcSafetyPlgWrap.h
+++ b/src/ecmcSafetyPlgWrap.h
@@ -19,11 +19,14 @@ extern "C" {
 
 /** \brief Create new Safetry group
  *
- *  \param[in] configStr Configuration string from load plugin.\n
+ *  \param[in] name      Name of safety group.\n
+ *  \param[in] ec_rampdown_cmd Name of ethercat entry for ramp down command\n
+ *  \param[in] ec_standstill_status Name of ethercat entry all axis standstill status\n
+ *  \param[in] time_delay_ms Timedelay between ec_rampdown_cmd going high untill STO is triggered by safety relay.\n
  *
  *  \return 0 if success or otherwise an error code.\n
  */
-int createSafetyGroup(char *configStr);
+//int createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
 
 /** \brief Add motion axis to safety group
  *
@@ -32,7 +35,7 @@ int createSafetyGroup(char *configStr);
  *
  *  \return 0 if success or otherwise an error code.\n
  */
-int addMotionAxis(int safetyGroupIndex, int axisIndex);
+int addAxisToSafetyGroup(int safetyGroupIndex, int axisIndex);
 
 /** \brief Deletes all created objects\n
  *
@@ -48,7 +51,7 @@ void deleteAllSafetyGroups();
  *  so are only fist accesible now).\n
  *  \return 0 if success or otherwise an error code.\n
  */
-int  linkDataToSafetyObjs();
+int  linkDataToSafetyGroups();
 
 /** \brief Execute all safety groups
  *

From d572233c504f93176ac8008cc0e2e9c9707478e6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Mon, 29 Jan 2024 17:07:17 +0100
Subject: [PATCH 2/5] 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<ecmcSafetyGroup*>  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

From bd5141b9fe9a6e8706b04f1f05d4a0d71e544a9b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Tue, 30 Jan 2024 17:06:41 +0100
Subject: [PATCH 3/5] WIP, not compiling

---
 iocsh/el7031.script       |   4 +-
 scripts/startup.cmd       |   3 +-
 src/ecmcSafetyGroup.cpp   | 179 ++++++++++++++++++++++++--------------
 src/ecmcSafetyGroup.h     |  24 ++---
 src/ecmcSafetyPlgWrap.cpp |  20 +----
 5 files changed, 132 insertions(+), 98 deletions(-)

diff --git a/iocsh/el7031.script b/iocsh/el7031.script
index 8424603..9c88bfe 100644
--- a/iocsh/el7031.script
+++ b/iocsh/el7031.script
@@ -45,9 +45,7 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT
 
 require ecmc_plugin_safety sandst_a "PLUGIN_ID=0"
 
-ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0",500)
-ecmcAddAxisToSafetyGroup("first",-10)
-ecmcAddAxisToSafetyGroup("first",0)
+ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE",500)
 ecmcAddAxisToSafetyGroup("first",1)
 
 ##############################################################################
diff --git a/scripts/startup.cmd b/scripts/startup.cmd
index 93036d9..999cba3 100644
--- a/scripts/startup.cmd
+++ b/scripts/startup.cmd
@@ -11,7 +11,7 @@
 #-
 #- Arguments
 #- [mandatory]
-#- PLUGIN_ID             = Plugin instansiation index, must be unique for each call
+#- PLUGIN_ID                 : Plugin instansiation index, must be unique for each call
 #-
 #################################################################################
 
@@ -21,4 +21,3 @@
 epicsEnvSet(ECMC_PLUGIN_FILNAME,"$(ecmc_plugin_safety_DIR)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_safety.so")
 epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;")
 ${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=${PLUGIN_ID=0},FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG=""}', REPORT=${REPORT=1}"
-
diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp
index b060472..1b950ac 100644
--- a/src/ecmcSafetyGroup.cpp
+++ b/src/ecmcSafetyGroup.cpp
@@ -55,88 +55,133 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
                    0, /* Default priority */
                    0) /* Default stack size */
                    {
-  name_                 = name;
-  ecRampDownCmdName_    = ec_rampdown_cmd;
-  ecAxesStandStillStat_ = ec_standstill_status;
-  cfgString_            = cfg_string;
+  sName_                = strdup(name);
+  sEcRampDownCmdName_   = strdup(ec_rampdown_cmd);
+  sEcAxesStandStillStat_= strdup(ec_standstill_status);
+  sConfig_              = strdup(cfg_string);
   delayMs_              = time_delay_ms;
   status_               = 0;
   asynStatusId_         = -1;
   axesCounter_          = 0;
   ecmcSampleRateHz_     = getEcmcSampleRate();
   dataSourcesLinked_    = 0;
+  dataItemRampDownCmd_  = NULL;
+  dataItemStandStillStat_ = NULL;
+  masterId_             = 0;
+  slaveIdRampDown_      = 0;
+  bitRampDown_          = 0;
+  slaveIdStandStill_    = 0;
+  bitStandStill_        = 0;
+  
   // Config defaults
   cfgDbgMode_           = 0;
+  parseConfigStr(cfg_string);
   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());
+  if(cfgDbgMode_) {
+    printf("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_,sEcRampDownCmdName_,sEcAxesStandStillStat_,
+           delayMs_,sConfig_);
+  }
 }
 
 ecmcSafetyGroup::~ecmcSafetyGroup() {
+  free(sName_);
+  free(sEcRampDownCmdName_);
+  free(sEcAxesStandStillStat_);
+  free(sConfig_);
 }
 
-//void ecmcSafetyGroup::parseConfigStr(char *configStr) {
-//
-//  // check config parameters
-//  if (configStr && configStr[0]) {    
-//    char *pOptions = strdup(configStr);
-//    char *pThisOption = pOptions;
-//    char *pNextOption = pOptions;
-//    
-//    while (pNextOption && pNextOption[0]) {
-//      pNextOption = strchr(pNextOption, ';');
-//      if (pNextOption) {
-//        *pNextOption = '\0'; /* Terminate */
-//        pNextOption++;       /* Jump to (possible) next */
-//      }
-//      
-//      // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
-//      if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
-//        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
-//        cfgDbgMode_ = atoi(pThisOption);
-//      } 
-//
-//      pThisOption = pNextOption;
-//    }
-//    free(pOptions);
-//  }
-//}
+void ecmcSafetyGroup::parseConfigStr(const char *configStr) {
+
+  // check config parameters
+  if (configStr && configStr[0]) {    
+    char *pOptions = strdup(configStr);
+    char *pThisOption = pOptions;
+    char *pNextOption = pOptions;
+    
+    while (pNextOption && pNextOption[0]) {
+      pNextOption = strchr(pNextOption, ';');
+      if (pNextOption) {
+        *pNextOption = '\0'; /* Terminate */
+        pNextOption++;       /* Jump to (possible) next */
+      }
+      
+      // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
+      if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
+        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
+        cfgDbgMode_ = atoi(pThisOption);
+      } 
+
+      pThisOption = pNextOption;
+    }
+    free(pOptions);
+  }
+}
+
+void ecmcSafetyGroup::validateCfgs() {
+  
+  // Validate data sources
+  char* alias;
+  int masterIdRampDown=-1;
+  if(parseEcPath(dataItemRampDownCmd_,
+                *masterIdRampDown,
+                *slaveIdRampDown_,
+                alias,
+                *bitRampDown_)) {
+    throw std::runtime_error( "Parse error: Data source for rampdown cmd.");
+  }
+
+  int masterIdStandStill=-1;
+  if(parseEcPath(dataItemStandStillStat_,
+                *masterIdStandStill,
+                *slaveIdStandStill_,
+                alias,
+                *bitStandStill_)) {
+    throw std::runtime_error( "Parse error: Data source for standstill status.");
+  }
+
+  if(masterIdStandStill != masterIdRampDown ) {
+    throw std::runtime_error( "Parse error: Master id for datasources different.");
+  }
+  masterId_ = masterIdStandStill;
+
+  if(bitRampDown_==<0) {
+    throw std::runtime_error( "Parse error: Rampdown cmd, bit invalid.");
+  }
+
+  if(bitStandStill_==<0) {
+    throw std::runtime_error( "Parse error: Standstill status, bit invalid.");
+  }
+
+}
 
 void ecmcSafetyGroup::connectToDataSource() {
-  ///* Check if already linked (one call to enterRT per loaded FFT lib (FFT object))
-  //    But link should only happen once!!*/
-  //if( dataSourcesLinked_ ) {
-  //  return;
-  //}
-  //
-  //// Get dataItem
-  //dataItem_        = (ecmcDataItem*) getEcmcDataItem(cfgDataSourceStr_);
-  //if(!dataItem_) {
-  //  throw std::runtime_error( "Data item NULL." );
-  //}
-  //
-  //dataItemInfo_ = dataItem_->getDataItemInfo();
-//
-  //// Register data callback
-  //callbackHandle_ = dataItem_->regDataUpdatedCallback(f_dataUpdatedCallback, this);
-  //if (callbackHandle_ < 0) {
-  //  throw std::runtime_error( "Failed to register data source callback.");
-  //}
-//
-  //// Check data source
-  //if( !dataTypeSupported(dataItem_->getEcmcDataType()) ) {
-  //  throw std::invalid_argument( "Data type not supported." );
-  //}
-//
-  //// Add oversampling
-  //cfgDataSampleRateHz_ = cfgSampleRateHz_ * dataItem_->getEcmcDataSize()/dataItem_->getEcmcDataElementSize();
-  //setDoubleParam(asynSRateId_, cfgDataSampleRateHz_);
-  //callParamCallbacks();
-//
- // dataSourcesLinked_ = 1;
- // updateStatus(IDLE);
+  if( dataSourcesLinked_ ) {
+    return;
+  }
+  
+  // Get dataItem for rampdown command
+  dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdName_);
+  if(!dataItemRampDownCmd_) {
+    throw std::runtime_error( "Data item for ramp down command NULL.");
+  }
+  
+  // Get dataItem for axes standstill status
+  dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStat_);
+  if(!dataItemStandStillStat_) {
+    throw std::runtime_error( "Data item for axes standstill status NULL.");
+  }
+  
+  if(cfgDbgMode_) {
+    printf("Safety group \"%s\"\": Data sources linked.\n",sName_);
+  }
 
- dataSourcesLinked_ = 1;
+  dataSourcesLinked_ = 1;
 }
 
 void ecmcSafetyGroup::initAsyn() {
@@ -186,12 +231,12 @@ 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());
+  printf("Added axis %d to safety group \"%s\"\n",axisId,sName_);
   return;
 }
 
 std::string ecmcSafetyGroup::getName() {
-  return name_;
+  return sName_;
 }
 
 uint8_t   ecmcSafetyGroup::getUint8(uint8_t* data) {
diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h
index 832e233..cc19854 100644
--- a/src/ecmcSafetyGroup.h
+++ b/src/ecmcSafetyGroup.h
@@ -45,7 +45,7 @@ class ecmcSafetyGroup : public asynPortDriver {
   std::string           getName();
 
  private:
-  //void                  parseConfigStr(char *configStr);
+  void                  parseConfigStr(const char *configStr);
   void                  initAsyn();
   double                ecmcSampleRateHz_;
   int                   dataSourcesLinked_;  // To avoid link several times
@@ -57,18 +57,22 @@ class ecmcSafetyGroup : public asynPortDriver {
 
   // Asyn
   int                   asynStatusId_;
-
-  //
   int                   status_;
   int                   axesCounter_;
-
-  std::vector<ecmcAxisBase*>  axes_;
-  std::string           name_;
-  std::string           ecRampDownCmdName_;
-  std::string           ecAxesStandStillStat_;
-  std::string           cfgString_;
-
+  char*                 sName_;
+  char*                 sEcRampDownCmdName_;
+  char*                 sEcAxesStandStillStat_;
+  char*                 sConfig_;
+  ecmcDataItem         *dataItemRampDownCmd_;    
+  ecmcDataItem         *dataItemStandStillStat_;
   int                   delayMs_;
+  int                   masterId_;
+  int                   slaveIdRampDown_;
+  int                   bitRampDown_;
+  int                   slaveIdStandStill_;
+  int                   bitStandStill_;
+  std::vector<ecmcAxisBase*>  axes_;
+  
   // Some generic utility functions
   static uint8_t        getUint8(uint8_t* data);
   static int8_t         getInt8(uint8_t* data);
diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp
index ef7baf6..442075c 100644
--- a/src/ecmcSafetyPlgWrap.cpp
+++ b/src/ecmcSafetyPlgWrap.cpp
@@ -93,17 +93,6 @@ void deleteAllSafetyGroups() {
   }
 }
 
-//int addAxisToSafetyGroup(int safetyGroupIndex, int axisIndex) {
-//  try {
-//    safetyGroups.at(safetyGroupIndex)->addAxis(axisIndex);
-//  }
-//  catch(std::exception& e) {
-//    printf("Exception: %s. Safety object index out of range.\n",e.what());
-//    return ECMC_PLUGIN_SAFETY_ERROR_CODE;
-//  }
-//  return 0;
-//}
-
 int  linkDataToSafetyGroups() {
   for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
     if(*psafetyGroup) {
@@ -173,16 +162,15 @@ int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char*
   }
   if(time_delay_ms <= 0) {
     printf("Error: time_delay invalid.\n");
-    return asynError;
+    exit (1);
   }
 
   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());
-    return asynError;
+    exit (1);    
   }
 
   return asynSuccess;
@@ -232,7 +220,7 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
 
   if(axis_id <= 0) {
     printf("Error: Invalid axis id.\n");
-    return asynError;
+    exit (1);
   }
 
   try {
@@ -240,7 +228,7 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
   }
   catch(std::exception& e) {
     printf("Exception: %s. Add axis to safety group failed.\n",e.what());
-    return asynError;
+    exit (1);
   }
 
   return asynSuccess;

From 108ce125a95fb60a39671cd734ade2df7471b92d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Wed, 31 Jan 2024 12:18:02 +0100
Subject: [PATCH 4/5] WIP

---
 iocsh/el7031.script       |   5 +-
 scripts/startup.cmd       |   7 ++
 src/ecmcPluginSafety.c    |   6 +-
 src/ecmcSafetyGroup.cpp   | 175 +++++++++++++++++++++++++++++---------
 src/ecmcSafetyGroup.h     |  38 +++++++--
 src/ecmcSafetyPlgWrap.cpp |  43 +++++++---
 src/ecmcSafetyPlgWrap.h   |  15 +---
 7 files changed, 214 insertions(+), 75 deletions(-)

diff --git a/iocsh/el7031.script b/iocsh/el7031.script
index 9c88bfe..693f0c0 100644
--- a/iocsh/el7031.script
+++ b/iocsh/el7031.script
@@ -37,7 +37,6 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml,LIMIT=1
 #
 ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT=5000,TYPE=1"
 
-
 ##############################################################################
 ## Load safety plugin
 #
@@ -45,8 +44,8 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT
 
 require ecmc_plugin_safety sandst_a "PLUGIN_ID=0"
 
-ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE",500)
-ecmcAddAxisToSafetyGroup("first",1)
+ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.1",500)
+ecmcAddAxisToSafetyGroup("first",1,1,0)
 
 ##############################################################################
 ############# Configure diagnostics:
diff --git a/scripts/startup.cmd b/scripts/startup.cmd
index 999cba3..89b6af6 100644
--- a/scripts/startup.cmd
+++ b/scripts/startup.cmd
@@ -15,6 +15,13 @@
 #-
 #################################################################################
 
+#- Print discalimer
+#
+# IMPORTANT!!! 
+# This plugin has _NO_ safety rated functionalities.
+# The intended use of this plugin is only to handle interfacing with a safety PLC.
+#
+
 #- Load plugin: Safety
 
 # Might need differet paths for PSI and ESS.. must check
diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c
index d69d763..a33f432 100644
--- a/src/ecmcPluginSafety.c
+++ b/src/ecmcPluginSafety.c
@@ -28,6 +28,7 @@ extern "C" {
 
 static int lastEcmcError = 0;
 static int alreadyLoaded = 0;
+static int destructs_ = 0;
 
 /** Optional. 
  *  Will be called once after successfull load into ecmc.
@@ -51,6 +52,7 @@ int safetyConstruct(char *configStr)
  **/
 void safetyDestruct(void)
 {
+  destructs_ = 1;
   deleteAllSafetyGroups();
 }
 
@@ -62,6 +64,8 @@ void safetyDestruct(void)
  **/
 int safetyRealtime(int ecmcError)
 { 
+  if(destructs_) return;
+  
   executeSafetyGroups();
   lastEcmcError = ecmcError;
   return 0;
@@ -71,7 +75,7 @@ int safetyRealtime(int ecmcError)
  *  (for example ecmc PLC variables are defined only at enter of realtime)
  **/
 int safetyEnterRT(){
-  return linkDataToSafetyGroups();
+  return validate();
 }
 
 /** Optional function.
diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp
index 1b950ac..783faa6 100644
--- a/src/ecmcSafetyGroup.cpp
+++ b/src/ecmcSafetyGroup.cpp
@@ -56,8 +56,10 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
                    0) /* Default stack size */
                    {
   sName_                = strdup(name);
-  sEcRampDownCmdName_   = strdup(ec_rampdown_cmd);
-  sEcAxesStandStillStat_= strdup(ec_standstill_status);
+  sEcRampDownCmdNameOrg_   = strdup(ec_rampdown_cmd);
+  sEcAxesStandStillStatOrg_= strdup(ec_standstill_status);
+  sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd);
+  sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status);
   sConfig_              = strdup(cfg_string);
   delayMs_              = time_delay_ms;
   status_               = 0;
@@ -72,27 +74,35 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
   bitRampDown_          = 0;
   slaveIdStandStill_    = 0;
   bitStandStill_        = 0;
+  aliasRampDown_[0]     = 0;
+  aliasStandStill_[0]   = 0;
   
   // Config defaults
   cfgDbgMode_           = 0;
   parseConfigStr(cfg_string);
   initAsyn();
   if(cfgDbgMode_) {
-    printf("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_,sEcRampDownCmdName_,sEcAxesStandStillStat_,
+    printf("Safety: 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_);
   }
 }
 
 ecmcSafetyGroup::~ecmcSafetyGroup() {
+  if(cfgDbgMode_) {
+    printf("Safety: Cleanup\n");
+  }
+
   free(sName_);
-  free(sEcRampDownCmdName_);
-  free(sEcAxesStandStillStat_);
+  free(sEcRampDownCmdNameOrg_);
+  free(sEcAxesStandStillStatOrg_);
+  free(sEcRampDownCmdNameStrip_);
+  free(sEcAxesStandStillStatStrip_);
   free(sConfig_);
 }
 
@@ -123,62 +133,138 @@ void ecmcSafetyGroup::parseConfigStr(const char *configStr) {
   }
 }
 
+void ecmcSafetyGroup::validate() { 
+  validateCfgs();
+  connectToDataSources();
+  validateAxes();
+}
+
 void ecmcSafetyGroup::validateCfgs() {
-  
+
   // Validate data sources
-  char* alias;
   int masterIdRampDown=-1;
-  if(parseEcPath(dataItemRampDownCmd_,
-                *masterIdRampDown,
-                *slaveIdRampDown_,
-                alias,
-                *bitRampDown_)) {
-    throw std::runtime_error( "Parse error: Data source for rampdown cmd.");
+  if(parseEcPath(sEcRampDownCmdNameOrg_,
+                &masterIdRampDown,
+                &slaveIdRampDown_,
+                aliasRampDown_,
+                &bitRampDown_)) {
+    throw std::runtime_error( "Safety: Parse error: Data source for rampdown cmd.");
   }
 
   int masterIdStandStill=-1;
-  if(parseEcPath(dataItemStandStillStat_,
-                *masterIdStandStill,
-                *slaveIdStandStill_,
-                alias,
-                *bitStandStill_)) {
-    throw std::runtime_error( "Parse error: Data source for standstill status.");
+  if(parseEcPath(sEcAxesStandStillStatOrg_,
+                &masterIdStandStill,
+                &slaveIdStandStill_,
+                aliasStandStill_,
+                &bitStandStill_)) {    
+    throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
   }
 
   if(masterIdStandStill != masterIdRampDown ) {
-    throw std::runtime_error( "Parse error: Master id for datasources different.");
+    throw std::runtime_error( "Safety: Parse error: Master id for datasources different.");
   }
   masterId_ = masterIdStandStill;
 
-  if(bitRampDown_==<0) {
-    throw std::runtime_error( "Parse error: Rampdown cmd, bit invalid.");
+  if(bitRampDown_ < 0) {
+    throw std::runtime_error( "Safety: Parse error: Rampdown cmd, bit invalid.");
   }
 
-  if(bitStandStill_==<0) {
-    throw std::runtime_error( "Parse error: Standstill status, bit invalid.");
+  if(bitStandStill_ < 0) {
+    throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid.");
   }
 
+  if(cfgDbgMode_) {
+    printf("Safety: I/O link parsed:\n" 
+           "    name:     %s\n"
+           "    masterid: %d\n"
+           "    slaveid:  %d\n"
+           "    alias:    %s\n"
+           "    bit:      %d\n",
+           sEcRampDownCmdNameOrg_,
+           masterIdRampDown,
+           slaveIdRampDown_,
+           aliasRampDown_,
+           bitRampDown_);
+    printf("\n");
+    printf("Safety: I/O link parsed:\n" 
+           "    name:     %s\n"
+           "    masterid: %d\n"
+           "    slaveid:  %d\n"
+           "    alias:    %s\n"
+           "    bit:      %d\n",
+           sEcAxesStandStillStatOrg_,
+           masterIdStandStill,
+           slaveIdStandStill_,
+           aliasStandStill_,
+           bitStandStill_);
+  }
+
+  // Now bits are idenfied. Now bits need to be removed in order to find data item.
+  stripBits();
 }
 
-void ecmcSafetyGroup::connectToDataSource() {
+void ecmcSafetyGroup::stripBits() {
+  char * lastdot = strrchr(sEcRampDownCmdNameStrip_,'.');
+  if(lastdot) {
+    lastdot[0] = 0;
+  }
+  else {
+    throw std::runtime_error( "Safety: Failed trim bit from ramp down I/O link.");
+  }
+
+  if(cfgDbgMode_) {
+    printf("Safety: I/O bit removed:\n" 
+           "    before:     %s\n"
+           "    after:      %s\n",
+           sEcRampDownCmdNameOrg_,
+           sEcRampDownCmdNameStrip_);
+  }
+
+  lastdot = strrchr(sEcAxesStandStillStatStrip_,'.');
+  if(lastdot) {
+    lastdot[0] = 0;
+  }
+  else {
+    throw std::runtime_error( "Safety: Failed trim bit from stand still I/O link.");
+  }
+  
+  if(cfgDbgMode_) {
+    printf("Safety: I/O bit removed:\n" 
+           "    before:     %s\n"
+           "    after:      %s\n",
+           sEcAxesStandStillStatOrg_,
+           sEcAxesStandStillStatStrip_);
+  }
+}
+
+void ecmcSafetyGroup::validateAxes() {
+
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    if(!*saxis) {
+      throw std::runtime_error( "Safety: Axis object NULL.");
+    }
+  }
+}
+
+void ecmcSafetyGroup::connectToDataSources() {
   if( dataSourcesLinked_ ) {
     return;
   }
   
   // Get dataItem for rampdown command
-  dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdName_);
+  dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_);
   if(!dataItemRampDownCmd_) {
-    throw std::runtime_error( "Data item for ramp down command NULL.");
+    throw std::runtime_error( "Safety: Data item for ramp down command NULL.");
   }
   
   // Get dataItem for axes standstill status
-  dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStat_);
+  dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_);
   if(!dataItemStandStillStat_) {
-    throw std::runtime_error( "Data item for axes standstill status NULL.");
+    throw std::runtime_error( "Safety: Data item for axes standstill status NULL.");
   }
   
   if(cfgDbgMode_) {
-    printf("Safety group \"%s\"\": Data sources linked.\n",sName_);
+    printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_);
   }
 
   dataSourcesLinked_ = 1;
@@ -191,7 +277,7 @@ void ecmcSafetyGroup::initAsyn() {
              "." + ECMC_PLUGIN_ASYN_SAFETY_STAT;
   int *paramId = &asynStatusId_;
   if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) {
-    throw std::runtime_error("Failed create asyn parameter mode");
+    throw std::runtime_error("Safety: Failed create asyn parameter mode");
   }
   setIntegerParam(*paramId, (epicsInt32)status_);
 
@@ -221,17 +307,22 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
   return asynError;
 }
 
-void ecmcSafetyGroup::addAxis(int axisId) {
-  ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId);
+void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
 
+  ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId);
+  
   if(axis) {
-    axes_.push_back(axis);
+    safetyAxis* saxis = new safetyAxis(axis, axisId, veloLimit, standStillTimeMs);
+    axes_.push_back(saxis);
     axesCounter_++;
   } else {
-    throw std::out_of_range("Invalid axis id"); 
+    throw std::out_of_range("Safety: Invalid axis id"); 
+  }
+  
+  if(cfgDbgMode_) {
+    printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_);
   }
 
-  printf("Added axis %d to safety group \"%s\"\n",axisId,sName_);
   return;
 }
 
diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h
index cc19854..b33d3f8 100644
--- a/src/ecmcSafetyGroup.h
+++ b/src/ecmcSafetyGroup.h
@@ -20,6 +20,23 @@
 #include "ecmcAxisBase.h"
 #include <vector>
 
+class safetyAxis {
+  public:
+  safetyAxis(ecmcAxisBase* axis,
+             int axisIndex,
+             double veloLimit,
+             int standStillTimeMs) {
+    veloLimit_ = veloLimit;
+    axisIndex_ = axisIndex;
+    axis_ = axis;
+    standStillTimeMs_ = standStillTimeMs;
+  }
+  double        veloLimit_;
+  int           axisIndex_;
+  int           standStillTimeMs_;
+  ecmcAxisBase* axis_;
+};
+
 class ecmcSafetyGroup : public asynPortDriver {
  public:
 
@@ -38,13 +55,17 @@ class ecmcSafetyGroup : public asynPortDriver {
   ~ecmcSafetyGroup();  
 
   // Call just before realtime because then all data sources should be available
-  void                  connectToDataSource();
-  void                  addAxis(int axisId);
+  void                  validate();
+  void                  addAxis(int axisId, double veloLimit,int standStillTimeMs);
   void                  execute();
   virtual asynStatus    readInt32(asynUser *pasynUser, epicsInt32 *value);
   std::string           getName();
 
  private:
+  void                  validateCfgs();
+  void                  validateAxes();
+  void                  stripBits();
+  void                  connectToDataSources();
   void                  parseConfigStr(const char *configStr);
   void                  initAsyn();
   double                ecmcSampleRateHz_;
@@ -60,8 +81,10 @@ class ecmcSafetyGroup : public asynPortDriver {
   int                   status_;
   int                   axesCounter_;
   char*                 sName_;
-  char*                 sEcRampDownCmdName_;
-  char*                 sEcAxesStandStillStat_;
+  char*                 sEcRampDownCmdNameOrg_;
+  char*                 sEcAxesStandStillStatOrg_;
+  char*                 sEcRampDownCmdNameStrip_;
+  char*                 sEcAxesStandStillStatStrip_;
   char*                 sConfig_;
   ecmcDataItem         *dataItemRampDownCmd_;    
   ecmcDataItem         *dataItemStandStillStat_;
@@ -71,8 +94,11 @@ class ecmcSafetyGroup : public asynPortDriver {
   int                   bitRampDown_;
   int                   slaveIdStandStill_;
   int                   bitStandStill_;
-  std::vector<ecmcAxisBase*>  axes_;
-  
+  char                  aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
+  char                  aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
+
+  std::vector<safetyAxis*>  axes_;
+
   // Some generic utility functions
   static uint8_t        getUint8(uint8_t* data);
   static int8_t         getInt8(uint8_t* data);
diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp
index 442075c..39e3370 100644
--- a/src/ecmcSafetyPlgWrap.cpp
+++ b/src/ecmcSafetyPlgWrap.cpp
@@ -71,12 +71,14 @@ int createSafetyGroup(const char *name,
 }
 
 int addAxisToSafetyGroup(const char *groupName, 
-                         int axisId) {
+                         int axisId,
+                         double veloLimit,
+                         int standStillTimeMs) {
   // Find group by name
   for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
     bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0;   
     if(found) {
-      (*psafetyGroup)->addAxis(axisId);
+      (*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs);
       return asynSuccess;
     }
   }
@@ -84,8 +86,10 @@ int addAxisToSafetyGroup(const char *groupName,
   return asynError;
 }
 
-
 void deleteAllSafetyGroups() {
+  
+  return;  // The delete process results in seg fault.. need to investigate..
+
   for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
     if(*psafetyGroup) {
       delete (*psafetyGroup);
@@ -93,11 +97,11 @@ void deleteAllSafetyGroups() {
   }
 }
 
-int  linkDataToSafetyGroups() {
+int validate() {
   for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
     if(*psafetyGroup) {
       try {
-        (*psafetyGroup)->connectToDataSource();
+        (*psafetyGroup)->validate();
       }
       catch(std::exception& e) {
         printf("Exception: %s. Plugin will unload.\n",e.what());
@@ -203,10 +207,12 @@ void ecmcAddAxisToSafetyGroupPrintHelp() {
   printf("       Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
   printf("          <name>         : Name of safety group.\n");
   printf("          <Axis id>      : Axis index to add.\n");
+  printf("          <velo limit>   : Axis standstill velo limit [unit of axis].\n");
+  printf("          <time>         : Time for axis to be below velo limit [ms].\n");
   printf("\n");
 }
 
-int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
+int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int stand_still_time) {
   if(!name) {
     printf("Error: name.\n");
     ecmcAddAxisToSafetyGroupPrintHelp();
@@ -223,8 +229,17 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
     exit (1);
   }
 
+  if(velo_lim < 0) {
+    printf("Error: Invalid velocity limit.\n");
+    exit (1);
+  }
+
+  if(stand_still_time < 0) {
+    printf("Error: Invalid stand still filter time.\n");
+    exit (1);
+  }
   try {
-    return addAxisToSafetyGroup(name,axis_id);
+    return addAxisToSafetyGroup(name,axis_id, velo_lim, stand_still_time);
   }
   catch(std::exception& e) {
     printf("Exception: %s. Add axis to safety group failed.\n",e.what());
@@ -237,14 +252,20 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id) {
 static const iocshArg initArg0_2 =
 { "Group name", iocshArgString };
 static const iocshArg initArg1_2 =
-{ "Axis id", iocshArgInt };
+{ "Axis id []", iocshArgInt };
+static const iocshArg initArg2_2 =
+{ "Velo limit [unit same as axis cfg]", iocshArgDouble };
+static const iocshArg initArg3_2 =
+{ "Velo stand still filter time [ms]", iocshArgInt };
 
 static const iocshArg *const initArgs_2[]  = { &initArg0_2, 
-                                               &initArg1_2};
+                                               &initArg1_2,
+                                               &initArg2_2,
+                                               &initArg3_2};
 
-static const iocshFuncDef    initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 2, initArgs_2};
+static const iocshFuncDef    initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 4, initArgs_2};
 static void initCallFunc_2(const iocshArgBuf *args) {
-  ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival);
+  ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival);
 }
 
 void ecmcSafetyPlgRegister(void) {
diff --git a/src/ecmcSafetyPlgWrap.h b/src/ecmcSafetyPlgWrap.h
index e159003..b9592b0 100644
--- a/src/ecmcSafetyPlgWrap.h
+++ b/src/ecmcSafetyPlgWrap.h
@@ -31,30 +31,21 @@ int setCfgString(const char* cfgString);
  */
 //int createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
 
-/** \brief Add motion axis to safety group
- *
- *  \param[in] safetyGroupIndex Safty group index
- *  \param[in] axisIndex Axis index to add to safety group
- *
- *  \return 0 if success or otherwise an error code.\n
- */
-int addAxisToSafetyGroup(int safetyGroupIndex, int axisIndex);
-
 /** \brief Deletes all created objects\n
  *
  * Should be called when destructs.\n
  */
 void deleteAllSafetyGroups();
 
-/** \brief Link data to _all_ safety objects
+/** \brief validate cfgs and link data to _all_ safety groups
  *
- *  This tells the safety lib to connect to ecmc to find it's data source.\n
+ *  This tells the safety lib to connect to ecmc to find it's data sources.\n
  *  This function should be called just before entering realtime since then all\n
  *  data sources in ecmc will be definded (plc sources are compiled just before runtime\n
  *  so are only fist accesible now).\n
  *  \return 0 if success or otherwise an error code.\n
  */
-int  linkDataToSafetyGroups();
+int  validate();
 
 /** \brief Execute all safety groups
  *

From bbc018b19fd55465fd8f872329eda75b2fca7ab9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Wed, 31 Jan 2024 12:27:57 +0100
Subject: [PATCH 5/5] USe el7041 as test instead

---
 iocsh/cfg/axis.yaml                         |   4 +-
 iocsh/cfg/axis_latch.yaml                   | 204 --------------------
 iocsh/cfg/el7031.ax                         |  93 ---------
 iocsh/{el7031.script => el7042-0052.script} |  10 +-
 src/ecmcSafetyGroup.cpp                     |   2 +-
 5 files changed, 6 insertions(+), 307 deletions(-)
 delete mode 100644 iocsh/cfg/axis_latch.yaml
 delete mode 100644 iocsh/cfg/el7031.ax
 rename iocsh/{el7031.script => el7042-0052.script} (89%)

diff --git a/iocsh/cfg/axis.yaml b/iocsh/cfg/axis.yaml
index 7dd90c9..9462689 100644
--- a/iocsh/cfg/axis.yaml
+++ b/iocsh/cfg/axis.yaml
@@ -133,9 +133,9 @@ trajectory:
 
 input:
   limit:
-    forward: ec0.s$(DRV_SLAVE).ONE.0                  # Ethercat entry for low limit switch input
+    forward: ec0.s$(DRV_SLAVE).driveStatus01.12       # Ethercat entry for low limit switch input
   #  forwardPolarity: 0                               # Polarity of forward limit switch
-    backward: ec0.s$(DRV_SLAVE).ONE.0                # Ethercat entry for high limit switch input
+    backward: ec0.s$(DRV_SLAVE).driveStatus01.11       # Ethercat entry for high limit switch input
   #  backwardPolarity: 0                              # Polarity of forward limit switch
   home: 'ec0.s$(DRV_SLAVE).ONE.0'                     # Ethercat entry for home switch
   # homePolarity: 0                                   # Polarity of home switch
diff --git a/iocsh/cfg/axis_latch.yaml b/iocsh/cfg/axis_latch.yaml
deleted file mode 100644
index ebf8745..0000000
--- a/iocsh/cfg/axis_latch.yaml
+++ /dev/null
@@ -1,204 +0,0 @@
-macros: LIMIT=10000,TYPE=0,DRV_SLAVE=${DRV_SLAVE}     # Macros for all below (evaluated before jinja linter)
-
-axis:
-  id: 1                                               # Axis id
-  type: joint                                         # this is for future selection of axis type
-  # mode: CSV                                         # supported mode, CSV and CSP, defaults CSV
-  # parameters: 'axisPar' # additional parameters     # Additional params to motor record driver
-  #healthOutput: ec0...                               # Ethercat entry for health output
-  # autoMode:                                         # Switch drive modes automaticaly for normal motion and homing (smaract for instance)
-  #   modeSet: ec0..                                  # Ethercat entry drive mode write (set CSV,CSP,homing)
-  #   modeAct: ec0..                                  # Ethercat entry drive mode reading (set CSV,CSP,homing)
-  #   modeCmdMotion: 9                                # Drive mode value for normal motion (written to axis.drvMode.modeSet when normal motion)
-  #   modeCmdHome: 10                                 # Drive mode value for when homing (written to axis.drvMode.modeSet when homing)
-  # features:
-  #   blockCom: false                                 # Block communication to axis
-  #   allowedFunctions:
-  #     homing: true                                  # Allow homing
-  #     constantVelocity: true                        # Allow constant velocity
-  #     positioning: true                             # Allow positioning
-
-epics:
-  name: Axis1                                         # Axis anme
-  precision: 3                                        # Decimal count
-  description: very important motor axis              # Axis description
-  unit: mm                                            # Unit
-  # motorRecord:
-  #   enable: true
-  #   description: This is MR
-  #   fieldInit: 'RRES=1.0,RTRY=2,RMOD=1,UEIP=0,RDBD=0.1,URIP=1,RDBL=$(IOC):$(ECMC_MOTOR_NAME)-PosActSim' # Extra config for Motor record
-
-drive:
-  numerator: 3600                                     # Fastest speed in engineering units
-  denominator: 32768                                  # I/O range for ECMC_EC_ALIAS_DRV_VELO_SET
-  # type: 0                                           # Stepper: 0. DS402: 1 (DS402 = servos and advanced stepper drives)
-  control: ec0.s$(DRV_SLAVE).driveControl01           # Control word ethercat entry
-  enable: 0                                           # Enable bit index in control word (not used if DS402)
-  enabled: 1                                          # Enabled bit index in status word (not used if DS402)
-  status: ec0.s$(DRV_SLAVE).driveStatus01             # Status word ethercat entry
-  setpoint: ec0.s$(DRV_SLAVE).velocitySetpoint01      # Velocity setpoint if CSV. Position setpoint if CSP
-#  reduceTorque: 2                                    # Reduce torque bit in drive control word
-#  reduceTorqueEnable: True                           # Enable reduce torque functionality
-#  brake:
-#    enable: true                                     # Enable brake
-#    output: ec0...                                   # Ethercat link to brake output
-#    openDelay: 0                                     # Brake timing parameter in cycles (default 1kHz)
-#    closeAhead: 0                                    # Brake timing parameter in cycles (default 1kHz)
-#  reset: 1                                           # Reset   (if no drive reset bit then leave empty)
-#  warning: 2                                         # Warning (if no drive warning bit then leave empty)
-#  error:                                             # max 3
-#    - 3                                              # Error 0 (if no drive error bit then leave empty)
-#    - 7                                              # Error 1 (if no drive error bit then leave empty)
-#    - 14                                             # Error 2 (if no drive error bit then leave empty)
-
-encoder:
-  numerator: 360                                      # Scaling numerator example 360 deg/rev
-  denominator: 12800                                  # Scaling denominator example 4096 ticks per 360 degree
-  # type: 0                                           # Type: 0=Incremental, 1=Absolute
-  bits: 16                                            # Total bit count of encoder raw data
-  # absBits: 0                                        # Absolute bit count (for absolute encoders) always least significant part of 'bits'
-  # absOffset: 0                                      # Encoder offset in eng units (for absolute encoders)
-  # mask: '0xFFF00'                                   # Mask applied to raw encoder value
-  position: ec0.s$(DRV_SLAVE).positionActual01        # Ethercat entry for actual position input (encoder)
-  control: ec0.s$(DRV_SLAVE).encoderControl01         # mandatory only if 'reset' is used
-  status: ec0.s$(DRV_SLAVE).encoderStatus01           # mandatory only if 'warning' or 'error' are used
-  # ready: 10                                         # Bit in encoder status word for encoder ready
-  # source: 0                                         # 0 = Encoder value from etehrcat hardware, 1 = Encoder value from PLC
-  # reset: 1                                          # Reset   (optional)
-  # warning: 2                                        # Warning (optional)
-  # error:                                            # max 3 (optional)
-  #   - 5                                             # Error 0
-  #   - 9                                             # Error 1
-  #   - 11                                            # Error 2
-  # filter:
-  #   velocity:
-  #     size: 100                                     # Filter size for velocity
-  #     enable: true                                  # enable velocity filter
-  #   position:
-  #     size: 100                                     # Filter size for encoder value
-  #     enable: true                                  # enable encoder value filter
-  latch:
-    position: 'ec0.s$(DRV_SLAVE).encoderLatchPostion01'                                   # Link to latched value. Used for some homing seqs
-    control: 0                                     # Bit in encoder control word to arm latch. Used for some homing seqs
-    status: 0                                      # Bit in encoder status word for latch triggered status. Used for some homing seqs
-  primary: 1                                       # Use this encoder as primary (for control)
-  homing:
-    type: 12                                       # Homing sequence type
-    position: -30                                  # Position to reference encoder to
-    velocity:
-      to: 10                                       # Velocity to cam/sensor (used for some homing seqs)
-      from: 5                                      # Velocity from cam/sensor (used for some homing seqs)
-    acceleration: 20                               # Acceleration during homing
-    deceleration: 100                              # Deceleration during homing
-  #    refToEncIDAtStartup: 1                         # At startup then set the start value of this encoder to actpos of this encoder id
-    refAtHome: 1                                   # If homing is executed then set position of this encoder
-  #    tolToPrim: 0                                   # If set then this is the max allowed tolerance between prim encoder and this encoder
-  #    postMoveEnable: yes                            # Enable move after successfull homing
-  #    postMovePosition: 10                           # Position to move to after successfull homing
-  #    trigg: ec0..                                   # Ethercat entry for triggering drive internal homing seq (seq id 26)
-  #    ready: ec0..                                   # Ethercat entry for reading drive internal homing seq ready (seq id 26)
-    latchCount: 1
-
-controller:
-  Kp: 1                                              # Kp proportinal gain
-  Ki: 0.02                                          # Ki integral gain
-  Kd: 0                                             # Kd derivative gain
-  # Kff: 1                                            # Feed forward gain
-  # deadband:
-  #   tol: 0.01                                       # Stop control if within this distance from target for the below time
-  #   time: 100
-  # limits:
-  #   minOutput: -100                                 # Minimum controller output
-  #   maxOutput: 100                                  # Maximum controller output
-  #   minIntegral: -100                               # Minimum integral output
-  #   maxIntegral: 100                                # Maximum integral output
-  # inner:
-  #   Kp: 0.1                                         # Kp for when close to target
-  #   Ki: 0.1                                         # Ki for when close to target
-  #   Kd: 0.1                                         # Kd for when close to target
-  #   tol: 0.1                                        # Distance from target for when inner PID params will be used, defaults to atTarget tol
-
-trajectory:
-  # type: 1                                           # Default 0 = trapetz, 1 = S-curve (ruckig)
-  axis:
-    velocity: 100                                     # Default velo for axis
-    acceleration: 100                                 # Default acc for axis
-    deceleration: 100                                 # Default dec for axis
-    # emergencyDeceleration: 0.05                     # Deceleration when axis in error state
-    jerk: 10                                          # Default jerk for axis
-  jog:
-    velocity: 5                                       # Default velo fro JOG (motor record)
-#  modulo:
-#    range: 360                                       # Modulo range 0..360
-#    type: 0                                          # Modulo type
-
-input:
-  limit:
-    forward: ec0.s$(DRV_SLAVE).ONE.0                  # Ethercat entry for low limit switch input
-  #  forwardPolarity: 0                               # Polarity of forward limit switch
-    backward: ec0.s$(DRV_SLAVE).ONE.0                # Ethercat entry for high limit switch input
-  #  backwardPolarity: 0                              # Polarity of forward limit switch
-  home: 'ec0.s$(DRV_SLAVE).ONE.0'                     # Ethercat entry for home switch
-  # homePolarity: 0                                   # Polarity of home switch
-  interlock: 'ec0.s$(DRV_SLAVE).ONE.0'                # Ethercat entry for interlock switch input
-  # interlockPolarity: 0                              # Polarity of interlock switch
-
-# homing:
-#   type: 3                                           # Homing sequence type
-#   position: -30                                     # Position to reference encoder to
-#   velocity:
-#     to: 10                                          # Velocity to cam/sensor (used for some homing seqs)
-#     from: 5                                         # Velocity from cam/sensor (used for some homing seqs)
-#   acc: 20                                           # Acceleration during homing
-#   dec: 100                                          # Deceleration during homing
-#   refToEncIDAtStartup: 1                            # At startup then set the start value of this encoder to actpos of this encoder id
-#   refAtHome: 1                                      # If homing is executed then set position of this encoder
-#   tolToPrim: 0                                      # If set then this is the max allowed tolerance between prim encoder and this encoder
-#   postMoveEnable: yes                               # Enable move after successfull homing
-#   postMovePosition: 10                              # Position to move to after successfull homing
-#   timeout: 100                                      # Sequence timeout
-
-softlimits:
-  enable: false                                       # Enable soft limits
-  forward: 100                                        # Soft limit position fwd
-  forwardEnable: false                                # Soft limit position fwd enable
-  backward: -100                                      # Soft limit position bwd
-  backwardEnable: false                               # Soft limit position bwd enable
-
-monitoring:
-  lag:
-    enable: true                                      # Enable position lag monitoring (following error)
-    tolerance: 2                                      # Allowed tolerance
-    time: 10                                          # Allowed time outside tolerance  target:
-    enable: false                                     # Enable at target monitoring (needs to be enabled if using motor record)
-    tolerance: 0.5                                    # Allowed tolerance
-    time: 10                                          # Filter time inside tolerance to be at target
-  velocity:
-    enable: false                                     # Enable velocity monitoring
-    max: 100                                          # Allowed max velocity
-    time:
-      trajectory: 100                                 # Time allowed outside max velo before system init halt
-      drive: 200                                      # Time allowed outside max velo before system disables drive
-  # velocityDifference:
-  #   enable: true                                    # Enable velocity diff monitoring (velo set vs velo act)
-  #   max: 100                                        # Allowed max difference
-  #   time:
-  #     trajectory: 100                               # Time allowed outside max diff velo before system init halt
-  #     drive: 200                                    # Time allowed outside max diff velo before system disables drive
-
-plc:
-  enable: true                                        # Enable axis plc
-  externalCommands: true                              # Allow axis to inputs from PLC
-  file: ${PLC_PATH}test.plc
-  code:                                                # Sync code
-  - "if(static.counter % ${LIMIT} == 0){println('AX PLC: Appended');static.counter:=0;};" # calculate set pos for physical axis
-  - "if(not(static.plc_code_loaded)) {static.counter:=static.counter+1;};"
-  - ec0.s$(DRV_SLAVE).ONE > 1; # Enable axis if one of master axes is enabled
-  - ec0.s$(DRV_SLAVE).ZERO > 1; # calculate set pos for physical axis
-#  velocity_filter:                                    # Filter used to smother velocity feedforward
-#    encoder:                                          # Filter plc enc velo
-#      enable: false                                   # Filter enable
-#      size: 100                                       # Filter size
-#    trajectory:                                       # Filter plc traj velo
-#      enable: false                                   # Filter enable
-#      size: 100                                       # Filter size
diff --git a/iocsh/cfg/el7031.ax b/iocsh/cfg/el7031.ax
deleted file mode 100644
index 230ffd9..0000000
--- a/iocsh/cfg/el7031.ax
+++ /dev/null
@@ -1,93 +0,0 @@
-#General
-epicsEnvSet("ECMC_MOTOR_NAME",            "Axis1")
-epicsEnvSet("ECMC_R",                     "Axis1-")
-epicsEnvSet("ECMC_AXIS_NO",               "1")
-epicsEnvSet("ECMC_DESC",                  "Test EL7037")
-epicsEnvSet("ECMC_EGU",                   "deg")                       # Motor Record Unit
-epicsEnvSet("ECMC_PREC",                  "3")                        # Motor Record Precision
-epicsEnvSet("ECMC_AXISCONFIG",            "")                         # Extra parameters to driver
-epicsEnvSet("ECMC_EC_AXIS_HEALTH",        "")                         # Entry for axis health output (example: ec0.s1.binaryOutput01.0)
-epicsEnvSet("ECMC_MOD_RANGE" ,            "0")                        # Modulo range (traj setpoints and encoder values will be in range 0..ECMC_MOD_RANGE)
-epicsEnvSet("ECMC_MOD_TYPE",              "0")                        # For positioning and MOD_RANGE is larger than 0: 0 = Normal, 1 = Always Fwd, 2 = Always Bwd, 3 = Closest Distance
-
-#Encoder
-epicsEnvSet("ECMC_ENC_SCALE_NUM"          "360")
-epicsEnvSet("ECMC_ENC_SCALE_DENOM"        "12800")
-epicsEnvSet("ECMC_ENC_TYPE"               "0")                        # Type: 0=Incremental, 1=Absolute
-epicsEnvSet("ECMC_ENC_BITS"               "16")                       # Total bit count of encoder raw data
-epicsEnvSet("ECMC_ENC_ABS_BITS",          "0")                        # Absolute bit count (for absolute encoders) always least significant part of ECMC_ENC_BITS
-epicsEnvSet("ECMC_ENC_ABS_OFFSET"         "0")                        # Encoder offset in eng units (for absolute encoders)
-epicsEnvSet("ECMC_EC_ENC_ACTPOS",         "ec0.s$(DRV_ID).positionActual01")         # Ethercat entry for actual position input (encoder)
-epicsEnvSet("ECMC_EC_ENC_RESET",          "")                         # Reset   (if no encoder reset bit then leave empty)
-epicsEnvSet("ECMC_EC_ENC_ALARM_0",        "")                         # Error 0 (if no encoder error bit then leave empty)
-epicsEnvSet("ECMC_EC_ENC_ALARM_1",        "")                         # Error 1 (if no encoder error bit then leave empty)
-epicsEnvSet("ECMC_EC_ENC_ALARM_2",        "")                         # Error 2 (if no encoder error bit then leave empty)
-epicsEnvSet("ECMC_EC_ENC_WARNING",        "")                         # Warning (if no encoder warning bit then leave empty)
-
-#Drive
-epicsEnvSet("ECMC_DRV_TYPE"               "0")                                           # Stepper: 0. DS402: 1 (DS402 = servos and advanced stepper drives)
-epicsEnvSet("ECMC_DRV_SCALE_NUM"          "3600")                                        # Fastest speed in engineering units
-epicsEnvSet("ECMC_DRV_SCALE_DENOM"        "32768")                                       # I/O range for ECMC_EC_ALIAS_DRV_VELO_SET
-epicsEnvSet("ECMC_EC_DRV_CONTROL",        "ec0.s$(DRV_ID).driveControl01.0")     # Ethercat entry for control word or bit output
-epicsEnvSet("ECMC_EC_DRV_STATUS",         "ec0.s$(DRV_ID).driveStatus01.1")      # Ethercat entry for status word or bit input
-epicsEnvSet("ECMC_EC_DRV_VELOCITY",       "ec0.s$(DRV_ID).velocitySetpoint01") # Ethercat entry for velocity setpoint output
-epicsEnvSet("ECMC_EC_DRV_REDUCE_TORQUE",  "ec0.s$(DRV_ID).driveControl01.2")     # Ethercat entry for reduce torque output
-epicsEnvSet("ECMC_EC_DRV_BRAKE",          "ec0.s$(DO_ID).binaryOutput01.0")                                            # Ethercat entry for brake output
-epicsEnvSet("ECMC_DRV_BRAKE_OPEN_DLY_TIME",    "1000")                                      # Brake timing parameter in cycles (default 1kHz)
-epicsEnvSet("ECMC_DRV_BRAKE_CLOSE_AHEAD_TIME", "2000")                                      # Brake timing parameter in cycles (default 1kHz)
-epicsEnvSet("ECMC_EC_DRV_RESET",          "ec0.s$(DRV_ID).driveControl01.1")     # Reset
-epicsEnvSet("ECMC_EC_DRV_ALARM_0",        "ec0.s$(DRV_ID).driveStatus01.3")      # Error
-epicsEnvSet("ECMC_EC_DRV_ALARM_1",        "ec0.s$(DRV_ID).driveStatus01.7")      # Stall
-epicsEnvSet("ECMC_EC_DRV_ALARM_2",        "ec0.s$(DRV_ID).driveStatus01.14")     # Sync error
-epicsEnvSet("ECMC_EC_DRV_WARNING",        "ec0.s$(DRV_ID).driveStatus01.2")      # Warning
-
-#Trajectory
-epicsEnvSet("ECMC_VELO",                  "360.0")
-epicsEnvSet("ECMC_JOG_VEL",               "360.0")
-epicsEnvSet("ECMC_JAR",                   "0.0")                       # JAR defaults to VELO/ACCL
-epicsEnvSet("ECMC_ACCS_EGU_PER_S2",       "360")
-epicsEnvSet("ECMC_EMERG_DECEL",           "1000")              # Emergency deceleration
-
-#Homing
-epicsEnvSet("ECMC_HOME_PROC",             "1")
-epicsEnvSet("ECMC_HOME_POS",              "0.0")
-epicsEnvSet("ECMC_HOME_VEL_TO",           "5")
-epicsEnvSet("ECMC_HOME_VEL_FRM",          "4")
-epicsEnvSet("ECMC_HOME_ACC",              "21")
-epicsEnvSet("ECMC_HOME_DEC",              "100")
-epicsEnvSet("ECMC_HOME_POS_MOVE_ENA",     "0")                        # Enable move to position after successfull homing 
-epicsEnvSet("ECMC_HOME_POS_MOVE_TARG_POS","0")                        # Target position to go to after successfull homing
-
-#Controller
-epicsEnvSet("ECMC_CNTRL_KP",              "5.0")
-epicsEnvSet("ECMC_CNTRL_KI",              "0.02")
-epicsEnvSet("ECMC_CNTRL_KD",              "0.0")
-epicsEnvSet("ECMC_CNTRL_KFF",             "1.0")
-
-#Monitoring
-# Switches
-epicsEnvSet("ECMC_EC_MON_LOWLIM",         "ec0.s$(DRV_ID).ONE.0")           #  Ethercat entry for low limit switch input
-epicsEnvSet("ECMC_EC_MON_HIGHLIM",        "ec0.s$(DRV_ID).ONE.0")           #  Ethercat entry for high limit switch inpuit
-epicsEnvSet("ECMC_EC_MON_HOME_SWITCH",    "ec0.s$(DRV_ID).ONE.0")           #  Ethercat entry for home switch input
-epicsEnvSet("ECMC_EC_MON_EXT_INTERLOCK",  "ec0.s$(DRV_ID).ONE.0")           #  Ethercat entry for external interlock input
-
-# Softlimits (disable with 0,0,0)
-epicsEnvSet("ECMC_SOFT_LOW_LIM",          "$(SM_DLLM=0)")
-epicsEnvSet("ECMC_SOFT_HIGH_LIM",         "$(SM_DHLM=0)")
-epicsEnvSet("ECMC_DXLM_ENABLE",           "0")
-
-# Position lag
-epicsEnvSet("ECMC_MON_LAG_MON_TOL",       "5")
-epicsEnvSet("ECMC_MON_LAG_MON_TIME",      "100")
-epicsEnvSet("ECMC_MON_LAG_MON_ENA",       "0")
-
-# At target
-epicsEnvSet("ECMC_MON_AT_TARGET_TOL",     "0.1")
-epicsEnvSet("ECMC_MON_AT_TARGET_TIME",     "100")
-epicsEnvSet("ECMC_MON_AT_TARGET_ENA",     "1")
-
-# Velocity
-epicsEnvSet("ECMC_MON_VELO_MAX",          "21000.0")
-epicsEnvSet("ECMC_MON_VELO_MAX_TRAJ_TIME","100")
-epicsEnvSet("ECMC_MON_VELO_MAX_DRV_TIME", "200")
-epicsEnvSet("ECMC_MON_VELO_MAX_ENA",      "0")
diff --git a/iocsh/el7031.script b/iocsh/el7042-0052.script
similarity index 89%
rename from iocsh/el7031.script
rename to iocsh/el7042-0052.script
index 693f0c0..ae3a1e8 100644
--- a/iocsh/el7031.script
+++ b/iocsh/el7042-0052.script
@@ -10,17 +10,13 @@ require ecmccomp sandst_a
 ##############################################################################
 ## Configure hardware
 
-epicsEnvSet("DRV_SLAVE",              "4")
-${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(DRV_SLAVE), HW_DESC=EL7031"
-
-#- Apply motor config with some custom macros
+epicsEnvSet("DRV_SLAVE",              "13")
+${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(DRV_SLAVE), HW_DESC=EL7041-0052"
 ${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Motor-Generic-2Phase-Stepper,MACROS='I_STDBY_MA=200,I_MAX_MA=1000,R_COIL_MOHM=1700'"
 
 epicsEnvSet("ENC_SLAVE",              "14")
 ${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(ENC_SLAVE), HW_DESC=EL5042"
-
-#- Apply motor config with some custom macros
-${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-Generic-BISS-C"
+${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-24bit"
 
 #Apply hardware configuration
 ecmcConfigOrDie "Cfg.EcApplyConfig(1)"
diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp
index 783faa6..3ed9290 100644
--- a/src/ecmcSafetyGroup.cpp
+++ b/src/ecmcSafetyGroup.cpp
@@ -185,7 +185,7 @@ void ecmcSafetyGroup::validateCfgs() {
            slaveIdRampDown_,
            aliasRampDown_,
            bitRampDown_);
-    printf("\n");
+
     printf("Safety: I/O link parsed:\n" 
            "    name:     %s\n"
            "    masterid: %d\n"