From 30a767fe596c0dab73b729fd8611dfefd545552b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Fri, 16 Feb 2024 12:01:58 +0100
Subject: [PATCH] Add soem printouts and cleanup

---
 src/ecmcSafetyGroup.cpp | 79 +++++++++++++++++++++++++----------------
 src/ecmcSafetyGroup.h   | 18 ++++++----
 2 files changed, 61 insertions(+), 36 deletions(-)

diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp
index d3a12a8..479f1c2 100644
--- a/src/ecmcSafetyGroup.cpp
+++ b/src/ecmcSafetyGroup.cpp
@@ -344,43 +344,39 @@ void ecmcSafetyGroup::execute() {
     setAxesStandstillStatus(0); // set output
     throw std::out_of_range("Safety: Read rampdown cmd failed");
   }
+
   rampDownCmdOld_ = rampDownCmd_;
   rampDownCmd_ = data == 0;
-  
-  if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) {
-    if(rampDownCmd_) {
-      printf("Safety %s: Ramp down cmd active\n",sName_);
-    } else {
-      printf("Safety %s: Ramp down cmd not active\n",sName_);
+
+  if(rampDownCmdOld_ != rampDownCmd_) {
+    resetPrintoutStatus();
+    if(cfgDbgMode_) {
+      if(rampDownCmd_) {
+        printf("Safety %s: Ramp down cmd active\n",sName_);
+      } else {
+        printf("Safety %s: Ramp down cmd not active\n",sName_);
+      }
     }
   }
 
   // set safety interlock in ecmc
   setAxesSafetyInterlocks(rampDownCmd_);
   // check if axes are standstill to safety PLC
-  axesAreStandstill_ = checkAxesStandstill();
+  axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded();
   setAxesStandstillStatus(axesAreStandstill_);
+ 
   // Disable
   if(axesAreStandstill_ && rampDownCmd_) {    
     setAxesDisable();
   } else {
     printEnableStatus_ = true;
   }
+}
 
-
-/*
-  //Needed functions in motion.h
-  int getAxisBusy(int  axisIndex,
-                  int *value);
-  int getAxisEncVelo(int  axisIndex,
-                     double *velo);
-  int getAxisTrajVelo(int  axisIndex,
-                      double *velo);
-  int getAxisEnabled(int  axisIndex,
-                     int *value);
-  int setAxisEnable(int axisIndex,
-                    int value);
-  */
+void ecmcSafetyGroup::resetPrintoutStatus() {
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    (*saxis)->printEnableStat_ = 1;    
+  }
 }
 
 void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
@@ -404,32 +400,54 @@ void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
 bool ecmcSafetyGroup::checkAxesStandstill() {
   bool standstill = 1;
   for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
-    standstill= standstill && checkAxisStandstill((*saxis)->axisIndex_,(*saxis)->veloLimit_);    
+    standstill= standstill && checkAxisStandstill((*saxis));    
   }
   return standstill;
 }
 
 // Check standstill axis
-bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) {
+bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) {
   double velo = 1;
-  int err = getAxisTrajVelo(axisId, &velo);
+  int err = getAxisTrajVelo(axis->axisId_, &velo);
   if(err) {
     return 0;
   }
-  return std::abs(velo) <= veloLimit;
+  return std::abs(velo) <= axis->veloLimit_;
 }
 
-// Set safety interlock
+// Check standstill and disable
+bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
+  bool standstill = 1;
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    standstill = standstill && checkAxisStandstillAndDisableIfNeeded((*saxis));
+  }
+  return standstill;
+}
+
+// Check standstill and disable
+bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
+  bool standstill = checkAxisStandstill(axis);
+  if( standstill && rampDownCmd_) {
+    setAxisEnable(axis->axisId_,0);
+    if(cfgDbgMode_ && axis->printEnableStat_) {
+      printf("Safety %s: Disabling axis %d\n",sName_,axis->axisId_);
+      axis->printEnableStat_ = 0;
+    }
+  }
+  return standstill;
+}
+
+// Disable all axes
 void ecmcSafetyGroup::setAxesDisable() {
   for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
     if(!*saxis) {
       throw std::runtime_error("Safety: Axis object NULL.");
     }
-    setAxisEnable((*saxis)->axisIndex_,0);    
+    setAxisEnable((*saxis)->axisId_,0);    
   }
 
   if(printEnableStatus_ && cfgDbgMode_) {
-    printf("Safety %s: Axes are beeing disabled\n",sName_);
+    printf("Safety %s: All axes in group are beeing disabled\n",sName_);
   }
   printEnableStatus_ = 0;
 }
@@ -440,7 +458,7 @@ void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
     if(!*saxis) {
       throw std::runtime_error( "Safety: Axis object NULL.");
     }
-    setAxisEmergencyStopInterlock((*saxis)->axisIndex_,stop, 1000);    
+    setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);    
   }
 }
 
@@ -460,9 +478,10 @@ void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs)
     throw std::out_of_range("Safety: Invalid axis id");
   }
   
-  axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));
+  axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));  
   axesCounter_++;
   
+  
   if(cfgDbgMode_) {
     printf("Safety %s: Added axis %d to safety group\n",sName_,axisId);
   }
diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h
index c51bc59..cb4cecb 100644
--- a/src/ecmcSafetyGroup.h
+++ b/src/ecmcSafetyGroup.h
@@ -26,13 +26,16 @@ class safetyAxis {
   safetyAxis(int axisIndex,
              double veloLimit,
              int standStillTimeMs) {
-    veloLimit_ = veloLimit;
-    axisIndex_ = axisIndex;    
+    veloLimit_        = veloLimit;
+    axisId_        = axisIndex;    
     standStillTimeMs_ = standStillTimeMs;
+    printEnableStat_  = 1;
   }
-  double        veloLimit_;
-  int           axisIndex_;
-  int           standStillTimeMs_;  
+
+  double  veloLimit_;
+  int     axisId_;
+  int     standStillTimeMs_;
+  int     printEnableStat_;
 };
 
 class ecmcSafetyGroup : public asynPortDriver {
@@ -68,7 +71,10 @@ class ecmcSafetyGroup : public asynPortDriver {
   void                  setAxesDisable();
   void                  setAxesStandstillStatus(int standstill);
   bool                  checkAxesStandstill();
-  bool                  checkAxisStandstill(int axisId, double veloLimit);
+  bool                  checkAxisStandstill(safetyAxis* axis);
+  bool                  checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis);
+  bool                  checkAxesStandstillAndDisableIfNeeded();
+  void                  resetPrintoutStatus();
   void                  parseConfigStr(const char *configStr);
   void                  initAsyn();
   double                ecmcSampleRateHz_;