From e52c909580aa6f79d09479f2ba88e48b3ced4ea2 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 13:32:02 +0100
Subject: [PATCH] health handling for ACS brd assy

---
 mission/system/objects/AcsBoardAssembly.cpp   | 27 +++++++++++++++++++
 mission/system/objects/AcsBoardAssembly.h     |  2 ++
 mission/system/objects/DualLaneAssemblyBase.h |  2 ++
 3 files changed, 31 insertions(+)

diff --git a/mission/system/objects/AcsBoardAssembly.cpp b/mission/system/objects/AcsBoardAssembly.cpp
index 01dd76b1..85fda507 100644
--- a/mission/system/objects/AcsBoardAssembly.cpp
+++ b/mission/system/objects/AcsBoardAssembly.cpp
@@ -280,3 +280,30 @@ ReturnValue_t AcsBoardAssembly::initialize() {
   }
   return AssemblyBase::initialize();
 }
+
+ReturnValue_t AcsBoardAssembly::checkAndHandleHealthState(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(objects::SYRLINKS_HANDLER, health);
+      status = NEED_TO_CHANGE_HEALTH;
+    } else if (health == 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);
+  }
+  return status;
+}
diff --git a/mission/system/objects/AcsBoardAssembly.h b/mission/system/objects/AcsBoardAssembly.h
index 0c25396f..187c8619 100644
--- a/mission/system/objects/AcsBoardAssembly.h
+++ b/mission/system/objects/AcsBoardAssembly.h
@@ -121,6 +121,8 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
 
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
+  ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode,
+                Submode_t deviceSubmode);
   void refreshHelperModes();
 };
 
diff --git a/mission/system/objects/DualLaneAssemblyBase.h b/mission/system/objects/DualLaneAssemblyBase.h
index eadfb77f..b0f4416a 100644
--- a/mission/system/objects/DualLaneAssemblyBase.h
+++ b/mission/system/objects/DualLaneAssemblyBase.h
@@ -70,6 +70,8 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
   virtual void handleChildrenLostMode(ReturnValue_t result) override;
   virtual void handleModeTransitionFailed(ReturnValue_t result) override;
   virtual void handleModeReached() override;
+  ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode,
+                Submode_t deviceSubmode);
 
   MessageQueueId_t getEventReceptionQueue() override;