From 845548ed2524dc7a1ad0f61d8c09d1ddaacbd2f4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 5 Apr 2023 10:27:19 +0200
Subject: [PATCH] maybe that fixes the issues

---
 mission/system/acs/AcsBoardAssembly.cpp | 11 ++++++++++-
 mission/system/acs/SusAssembly.cpp      |  5 +++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp
index befe82e0..56bfe506 100644
--- a/mission/system/acs/AcsBoardAssembly.cpp
+++ b/mission/system/acs/AcsBoardAssembly.cpp
@@ -260,6 +260,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
                                                            Submode_t deviceSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
+  bool healthNeedsToBeOverwritten = false;
   auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2,
                                       object_id_t o3) {
     HealthState h0 = healthHelper.healthTable->getHealth(o0);
@@ -272,6 +273,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
       overwriteDeviceHealth(o1, h1);
       overwriteDeviceHealth(o2, h2);
       overwriteDeviceHealth(o3, h3);
+      healthNeedsToBeOverwritten = true;
     }
     if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or
         h3 == EXTERNAL_CONTROL) {
@@ -285,22 +287,29 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
   if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == PERMANENT_FAULTY and
       healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY) {
     overwriteDeviceHealth(helper.healthDevGps1, FAULTY);
+    healthNeedsToBeOverwritten = true;
   } else if (healthHelper.healthTable->getHealth(helper.healthDevGps1) == PERMANENT_FAULTY and
              healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY) {
     overwriteDeviceHealth(helper.healthDevGps0, FAULTY);
+    healthNeedsToBeOverwritten = true;
   } else if (healthHelper.healthTable->isFaulty(helper.healthDevGps0) or
              healthHelper.healthTable->isFaulty(helper.healthDevGps1)) {
     overwriteDeviceHealth(helper.healthDevGps0,
                           healthHelper.healthTable->getHealth(helper.healthDevGps0));
     overwriteDeviceHealth(helper.healthDevGps1,
                           healthHelper.healthTable->getHealth(helper.healthDevGps1));
+    healthNeedsToBeOverwritten = true;
   }
 
   if (deviceSubmode == duallane::DUAL_MODE) {
     checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
                              helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
     checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
-                             helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
+
+       helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
+  }
+  if(healthNeedsToBeOverwritten) {
+    return NEED_TO_CHANGE_HEALTH;
   }
   return status;
 }
diff --git a/mission/system/acs/SusAssembly.cpp b/mission/system/acs/SusAssembly.cpp
index 75a3cb57..124c41b0 100644
--- a/mission/system/acs/SusAssembly.cpp
+++ b/mission/system/acs/SusAssembly.cpp
@@ -143,6 +143,7 @@ void SusAssembly::refreshHelperModes() {
 ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
+  bool needsHealthOverwritten = false;
   auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) {
     HealthState healthNom = healthHelper.healthTable->getHealth(devNom);
     HealthState healthRed = healthHelper.healthTable->getHealth(devRed);
@@ -150,6 +151,7 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
         (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
       overwriteDeviceHealth(devNom, healthNom);
       overwriteDeviceHealth(devRed, healthRed);
+      needsHealthOverwritten = true;
     }
   };
   auto checkHealthForOneDev = [&](object_id_t dev) {
@@ -168,5 +170,8 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
       checkHealthForOneDev(helper.susIds[idx]);
     }
   }
+  if(needsHealthOverwritten) {
+    return NEED_TO_CHANGE_HEALTH;
+  }
   return status;
 }