From 4a404835c0e344f04e9c4ff013043f8493a13596 Mon Sep 17 00:00:00 2001
From: Irini Kosmidou <kosmidoui@irs.uni-stuttgart.de>
Date: Mon, 28 Nov 2022 17:40:29 +0100
Subject: [PATCH] further development thermal controller

---
 mission/controller/ThermalController.cpp | 52 +++++++++++++-----------
 mission/controller/ThermalController.h   |  2 +
 2 files changed, 30 insertions(+), 24 deletions(-)

diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index 274be244..fc6a6088 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -106,31 +106,8 @@ void ThermalController::performControlOperation() {
 
   // TODO: Heater control
   // TODO: Hysterese offset
+  ctrl4KCameraHeater();
 
-  // 4K Camera - Heater
-  if (sensorTemperatures.sensor_4k_camera.isValid()) {
-    if (not heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
-      if (sensorTemperatures.sensor_4k_camera.value < cameraLimits.opLowerLimit) {
-        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::ON);
-        heater4Countdown.resetTimer();
-      }
-    } else if (heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
-      if (sensorTemperatures.sensor_4k_camera.value >= cameraLimits.opLowerLimit + TEMP_OFFSET) {
-        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
-      }
-    } else {
-      if (heater4Countdown.hasTimedOut()) {
-        // Sensor or heater failure
-        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
-        triggerEvent(HEATER_MAX_BURNTIME_REACHED, heater::HEATER_4_CAMERA);
-      };
-    }
-  } else {
-    if (heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
-      heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
-    }
-    triggerEvent(INVALID_SENSOR_TEMPERATURE, heater::HEATER_4_CAMERA);
-  }
 }
 
 ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -977,3 +954,30 @@ void ThermalController::copyDevices() {
     }
   }
 }
+
+void ThermalController::ctrl4KCameraHeater(){
+  // 4K Camera - Heater
+  if (sensorTemperatures.sensor_4k_camera.isValid()) {
+    if (not heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
+      if (sensorTemperatures.sensor_4k_camera.value < cameraLimits.opLowerLimit) {
+        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::ON);
+        heater4Countdown.resetTimer();
+      }
+    } else if (heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
+      if (sensorTemperatures.sensor_4k_camera.value >= cameraLimits.opLowerLimit + TEMP_OFFSET) {
+        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
+      }
+    } else {
+      if (heater4Countdown.hasTimedOut()) {
+        // Sensor or heater failure
+        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
+        triggerEvent(HEATER_MAX_BURNTIME_REACHED, heater::HEATER_4_CAMERA);
+      };
+    }
+  } else {
+    if (heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
+      heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
+    }
+    triggerEvent(INVALID_SENSOR_TEMPERATURE, heater::HEATER_4_CAMERA);
+  }
+}
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index 203f779e..076956e8 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -150,6 +150,8 @@ class ThermalController : public ExtendedControllerBase {
   void copySensors();
   void copySus();
   void copyDevices();
+
+  void ctrl4KCameraHeater();
 };
 
 #endif /* MISSION_CONTROLLER_THERMALCONTROLLER_H_ */