From 8f68bc021da4f52eae363ca4006ded18e500f5cd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Tue, 25 Feb 2025 09:18:12 +0100
Subject: [PATCH] Calc emerg decelartion depending on actual traj velo

---
 qt/ecmc_plugin_safety_main.ui | 14 ++++++------
 qt/sim_motion_safety_box.ui   | 43 +++++++++++++++++++++++++++++++----
 src/ecmcSS1SafetyGroup.cpp    | 19 +++++++++++++++-
 src/ecmcSS1SafetyGroup.h      |  3 ++-
 src/ecmcSafetyPlgDefs.h       |  3 +++
 5 files changed, 69 insertions(+), 13 deletions(-)

diff --git a/qt/ecmc_plugin_safety_main.ui b/qt/ecmc_plugin_safety_main.ui
index 18a815b..d4ee110 100644
--- a/qt/ecmc_plugin_safety_main.ui
+++ b/qt/ecmc_plugin_safety_main.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>315</width>
-    <height>150</height>
+    <height>143</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -17,7 +17,7 @@
    <property name="geometry">
     <rect>
      <x>20</x>
-     <y>45</y>
+     <y>98</y>
      <width>30</width>
      <height>30</height>
     </rect>
@@ -44,7 +44,7 @@
    <property name="geometry">
     <rect>
      <x>55</x>
-     <y>47</y>
+     <y>100</y>
      <width>251</width>
      <height>26</height>
     </rect>
@@ -60,7 +60,7 @@
    <property name="geometry">
     <rect>
      <x>55</x>
-     <y>75</y>
+     <y>40</y>
      <width>246</width>
      <height>28</height>
     </rect>
@@ -76,7 +76,7 @@
    <property name="geometry">
     <rect>
      <x>20</x>
-     <y>75</y>
+     <y>40</y>
      <width>30</width>
      <height>30</height>
     </rect>
@@ -103,7 +103,7 @@
    <property name="geometry">
     <rect>
      <x>55</x>
-     <y>105</y>
+     <y>70</y>
      <width>206</width>
      <height>28</height>
     </rect>
@@ -119,7 +119,7 @@
    <property name="geometry">
     <rect>
      <x>20</x>
-     <y>105</y>
+     <y>70</y>
      <width>30</width>
      <height>30</height>
     </rect>
diff --git a/qt/sim_motion_safety_box.ui b/qt/sim_motion_safety_box.ui
index fe2a81c..eb30c88 100644
--- a/qt/sim_motion_safety_box.ui
+++ b/qt/sim_motion_safety_box.ui
@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>364</width>
+    <width>341</width>
     <height>312</height>
    </rect>
   </property>
@@ -120,12 +120,15 @@
   <widget class="caTextEntry" name="catextentry">
    <property name="geometry">
     <rect>
-     <x>170</x>
+     <x>54</x>
      <y>280</y>
-     <width>100</width>
+     <width>41</width>
      <height>22</height>
     </rect>
    </property>
+   <property name="alignment">
+    <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+   </property>
    <property name="channel" stdset="0">
     <string notr="true">$(IOC):m$(M_ID=0)s$(BI_S_ID)-One</string>
    </property>
@@ -133,7 +136,7 @@
   <widget class="caLabel" name="calabel_4">
    <property name="geometry">
     <rect>
-     <x>30</x>
+     <x>20</x>
      <y>10</y>
      <width>301</width>
      <height>28</height>
@@ -179,6 +182,38 @@
     <enum>EPushButton::None</enum>
    </property>
   </widget>
+  <widget class="caLabel" name="calabel_5">
+   <property name="geometry">
+    <rect>
+     <x>135</x>
+     <y>85</y>
+     <width>131</width>
+     <height>28</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>(Ramp down command)</string>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+   </property>
+  </widget>
+  <widget class="caLabel" name="calabel_6">
+   <property name="geometry">
+    <rect>
+     <x>120</x>
+     <y>275</y>
+     <width>171</width>
+     <height>28</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Command PV (ONE)</string>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+   </property>
+  </widget>
  </widget>
  <customwidgets>
   <customwidget>
diff --git a/src/ecmcSS1SafetyGroup.cpp b/src/ecmcSS1SafetyGroup.cpp
index 1065e7e..3635cdd 100644
--- a/src/ecmcSS1SafetyGroup.cpp
+++ b/src/ecmcSS1SafetyGroup.cpp
@@ -94,6 +94,7 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
   limitVeloCmd_             = 0;
   axesDisabled_             = 0;
   cfgDbgMode_               = 0;
+  oldStop_                  = 0;
   memset(&status_,0,sizeof(status_));
 
   parseConfigStr(cfg_string);
@@ -621,8 +622,24 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
     if(!*saxis) {
       throw std::runtime_error( "Safety: Axis object NULL.");
     }
-    setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);    
+
+    setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);
+
+    if(!oldStop_ && stop) {
+      //Calcualte new deceleration if delay is defined and abs(velo) > 0
+      if(delayMs_ > 0) {
+        double velo = 0;
+        getAxisTrajVelo((*saxis)->axisId_,&velo);
+        velo = std::abs(velo);        
+        if(velo > 0) {          
+          double newEmergDec = velo / (ECMC_PLUGIN_RAMP_DOWN_TIME_RATIO * delayMs_ / 1000.0);
+          setAxisEmergDeceleration((*saxis)->axisId_,newEmergDec);
+          printf("Safety %s: Set new emergency deceleration %lf \n",sName_,newEmergDec);
+        }
+      }
+    } 
   }
+  oldStop_ = stop;
 }
 
 void ecmcSS1SafetyGroup::addAxis(int axisId,
diff --git a/src/ecmcSS1SafetyGroup.h b/src/ecmcSS1SafetyGroup.h
index 520f64f..4ce1b80 100644
--- a/src/ecmcSS1SafetyGroup.h
+++ b/src/ecmcSS1SafetyGroup.h
@@ -162,8 +162,9 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
   ecmcEcEntry          *ecEntryRampDown_;
   ecmcEcEntry          *ecEntryStandstill_;
   ecmcEcEntry          *ecEntryLimitVelo_;
-  
+  bool                  oldStop_;
   static std::string    to_string(int value);
+
 };
 
 #endif  /* ECMC_SAFETY_GROUP_PLG_H_ */
diff --git a/src/ecmcSafetyPlgDefs.h b/src/ecmcSafetyPlgDefs.h
index 88dfdba..0ba59d3 100644
--- a/src/ecmcSafetyPlgDefs.h
+++ b/src/ecmcSafetyPlgDefs.h
@@ -25,4 +25,7 @@
 #define ECMC_PLUGIN_SAFETY_ERROR_CODE 1
 #define ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE 2
 
+// How big part of delay is used for rampdown
+#define ECMC_PLUGIN_RAMP_DOWN_TIME_RATIO 0.9
+
 #endif  /* ECMC_MOTION_PLG_DEFS_H_ */