From 32cda0f58b48b042cc1ef50995342bc175611187 Mon Sep 17 00:00:00 2001
From: Irini Kosmidou <kosmidoui@irs.uni-stuttgart.de>
Date: Mon, 28 Nov 2022 10:06:49 +0100
Subject: [PATCH] further development thermal controller

---
 fsfw                                     |  2 +-
 mission/controller/ThermalController.cpp | 42 +++++++++++++-----------
 mission/controller/ThermalController.h   | 11 +++++--
 3 files changed, 32 insertions(+), 23 deletions(-)

diff --git a/fsfw b/fsfw
index 46a1c2ba..160ff799 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 46a1c2bacea142994666b2201acf0246ba0fd0b4
+Subproject commit 160ff799ace61e24708dcf1fdeaf5fafdf23a4ca
diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index aeaeb209..274be244 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -106,28 +106,30 @@ void ThermalController::performControlOperation() {
 
   // TODO: Heater control
   // TODO: Hysterese offset
-  if (heaterHandler.heaterVec[heater::HEATER_4_CAMERA].switchState ==
-      HeaterHandler::SwitchState::OFF) {
-    if (sensorTemperatures.sensor_4k_camera.value < cameraLimits.opLowerLimit) {
-      heaterHandler.heaterVec[heater::HEATER_4_CAMERA].cmdActive = true;
-      heaterHandler.heaterVec[heater::HEATER_4_CAMERA].action = HeaterHandler::SET_SWITCH_ON;
-      heaterHandler.handleSwitchOnCommand(heater::HEATER_4_CAMERA);
-      heater4Countdown.resetTimer();
-    }
-  } else if (heaterHandler.heaterVec[heater::HEATER_4_CAMERA].switchState ==
-             HeaterHandler::SwitchState::ON) {
-    if (sensorTemperatures.sensor_4k_camera.value < cameraLimits.opLowerLimit) {
-      if (heater4Countdown.hasTimedOut()) {
-        heaterHandler.heaterVec[heater::HEATER_4_CAMERA].cmdActive = true;
-        heaterHandler.heaterVec[heater::HEATER_4_CAMERA].action = HeaterHandler::SET_SWITCH_OFF;
-        // triggerEvent(HEATER_TIMEOUT, heater::HEATER_4_CAMERA);
-        heaterHandler.handleSwitchOffCommand(heater::HEATER_4_CAMERA);
+
+  // 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 {
-      heaterHandler.heaterVec[heater::HEATER_4_CAMERA].cmdActive = true;
-      heaterHandler.heaterVec[heater::HEATER_4_CAMERA].action = HeaterHandler::SET_SWITCH_OFF;
-      heaterHandler.handleSwitchOffCommand(heater::HEATER_4_CAMERA);
-    };
+      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 a91e531d..203f779e 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -28,7 +28,6 @@ struct TempLimits {
   float opUpperLimit;
   float nopLowerLimit;
   float nopUpperLimit;
-  // TODO define limits
 };
 
 class ThermalController : public ExtendedControllerBase {
@@ -52,8 +51,15 @@ class ThermalController : public ExtendedControllerBase {
                                          uint32_t* msToReachTheMode) override;
 
  private:
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::HEATER_HANDLER;
+  static constexpr Event HEATER_MAX_BURNTIME_REACHED = MAKE_EVENT(0, severity::LOW);
+  static constexpr Event INVALID_SENSOR_TEMPERATURE = MAKE_EVENT(1, severity::LOW);
+
   static const uint32_t DELAY = 500;
-  static const uint32_t OP_TIME = 1000;  // TODO to be changed
+
+  // TODO to be changed
+  static const uint32_t OP_TIME = 1000;
+  static const uint32_t TEMP_OFFSET = 10;
 
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
 
@@ -103,6 +109,7 @@ class ThermalController : public ExtendedControllerBase {
   SUS::SusDataset susSet11;
 
   // TempLimits
+  // TODO: find missing temperatures
   // TempLimits plocHeatspreaderLimits = TempLimits(-20.0, 70.0, -30.0, 80.0);
   // TempLimits plocMissionBoardLimits = TempLimits(-20.0, 70.0, -30.0, 80.0);
   TempLimits cameraLimits = TempLimits(-40.0, -30.0, 65.0, 85.0);