From 520b41c53b2575f1bb679c105923dd6be941887f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 28 Mar 2023 17:21:43 +0200
Subject: [PATCH] transition basic mode handling

---
 mission/controller/ThermalController.cpp | 48 ++++++++++++++++++------
 mission/controller/ThermalController.h   |  5 +++
 2 files changed, 42 insertions(+), 11 deletions(-)

diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index 301470cb..38c32d3b 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -150,7 +150,14 @@ void ThermalController::performControlOperation() {
     }
   }
 
-  if(mode != MODE_OFF) {
+  if (transitionToOff) {
+    for (const auto& switchState : heaterSwitchStateArray) {
+      if (switchState != HeaterHandler::SwitchState::OFF) {
+        return;
+      }
+      setMode(MODE_OFF);
+    }
+  } else if (mode != MODE_OFF) {
     performThermalModuleCtrl();
   }
   cycles++;
@@ -990,15 +997,14 @@ void ThermalController::ctrlAcsBoard() {
 
 void ThermalController::ctrlMgt() {
   thermalComponent = MGT;
-    sensors[0].first = sensorTemperatures.mgt.isValid();
-    sensors[0].second = sensorTemperatures.mgt.value;
-    sensors[1].first = deviceTemperatures.mgt.isValid();
-    sensors[1].second = deviceTemperatures.mgt.value;
-    sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid();
-    sensors[2].second = sensorTemperatures.plpcduHeatspreader.value;
-    numSensors = 3;
-    ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits);
-
+  sensors[0].first = sensorTemperatures.mgt.isValid();
+  sensors[0].second = sensorTemperatures.mgt.value;
+  sensors[1].first = deviceTemperatures.mgt.isValid();
+  sensors[1].second = deviceTemperatures.mgt.value;
+  sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid();
+  sensors[2].second = sensorTemperatures.plpcduHeatspreader.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits);
 }
 
 void ThermalController::ctrlRw() {
@@ -1212,7 +1218,7 @@ void ThermalController::ctrlPcduAcu() {
 
   if (chooseHeater(switchNr, redSwitchNr)) {
     bool sensorTempAvailable = true;
-    //TODO: check
+    // TODO: check
     if (deviceTemperatures.acu.value[0] != INVALID_TEMPERATURE) {
       sensorTemp = deviceTemperatures.acu.value[0];
     } else if (deviceTemperatures.acu.value[1] != INVALID_TEMPERATURE) {
@@ -1599,6 +1605,13 @@ uint32_t ThermalController::tempFloatToU32() const {
   return tempRaw;
 }
 
+void ThermalController::setMode(Mode_t mode) {
+  this->mode = mode;
+  modeHelper.modeChanged(mode, submode);
+  modeChanged(mode, submode);
+  announceMode(false);
+}
+
 void ThermalController::tooHotHandler(object_id_t object, bool& oneShotFlag) {
   if (componentAboveUpperLimit and not oneShotFlag) {
     EventManagerIF::triggerEvent(object, overHeatEventToTrigger, tempFloatToU32());
@@ -1607,3 +1620,16 @@ void ThermalController::tooHotHandler(object_id_t object, bool& oneShotFlag) {
     oneShotFlag = false;
   }
 }
+
+void ThermalController::startTransition(Mode_t mode_, Submode_t submode_) {
+  triggerEvent(CHANGING_MODE, mode, submode);
+  if (mode == MODE_OFF) {
+    for (uint8_t i; i < heater::Switchers::NUMBER_OF_SWITCHES; i++) {
+      heaterHandler.switchHeater(static_cast<heater::Switchers>(i),
+                                 HeaterHandler::SwitchState::OFF);
+    }
+    transitionToOff = true;
+  } else {
+    setMode(mode_);
+  }
+}
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index 4fe38c4e..d2e1954d 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -243,6 +243,8 @@ class ThermalController : public ExtendedControllerBase {
   bool obcTooHotFlag = false;
   bool strTooHotFlag = false;
   bool rwTooHotFlag = false;
+
+  bool transitionToOff = false;
   uint32_t cycles = 0;
   std::array<ThermalState, 30> thermalStates{};
   std::array<HeaterState, 7> heaterStates{};
@@ -268,6 +270,8 @@ class ThermalController : public ExtendedControllerBase {
 
   static constexpr dur_millis_t MUTEX_TIMEOUT = 50;
 
+  void startTransition(Mode_t mode, Submode_t submode) override;
+
   void resetSensorsArray();
   void copySensors();
   void copySus();
@@ -304,6 +308,7 @@ class ThermalController : public ExtendedControllerBase {
   void ctrlMpa();
   void ctrlScexBoard();
   void heaterTransitionControl();
+  void setMode(Mode_t mode);
   uint32_t tempFloatToU32() const;
   void tooHotHandler(object_id_t object, bool& oneShotFlag);
 };