From 2334ecf512335cc399479a14f697705e4fee1d7e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= <anders.sandstroem@psi.ch>
Date: Mon, 19 Feb 2024 09:26:06 +0100
Subject: [PATCH] Add check that axis is not linked multiple times to group

---
 src/ecmcSS1SafetyGroup.cpp | 31 +++++++++++++++++++++++++++++--
 1 file changed, 29 insertions(+), 2 deletions(-)

diff --git a/src/ecmcSS1SafetyGroup.cpp b/src/ecmcSS1SafetyGroup.cpp
index c1d4eaa..0bfd341 100644
--- a/src/ecmcSS1SafetyGroup.cpp
+++ b/src/ecmcSS1SafetyGroup.cpp
@@ -183,10 +183,37 @@ void ecmcSS1SafetyGroup::validateCfgs() {
 
 void ecmcSS1SafetyGroup::validateAxes() {
 
-  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
-    if(!(*saxis)) {
+  // Ensure that axis is not linked twice to group 
+  for(std::vector<safetyAxis*>::iterator iaxis = axes_.begin(); iaxis != axes_.end(); ++iaxis) {
+    int axisLinkedCounter = 0;
+    
+    if(!(*iaxis)) {
       throw std::runtime_error( "Safety: Axis object NULL.");
     }
+    int axisCheck_1 = (*iaxis)->axisId_;
+    int axisCheck_2 = -1;
+    for(std::vector<safetyAxis*>::iterator jaxis = axes_.begin(); jaxis != axes_.end(); ++jaxis) {
+      if(!(*jaxis)) {
+        throw std::runtime_error( "Safety: Axis object NULL.");
+      }
+
+      axisCheck_2 =(*jaxis)->axisId_;
+      if( axisCheck_1 == axisCheck_2) {
+        axisLinkedCounter++;
+      }
+    }
+
+    if(axisLinkedCounter > 1) {
+      if(cfgDbgMode_) {
+        printf("Safety %s: Axis %d linked %d times to group\n",
+              sName_,axisCheck_2,axisLinkedCounter);
+      }
+      throw std::runtime_error( "Safety: Axis linked multiple times");
+    }    
+  }
+
+  // Check axis object exists and valid
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
     if(!getAxisValid((*saxis)->axisId_)) {
       throw std::runtime_error( "Safety: Ecmc Axis object not valid.");
     }