From 39417703784f2a0f64f4a486928d0cf01a8a6214 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 4 Apr 2023 14:20:27 +0200
Subject: [PATCH] fancy GPS fdir

---
 mission/system/acs/AcsBoardAssembly.cpp | 31 ++++++++++++++++++++-----
 1 file changed, 25 insertions(+), 6 deletions(-)

diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp
index de820535..9900f275 100644
--- a/mission/system/acs/AcsBoardAssembly.cpp
+++ b/mission/system/acs/AcsBoardAssembly.cpp
@@ -278,15 +278,34 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
       modeHelper.setForced(true);
     }
   };
-  if (healthHelper.healthTable->getHealth(helper.gpsId) == EXTERNAL_CONTROL) {
+  if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == EXTERNAL_CONTROL or
+      healthHelper.healthTable->getHealth(helper.healthDevGps1) == EXTERNAL_CONTROL) {
     modeHelper.setForced(true);
   }
-  if (deviceSubmode == duallane::DUAL_MODE) {
-    checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
-                             helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
-    checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
-                             helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
+  if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == PERMANENT_FAULTY and
+      healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY) {
+    overwriteDeviceHealth(helper.healthDevGps1, FAULTY);
+  } else if (healthHelper.healthTable->getHealth(helper.healthDevGps1) == PERMANENT_FAULTY and
+             healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY) {
+    overwriteDeviceHealth(helper.healthDevGps0, FAULTY);
+  } 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));
   }
+  if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY and
+      healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY) {
+    overwriteDeviceHealth(helper.healthDevGps0, FAULTY);
+    overwriteDeviceHealth(helper.healthDevGps1, FAULTY);
+  } else if ()
+    if (deviceSubmode == duallane::DUAL_MODE) {
+      checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
+                               helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
+      checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
+                               helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
+    }
   return status;
 }