From 0c5f9a409172ab771d5fd6f35f7c649049d7a4fb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Wed, 20 Mar 2024 09:29:40 +0100
Subject: [PATCH] Add check of bit id

---
 GNUmakefile                |  3 +--
 README.md                  |  2 +-
 iocsh/test_4ax_box.script  |  2 +-
 src/ecmcSS1SafetyGroup.cpp | 20 ++++++++++++++++++--
 src/ecmcSS1SafetyGroup.h   |  3 ++-
 5 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/GNUmakefile b/GNUmakefile
index 441af58..62d736f 100644
--- a/GNUmakefile
+++ b/GNUmakefile
@@ -16,8 +16,7 @@ OPT_CXXFLAGS_YES = -O3
 
 # dependencies
 ECmasterECMC_VERSION = v1.1.0
-# ecmc_VERSION = 9.2.0
-ecmc_VERSION = safety3
+ecmc_VERSION = 9.4.0
 
 ################################################################################
 # THIS RELATES TO THE EtherCAT MASTER LIBRARY
diff --git a/README.md b/README.md
index 51939f1..8071159 100644
--- a/README.md
+++ b/README.md
@@ -24,7 +24,7 @@ For initiation of new movements, the target velocity will be limited to 95% of t
 ## Interface
 Basically the safey system is interfaced with three binary signals (ethercat I/O):
 * Ramp down command (from safety system to ecmc). 0 means ramp down command is active.
-* Axis stand still status (to safety system from ecmc). 1 means that all axes are at rest
+* Axis stand still status (to safety system from ecmc). 1 means that all axes are at rest _AND_ disabled
 * Limitation of velocity (optional). 0 means limitation of velocity is acrive
 
  If, for instance, an safety event is triggerd by the safety system, it will immediately command this plugin to rampdown velocity of all axes (that a configured to stop). When all axes, that are configured to rampdown, have stopped then this plugin will disable the axes and set an ethercat output informing the safety system that the axes are standstill. After a certain timout the safety system will make sure power is removed from the motion axes by triggering an STO or removing power. The removal of power or triggering of STO will made regardless if the axes are at rest or not. A reset of the safety system, allowing power to the drives, will only be possible once the safety system gets a confirmation that all axes are at rest.
diff --git a/iocsh/test_4ax_box.script b/iocsh/test_4ax_box.script
index f42aa84..449f4eb 100644
--- a/iocsh/test_4ax_box.script
+++ b/iocsh/test_4ax_box.script
@@ -12,7 +12,7 @@
 ##
 
 #epicsEnvSet(IOC,c6025a)
-require ecmccfg safety3,"ECMC_VER=safety3,ENG_MODE=1,MASTER_ID=1"
+require ecmccfg sandst_a,"ECMC_VER=safety3,ENG_MODE=1,MASTER_ID=1"
 
 ##############################################################################
 ## Load components lib
diff --git a/src/ecmcSS1SafetyGroup.cpp b/src/ecmcSS1SafetyGroup.cpp
index eac5317..06f3bde 100644
--- a/src/ecmcSS1SafetyGroup.cpp
+++ b/src/ecmcSS1SafetyGroup.cpp
@@ -92,6 +92,7 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
   printEnableStatus_        = 1;
   limitVeloCmdOld_          = 0;
   limitVeloCmd_             = 0;
+  axesDisabled_             = 0;
   cfgDbgMode_               = 0;
   memset(&status_,0,sizeof(status_));
 
@@ -390,9 +391,12 @@ void ecmcSS1SafetyGroup::exeRampDown() {
 
   // set safety interlock in ecmc
   setAxesSafetyInterlocks(rampDownCmd_);
+
+  axesDisabled_ = checkAxesDisabled();
   
   // check if axes are standstill to safety PLC
   axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded();
+
   setAxesStandstillStatus(axesAreStandstill_);
  
   // Disable
@@ -472,9 +476,10 @@ void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) {
     status_.axesAtStandstill = standstill;
     refreshAsyn();
   }
-
+  
+  // Only write axis standstill bit if all axes are disabled
   if(ecEntryStandstill_->writeBit(bitStandStill_,
-                                  standstill > 0)) {
+                                 ((standstill > 0) && axesDisabled_) )) {
     throw std::out_of_range("Safety: Read rampdown cmd failed");
   }
   axesAreStandstillOld_ = standstill;
@@ -489,6 +494,17 @@ bool ecmcSS1SafetyGroup::checkAxesStandstill() {
   return standstill;
 }
 
+// Check axes disabled
+bool ecmcSS1SafetyGroup::checkAxesDisabled() {
+  bool enabledsum = 1;
+  int enabled     = 0;
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {    
+    getAxisEnabled((*saxis)->axisId_,&enabled);
+    enabledsum = enabledsum && enabled;
+  }
+  return !enabledsum;
+}
+
 // Check max velo violation
 void ecmcSS1SafetyGroup::checkAxesMaxVeloAndDisableIfNeeded() {
   if(!limitVeloCmd_) {
diff --git a/src/ecmcSS1SafetyGroup.h b/src/ecmcSS1SafetyGroup.h
index 8967536..520f64f 100644
--- a/src/ecmcSS1SafetyGroup.h
+++ b/src/ecmcSS1SafetyGroup.h
@@ -100,6 +100,7 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
   void                  setAxesDisable();
   void                  setAxesStandstillStatus(int standstill);
   bool                  checkAxesStandstill();
+  bool                  checkAxesDisabled();
   bool                  checkAxisStandstill(safetyAxis* axis);
   bool                  checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis);
   bool                  checkAxesStandstillAndDisableIfNeeded();
@@ -148,7 +149,7 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
   int                   bitLimitVelo_;
 
   int                   limitMaxVeloEnable_;
-
+  int                   axesDisabled_;
   int                   axesAreStandstill_;
   int                   axesAreStandstillOld_;
   int                   printEnableStatus_;