diff --git a/fsfw b/fsfw
index 05cad893..5b0ea912 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 05cad893a2b713827cf4cdc9afe49675f18afcc7
+Subproject commit 5b0ea91222a6b8efb2f4562cfecbcb735dfeedd5
diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index a8c378df..008a8077 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -105,7 +105,7 @@ void ThermalController::performControlOperation() {
   }
 
   // TODO: Heater control
-  ctrl4KCamera();
+  ctrlCameraBody();
 }
 
 ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -953,36 +953,111 @@ void ThermalController::copyDevices() {
   }
 }
 
-void ThermalController::ctrl4KCamera() {
-  // 4K Camera
-  float cameraTemp;
+void ThermalController::ctrlMgt() {
+  float sensorTemp;
+  float opLowerLimit = mgtLimits.opLowerLimit;
+  float cutOffLimit = mgtLimits.cutOffLimit;
+  heater::Switchers switchNr = heater::HEATER_2_ACS_BRD;
+  heater::Switchers redSwitchNr = heater::HEATER_3_PCDU_PDU;
   bool cameraTempAvailable = true;
+  bool heaterAvailable = true;
+  bool redSwitchNrInUse = false;
 
-  if (sensorTemperatures.sensor_4k_camera.isValid()) {
-    cameraTemp = sensorTemperatures.sensor_4k_camera.value;
-  } else if (sensorTemperatures.sensor_dro.isValid()) {
-    cameraTemp = sensorTemperatures.sensor_dro.value;
-  } else if (sensorTemperatures.sensor_mpa.isValid()) {
-    cameraTemp = sensorTemperatures.sensor_mpa.value;
-  } else {
-    if (heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
-      heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
+  // Choose Heater
+  chooseHeater(switchNr, redSwitchNr, redSwitchNrInUse, heaterAvailable);
+
+  if (heaterAvailable) {
+    // Choose sensor
+    if (sensorTemperatures.sensor_magnettorquer.isValid()) {
+      sensorTemp = sensorTemperatures.sensor_magnettorquer.value;
+    } else if (sensorTemperatures.sensor_magnettorquer.isValid()) {  // TODO: int sensor
+      sensorTemp = sensorTemperatures.sensor_magnettorquer.value;
+    } else if (sensorTemperatures.sensor_plpcdu_heatspreader.isValid()) {
+      sensorTemp = sensorTemperatures.sensor_plpcdu_heatspreader.value;
+    } else {
+      if (heaterHandler.checkSwitchState(switchNr)) {
+        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+      }
+      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
+      cameraTempAvailable = false;
     }
-    triggerEvent(NO_VALID_SENSOR_TEMPERATURE, heater::HEATER_4_CAMERA);
-    cameraTempAvailable = false;
-  }
 
-  // TODO: include redundant heater
-  if (cameraTempAvailable) {
-    if (not heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
-      // TODO: check if op or nop
-      if (cameraTemp < cameraLimits.opLowerLimit) {
-        heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::ON);
-      } else if (heaterHandler.checkSwitchState(heater::HEATER_4_CAMERA)) {
-        if (cameraTemp >= cameraLimits.opLowerLimit + TEMP_OFFSET) {
-          heaterHandler.switchHeater(heater::HEATER_4_CAMERA, HeaterHandler::SwitchState::OFF);
-        }
+    // Thermal Control
+    if (cameraTempAvailable) {
+      ctrlHeater(switchNr, redSwitchNr, redSwitchNrInUse, sensorTemp, opLowerLimit, cutOffLimit);
+    }
+  }
+}
+void ThermalController::ctrlCameraBody() {
+  // Camera Body
+  float sensorTemp;
+  float opLowerLimit = cameraLimits.opLowerLimit;
+  float cutOffLimit = cameraLimits.cutOffLimit;
+  heater::Switchers switchNr = heater::HEATER_4_CAMERA;
+  heater::Switchers redSwitchNr = heater::HEATER_6_DRO;
+  bool cameraTempAvailable = true;
+  bool heaterAvailable = true;
+  bool redSwitchNrInUse = false;
+
+  // Choose Heater
+  chooseHeater(switchNr, redSwitchNr, redSwitchNrInUse, heaterAvailable);
+
+  if (heaterAvailable) {
+    // Choose sensor
+    if (sensorTemperatures.sensor_4k_camera.isValid()) {
+      sensorTemp = sensorTemperatures.sensor_4k_camera.value;
+    } else if (sensorTemperatures.sensor_dro.isValid()) {
+      sensorTemp = sensorTemperatures.sensor_dro.value;
+    } else if (sensorTemperatures.sensor_mpa.isValid()) {
+      sensorTemp = sensorTemperatures.sensor_mpa.value;
+    } else {
+      if (heaterHandler.checkSwitchState(switchNr)) {
+        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+      }
+      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
+      cameraTempAvailable = false;
+    }
+
+    // Thermal Control
+    if (cameraTempAvailable) {
+      ctrlHeater(switchNr, redSwitchNr, redSwitchNrInUse, sensorTemp, opLowerLimit, cutOffLimit);
+    }
+  }
+}
+
+void ThermalController::ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr,
+                                   bool redSwitchNrInUse, float sensorTemp, float opLowerLimit,
+                                   float cutOffLimit) {
+  // Heater off
+  if (not heaterHandler.checkSwitchState(switchNr)) {
+    // TODO: check if OP or NOP
+    if (sensorTemp < opLowerLimit) {
+      heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::ON);
+    }
+    // Heater on
+  } else if (heaterHandler.checkSwitchState(switchNr)) {
+    if (sensorTemp >= opLowerLimit + TEMP_OFFSET) {
+      heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+    }
+  } else if (not redSwitchNrInUse) {
+    if (heaterHandler.checkSwitchState(redSwitchNr)) {
+      if (sensorTemp >= cutOffLimit) {
+        heaterHandler.switchHeater(redSwitchNr, HeaterHandler::SwitchState::OFF);
       }
     }
   }
+}
+
+void ThermalController::chooseHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr,
+                                     bool& redSwitchNrInUse, bool& heaterAvailable) {
+  // Choose Heater
+  if (heaterHandler.getHealth(switchNr) != HasHealthIF::HEALTHY) {
+    if (heaterHandler.getHealth(redSwitchNr) == HasHealthIF::HEALTHY) {
+      switchNr = redSwitchNr;
+      redSwitchNrInUse = true;
+    } else {
+      heaterAvailable = false;
+      // TODO: triggerEvent ?
+    }
+  }
 }
\ No newline at end of file
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index a6d6a3b3..4fe61e8c 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -148,7 +148,34 @@ class ThermalController : public ExtendedControllerBase {
   void copySus();
   void copyDevices();
 
-  void ctrl4KCamera();
+  void ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr, bool redSwitchNrInUse,
+                  float sensorTemp, float opLowerLimit, float cutOffLimit);
+  void chooseHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr,
+                    bool& redSwitchNrInUse, bool& heaterAvailable);
+
+  void ctrlAcsBoard();
+  void ctrlMgt();
+  void ctrlRw();
+  void ctrlStr();
+  void ctrlIfBoard();
+  void ctrlTcsBoard();
+  void ctrlObc();
+  void ctrlObcIfBoard();
+  void ctrlSBandTransceiver();
+  void ctrlPcduP60Board();
+  void ctrlPcduAcu();
+  void ctrlPcduPdu();
+  void ctrlPlPcduBoard();
+  void ctrlPlocMissionBoard();
+  void ctrlPlocProcessingBoard();
+  void ctrlDac();
+  void ctrlCameraBody();
+  void ctrlDro();
+  void ctrlX8();
+  void ctrlHpa();
+  void ctrlTx();
+  void ctrlMpa();
+  void ctrlScexBoard();
 };
 
 #endif /* MISSION_CONTROLLER_THERMALCONTROLLER_H_ */
diff --git a/tmtc b/tmtc
index 30cf4736..56d0f26c 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 30cf47365fec68a4b78395de03ecb2ae95af4e7e
+Subproject commit 56d0f26cbffbfbf4e790d3a19858162291104934