From 282a67c1a6673b69c0da1b09ae63b34a444d3f20 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Thu, 15 Feb 2024 15:52:09 +0100
Subject: [PATCH] WIP

---
 src/ecmcSafetyGroup.cpp | 75 +++++++++++++++++++++++++++++++----------
 src/ecmcSafetyGroup.h   | 15 +++++++--
 2 files changed, 69 insertions(+), 21 deletions(-)

diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp
index f8e8d83..89acd21 100644
--- a/src/ecmcSafetyGroup.cpp
+++ b/src/ecmcSafetyGroup.cpp
@@ -67,8 +67,6 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
   axesCounter_          = 0;
   ecmcSampleRateHz_     = getEcmcSampleRate();
   dataSourcesLinked_    = 0;
-  //dataItemRampDownCmd_  = NULL;
-  //dataItemStandStillStat_ = NULL;
   ecMaster_             = NULL;
   ecEntryRampDown_      = NULL;
   ecEntryStandstill_    = NULL;
@@ -80,6 +78,8 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
   aliasRampDown_[0]     = 0;
   aliasStandStill_[0]   = 0;
   rampDownCmd_          = 0;
+  axesAreStandstill_    = 0;
+  rampDownCmdOld_       = 0;
   
   // Config defaults
   cfgDbgMode_           = 0;
@@ -328,23 +328,28 @@ std::string ecmcSafetyGroup::to_string(int value) {
 // Executed by ecmc rt thread.
 void ecmcSafetyGroup::execute() {
   
-  uint64_t data = 1;
-
-  if(!ecEntryRampDown_) {
-    return;
-  }
-
+  uint64_t data = 0;
+  // Read ramp down command from safety plc
   if(ecEntryRampDown_->readBit(bitRampDown_,
                                &data)) {
+    // Disable all axes
+    setAxesEnable(0);           // disable
+    setAxesSafetyInterlocks(0); // stop
+    setAxesStandstillStatus(0); // set output
     throw std::out_of_range("Safety: Read rampdown cmd failed");
   }
-  rampDownCmd_ = data > 0;
-
-  if(rampDownCmd_) {
-    // set safety interlock in ecmc
-    setSafetyInterlocks();
+  rampDownCmdOld_ = rampDownCmd_;
+  rampDownCmd_ = data == 0;
+  
+  if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) {
+    printf("Safety: Ramp down cmd changed state %d\n",rampDownCmd_);
   }
 
+  // set safety interlock in ecmc
+  setAxesSafetyInterlocks(rampDownCmd_);
+  // check if axes are standstill to safety PLC
+  axesAreStandstill_ = checkAxesStandstill();
+  setAxesStandstillStatus(axesAreStandstill_);
 /*
   //Needed functions in motion.h
   int getAxisBusy(int  axisIndex,
@@ -357,19 +362,53 @@ void ecmcSafetyGroup::execute() {
                      int *value);
   int setAxisEnable(int axisIndex,
                     int value);
-  int setAxisClearEmergencyInterlock(int axisIndex);
-  int setAxisEmergencyStopInterlock(int axisIndex,
-                             double deceleration);
   */
 }
 
+void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
+
+  if(ecEntryStandstill_->writeBit(bitStandStill_,
+                                  standstill > 0)) {
+    throw std::out_of_range("Safety: Read rampdown cmd failed");
+  }
+}
+
+// Check standstill axes
+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_);    
+  }
+  return standstill;
+}
+
+// Check standstill axis
+bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) {
+  double velo = 1;
+  int err = getAxisTrajVelo(axisId, &velo);
+  if(err) {
+    return 0;
+  }
+  return std::abs(velo) <= veloLimit;
+}
+
 // Set safety interlock
-void ecmcSafetyGroup::setSafetyInterlocks() {
+void ecmcSafetyGroup::setAxesEnable(int enable) {
   for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
     if(!*saxis) {
       throw std::runtime_error( "Safety: Axis object NULL.");
     }
-    setAxisEmergencyStopInterlock((*saxis)->axisIndex_,1000);    
+    setAxisEnable((*saxis)->axisIndex_,enable);    
+  }
+}
+
+// Set safety interlock
+void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    if(!*saxis) {
+      throw std::runtime_error( "Safety: Axis object NULL.");
+    }
+    setAxisEmergencyStopInterlock((*saxis)->axisIndex_,stop, 1000);    
   }
 }
 
diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h
index bf9f3d7..43ee546 100644
--- a/src/ecmcSafetyGroup.h
+++ b/src/ecmcSafetyGroup.h
@@ -64,7 +64,11 @@ class ecmcSafetyGroup : public asynPortDriver {
   void                  validateAxes();
   void                  stripBits();
   void                  connectToDataSources();
-  void                  setSafetyInterlocks();
+  void                  setAxesSafetyInterlocks(int stop);
+  void                  setAxesEnable(int enable);
+  void                  setAxesStandstillStatus(int standstill);
+  bool                  checkAxesStandstill();
+  bool                  checkAxisStandstill(int axisId, double veloLimit);
   void                  parseConfigStr(const char *configStr);
   void                  initAsyn();
   double                ecmcSampleRateHz_;
@@ -72,6 +76,8 @@ class ecmcSafetyGroup : public asynPortDriver {
   int                   objectId_;           // Unique object id
   int                   cycleCounter_;
   int                   rampDownCmd_;
+  int                   rampDownCmdOld_;
+
   // Config options
   int                   cfgDbgMode_;         // Config: allow dbg printouts
 
@@ -87,19 +93,22 @@ class ecmcSafetyGroup : public asynPortDriver {
   char*                 sEcRampDownCmdNameStrip_;
   char*                 sEcAxesStandStillStatStrip_;
   char*                 sConfig_;
-  //ecmcDataItem         *dataItemRampDownCmd_;    
-  //ecmcDataItem         *dataItemStandStillStat_;
+  
   int                   delayMs_;
   int                   masterId_;
   int                   slaveIdRampDown_;
   int                   bitRampDown_;
   int                   slaveIdStandStill_;
   int                   bitStandStill_;
+  int                   axesAreStandstill_;
+  
   char                  aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
   char                  aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
+  
   ecmcEc               *ecMaster_;
   ecmcEcEntry          *ecEntryRampDown_;
   ecmcEcEntry          *ecEntryStandstill_;
+  
   // Some generic utility functions
   static uint8_t        getUint8(uint8_t* data);
   static int8_t         getInt8(uint8_t* data);