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
  *