From 3a236a1a3bb095b1f55271f4d03c7f4901575460 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 6 Apr 2023 13:01:31 +0200
Subject: [PATCH] all heaters off wrapper

---
 mission/controller/ThermalController.cpp | 22 +++++++++++++++++-----
 mission/controller/ThermalController.h   |  1 +
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index 738b9e14..0fe5c862 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -1031,7 +1031,7 @@ void ThermalController::ctrlAcsBoard() {
       if (chooseHeater(switchNr, redSwitchNr)) {
         if (heaterHandler.getSwitchState(switchNr)) {
           if (submode != SUBMODE_NO_HEATER_CTRL) {
-            heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+            heaterSwitchHelper(switchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
           }
         }
       }
@@ -1566,6 +1566,7 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate
 void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) {
   if (selectAndReadSensorTemp(htrCtx)) {
     if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) {
+      // Core loop for a thermal component, after sensors and heaters were selected.
       checkLimitsAndCtrlHeater(htrCtx);
     }
   } else {
@@ -1574,7 +1575,7 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) {
     if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) {
       if (heaterCtrlAllowed() and
           (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) {
-        heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
+        heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
       }
     }
   }
@@ -1786,6 +1787,19 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr,
   }
 }
 
+void ThermalController::heaterSwitchHelperAllOff() {
+  timeval currentTime;
+  Clock::getClockMonotonic(&currentTime);
+  size_t idx = 0;
+  for (; idx < heater::Switch::NUMBER_OF_SWITCHES; idx++) {
+    heaterHandler.switchHeater(static_cast<heater::Switch>(idx), HeaterHandler::SwitchState::OFF);
+  }
+  for (idx = 0; idx < thermalStates.size(); idx++) {
+    thermalStates[idx].heating = false;
+    thermalStates[idx].heaterEndTime = currentTime.tv_sec;
+  }
+}
+
 void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag) {
   // Clear the one shot flag is the component is in acceptable temperature range.
   if (not tooHotHandler(object, oneShotFlag) and not componentAboveUpperLimit) {
@@ -1798,9 +1812,7 @@ void ThermalController::startTransition(Mode_t mode_, Submode_t submode_) {
   // For MODE_OFF and the no heater control submode, we command all switches to off before
   // completing the transition. This ensures a consistent state when commanding these modes.
   if ((mode_ == MODE_OFF) or ((mode_ == MODE_ON) and (submode_ == SUBMODE_NO_HEATER_CTRL))) {
-    for (uint8_t i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) {
-      heaterHandler.switchHeater(static_cast<heater::Switch>(i), HeaterHandler::SwitchState::OFF);
-    }
+    heaterSwitchHelperAllOff();
     transitionWhenHeatersOff = true;
     targetMode = mode_;
     targetSubmode = submode_;
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index af27dd29..b47fb8f5 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -315,6 +315,7 @@ class ThermalController : public ExtendedControllerBase {
   bool chooseHeater(heater::Switch& switchNr, heater::Switch redSwitchNr);
   bool selectAndReadSensorTemp(HeaterContext& htrCtx);
 
+  void heaterSwitchHelperAllOff();
   void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state,
                           unsigned componentIdx);