From 53c839e3890ec0668516bceb50218a1373b94f2f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 27 Mar 2023 14:53:18 +0200
Subject: [PATCH] proper fault/ext ctrl handling for dual side

---
 mission/system/acs/AcsBoardAssembly.cpp | 34 ++++++++++++++-----------
 mission/system/acs/SusAssembly.cpp      | 25 +++++++++++++-----
 tmtc                                    |  2 +-
 3 files changed, 38 insertions(+), 23 deletions(-)

diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp
index 2441beda..08711f76 100644
--- a/mission/system/acs/AcsBoardAssembly.cpp
+++ b/mission/system/acs/AcsBoardAssembly.cpp
@@ -289,25 +289,29 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
                                                            Submode_t deviceSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
-  auto overwriteHealthForOneDev = [&](object_id_t dev) {
-    HealthState health = healthHelper.healthTable->getHealth(dev);
-    if (health == FAULTY or health == PERMANENT_FAULTY) {
-      overwriteDeviceHealth(dev, health);
-      status = NEED_TO_CHANGE_HEALTH;
-    } else if (health == EXTERNAL_CONTROL) {
+  auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2,
+                                      object_id_t o3) {
+    HealthState h0 = healthHelper.healthTable->getHealth(o0);
+    HealthState h1 = healthHelper.healthTable->getHealth(o1);
+    HealthState h2 = healthHelper.healthTable->getHealth(o2);
+    HealthState h3 = healthHelper.healthTable->getHealth(o3);
+    if ((h0 == FAULTY or h0 == PERMANENT_FAULTY) and (h1 == FAULTY or h1 == PERMANENT_FAULTY) and
+        (h2 == FAULTY or h2 == PERMANENT_FAULTY) and (h3 == FAULTY or h3 == PERMANENT_FAULTY)) {
+      overwriteDeviceHealth(o0, h0);
+      overwriteDeviceHealth(o1, h1);
+      overwriteDeviceHealth(o2, h2);
+      overwriteDeviceHealth(o3, h3);
+    }
+    if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or
+        h3 == EXTERNAL_CONTROL) {
       modeHelper.setForced(true);
     }
   };
   if (deviceSubmode == duallane::DUAL_MODE) {
-    overwriteHealthForOneDev(helper.mgm0Lis3IdSideA);
-    overwriteHealthForOneDev(helper.mgm1Rm3100IdSideA);
-    overwriteHealthForOneDev(helper.mgm2Lis3IdSideB);
-    overwriteHealthForOneDev(helper.mgm3Rm3100IdSideB);
-    overwriteHealthForOneDev(helper.gyro0AdisIdSideA);
-    overwriteHealthForOneDev(helper.gyro1L3gIdSideA);
-    overwriteHealthForOneDev(helper.gyro2AdisIdSideB);
-    overwriteHealthForOneDev(helper.gyro3L3gIdSideB);
-    overwriteHealthForOneDev(helper.gpsId);
+    checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
+                             helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
+    checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
+                             helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
   }
   return status;
 }
diff --git a/mission/system/acs/SusAssembly.cpp b/mission/system/acs/SusAssembly.cpp
index 9008e7e0..45123ce4 100644
--- a/mission/system/acs/SusAssembly.cpp
+++ b/mission/system/acs/SusAssembly.cpp
@@ -158,18 +158,29 @@ void SusAssembly::refreshHelperModes() {
 ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
-  auto overwriteHealthForOneDev = [&](object_id_t dev) {
+  auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) {
+    HealthState healthNom = healthHelper.healthTable->getHealth(devNom);
+    HealthState healthRed = healthHelper.healthTable->getHealth(devRed);
+    if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
+        (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
+      overwriteDeviceHealth(devNom, healthNom);
+      overwriteDeviceHealth(devRed, healthRed);
+    }
+  };
+  auto checkHealthForOneDev = [&](object_id_t dev) {
     HealthState health = healthHelper.healthTable->getHealth(dev);
-    if (health == FAULTY or health == PERMANENT_FAULTY) {
-      overwriteDeviceHealth(dev, health);
-      status = NEED_TO_CHANGE_HEALTH;
-    } else if (health == EXTERNAL_CONTROL) {
+    if (health == EXTERNAL_CONTROL) {
       modeHelper.setForced(true);
     }
   };
   if (deviceSubmode == duallane::DUAL_MODE) {
-    for (uint8_t idx = 0; idx < 12; idx++) {
-      overwriteHealthForOneDev(helper.susIds[idx]);
+    uint8_t idx = 0;
+    for (idx = 0; idx < 6; idx++) {
+      checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]);
+      checkHealthForOneDev(helper.susIds[idx]);
+    }
+    for (idx = 6; idx < 12; idx++) {
+      checkHealthForOneDev(helper.susIds[idx]);
     }
   }
   return status;
diff --git a/tmtc b/tmtc
index f6fcb2fb..333faaa5 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit f6fcb2fb282d79b1e250722eba46a319603b0232
+Subproject commit 333faaa5f70646bfde1101f8e0538505574d1d7e