diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index c1e3cf71..1feef038 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -105,9 +105,9 @@ void ThermalController::performControlOperation() {
   }
 
   // TODO: Heater control
-  //ctrlCameraBody();
-  /*ctrlAcsBoard();
-  ctrlMgt();
+  // ctrlCameraBody();
+  // ctrlAcsBoard();
+  /*ctrlMgt();
   ctrlRw();
   ctrlStr();
   ctrlIfBoard();
@@ -976,31 +976,19 @@ void ThermalController::copyDevices() {
   }
 }
 
-/*
 void ThermalController::ctrlAcsBoard() {
   ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_0_OBC_BRD,
                            deviceTemperatures.gyro0SideA, deviceTemperatures.mgm0SideA,
-                           deviceTemperatures.gyro1SideA,
+                           deviceTemperatures.gyro1SideA,  // mgm1(int),
+                           // sensorTemperatures.sensor_tcs_board,
                            &acsBoardLimits);  // TODO: add red sensors
 }
 
 void ThermalController::ctrlMgt() {
-  heater::Switchers switchNr = heater::HEATER_2_ACS_BRD;
-  heater::Switchers redSwitchNr = heater::HEATER_3_PCDU_PDU;
-
-  chooseHeater(switchNr, redSwitchNr);
-
-  if (heaterAvailable) {
-    chooseSensorOldVersion(
-        switchNr, sensorTemperatures.sensor_magnettorquer.value, deviceTemperatures.mgt.value,
-        sensorTemperatures.sensor_plpcdu_heatspreader.value,
-        sensorTemperatures.sensor_magnettorquer.isValid(), deviceTemperatures.mgt.isValid(),
-        sensorTemperatures.sensor_plpcdu_heatspreader.isValid());
-
-    if (sensorTempAvailable) {
-      ctrlHeater(switchNr, redSwitchNr, &mgtLimits);
-    }
-  }
+  // TODO: cast deviceTemperatures.mgt
+  // ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU,
+  // sensorTemperatures.sensor_magnettorquer, deviceTemperatures.mgt,
+  // sensorTemperatures.sensor_plpcdu_heatspreader, &mgtLimits);
 }
 
 void ThermalController::ctrlRw() {
@@ -1021,41 +1009,61 @@ void ThermalController::ctrlIfBoard() {
 }
 
 void ThermalController::ctrlTcsBoard() {
-  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, &tcsBoardLimits);
+  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD,
+                           sensorTemperatures.sensor_tcs_board, sensorTemperatures.tmp1075Tcs0,
+                           sensorTemperatures.tmp1075Tcs1, &tcsBoardLimits);
 }
 
 void ThermalController::ctrlObc() {
-  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, &obcLimits);
+  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD,
+                           deviceTemperatures.q7s, sensorTemperatures.tmp1075Tcs1,
+                           sensorTemperatures.tmp1075Tcs0, &obcLimits);
 }
 
 void ThermalController::ctrlObcIfBoard() {
-  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, &obcIfBoardLimits);
+  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD,
+                           deviceTemperatures.q7s, sensorTemperatures.tmp1075Tcs0,
+                           sensorTemperatures.tmp1075Tcs1, &obcIfBoardLimits);
 }
 
 void ThermalController::ctrlSBandTransceiver() {
   ctrlComponentTemperature(heater::HEATER_7_S_BAND, heater::HEATER_4_CAMERA,
-                           &sBandTransceiverLimits);
+                           deviceTemperatures.syrlinksPowerAmplifier,
+                           deviceTemperatures.syrlinksBasebandBoard,
+                           sensorTemperatures.sensor_4k_camera, &sBandTransceiverLimits);
 }
 void ThermalController::ctrlPcduP60Board() {
+  // TODO: remove sensor3
   ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD,
-                           &pcduP60BoardLimits);
+                           deviceTemperatures.temp1P60dock, deviceTemperatures.temp2P60dock,
+                           deviceTemperatures.temp2P60dock, &pcduP60BoardLimits);
 }
 
 void ThermalController::ctrlPcduAcu() {
-  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, &pcduAcuLimits);
+  // TODO: check sensors (float,3)
+  // ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD,
+  // deviceTemperatures.acu, deviceTemperatures.acu, sensorTemperatures.sensor_acu, &pcduAcuLimits);
 }
 
 void ThermalController::ctrlPcduPdu() {
-  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, &pcduPduLimits);
+  // TODO: remove sensor3
+  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD,
+                           deviceTemperatures.pdu1, deviceTemperatures.pdu2,
+                           deviceTemperatures.pdu2, &pcduPduLimits);
 }
 
 void ThermalController::ctrlPlPcduBoard() {
-  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, &plPcduBoardLimits);
+  // TODO: add sensor 4: sensorTemperatures.sensor_plpcdu_heatspreader
+  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD,
+                           sensorTemperatures.tmp1075PlPcdu0, sensorTemperatures.tmp1075PlPcdu1,
+                           deviceTemperatures.adcPayloadPcdu, &plPcduBoardLimits);
 }
 
 void ThermalController::ctrlPlocMissionBoard() {
   ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
-                           &plocMissionBoardLimits);
+                           sensorTemperatures.sensor_ploc_heatspreader,
+                           sensorTemperatures.sensor_ploc_missionboard,
+                           sensorTemperatures.sensor_dac_heatspreader, &plocMissionBoardLimits);
 }
 
 void ThermalController::ctrlPlocProcessingBoard() {
@@ -1079,29 +1087,41 @@ void ThermalController::ctrlCameraBody() {
 }
 
 void ThermalController::ctrlDro() {
-  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, &droLimits);
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA,
+                           sensorTemperatures.sensor_dro, sensorTemperatures.sensor_4k_camera,
+                           sensorTemperatures.sensor_mpa, &droLimits);
 }
 
 void ThermalController::ctrlX8() {
-  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, &x8Limits);
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA,
+                           sensorTemperatures.sensor_x8, sensorTemperatures.sensor_hpa,
+                           sensorTemperatures.sensor_tx_modul, &x8Limits);
 }
 
 void ThermalController::ctrlTx() {
-  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, &txLimits);
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA,
+                           sensorTemperatures.sensor_tx_modul, sensorTemperatures.sensor_x8,
+                           sensorTemperatures.sensor_mpa, &txLimits);
 }
 
 void ThermalController::ctrlMpa() {
-  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, &mpaLimits);
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA,
+                           sensorTemperatures.sensor_mpa, sensorTemperatures.sensor_hpa,
+                           sensorTemperatures.sensor_tx_modul, &mpaLimits);
 }
 
 void ThermalController::ctrlHpa() {
-  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, &hpaLimits);
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA,
+                           sensorTemperatures.sensor_hpa, sensorTemperatures.sensor_x8,
+                           sensorTemperatures.sensor_mpa, &hpaLimits);
 }
 
 void ThermalController::ctrlScexBoard() {
-  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_5_STR, &scexBoardLimits);
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_5_STR,
+                           sensorTemperatures.sensor_scex, sensorTemperatures.sensor_x8,
+                           sensorTemperatures.sensor_hpa, &scexBoardLimits);
 }
-*/
+
 void ThermalController::ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr,
                                    struct TempLimits* tempLimit) {
   // Heater off
@@ -1144,7 +1164,6 @@ void ThermalController::chooseHeater(heater::Switchers& switchNr, heater::Switch
 
 void ThermalController::chooseSensor(heater::Switchers switchNr, const lp_float_t& sensor1,
                                      const lp_float_t& sensor2, const lp_float_t& sensor3,
-                                     const lp_float_t& sensor4, const lp_float_t& sensor5,
                                      bool moreThan3Sensors) {
   sensorTempAvailable = true;
 
@@ -1154,10 +1173,10 @@ void ThermalController::chooseSensor(heater::Switchers switchNr, const lp_float_
     sensorTemp = sensor2.value;
   } else if (sensor3.isValid()) {
     sensorTemp = sensor3.value;
-  } else if (moreThan3Sensors and sensor4.isValid()) {
-    sensorTemp = sensor4.value;
-  } else if (moreThan3Sensors and sensor5.isValid()) {
-    sensorTemp = sensor5.value;
+    /*} else if (moreThan3Sensors and sensor4.isValid()) {
+      sensorTemp = sensor4.value;
+    } else if (moreThan3Sensors and sensor5.isValid()) {
+      sensorTemp = sensor5.value; */
   } else {
     if (heaterHandler.checkSwitchState(switchNr)) {
       heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
@@ -1168,15 +1187,16 @@ void ThermalController::chooseSensor(heater::Switchers switchNr, const lp_float_
   }
 }
 
-
-void ThermalController::ctrlComponentTemperature(
-    heater::Switchers switchNr, heater::Switchers redSwitchNr, const lp_float_t& sensor1,
-    const lp_float_t& sensor2, const lp_float_t& sensor3, const lp_float_t& sensor4,
-    const lp_float_t& sensor5, TempLimits* tempLimit, bool moreThan3Sensors) {
+void ThermalController::ctrlComponentTemperature(heater::Switchers switchNr,
+                                                 heater::Switchers redSwitchNr,
+                                                 const lp_float_t& sensor1,
+                                                 const lp_float_t& sensor2,
+                                                 const lp_float_t& sensor3, TempLimits* tempLimit,
+                                                 bool moreThan3Sensors) {
   chooseHeater(switchNr, redSwitchNr);
 
   if (heaterAvailable) {
-    chooseSensor(switchNr, sensor1, sensor2, sensor3, sensor4, sensor5, moreThan3Sensors);
+    chooseSensor(switchNr, sensor1, sensor2, sensor3, moreThan3Sensors);
 
     if (sensorTempAvailable) {
       ctrlHeater(switchNr, redSwitchNr, tempLimit);
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index 439bc5fe..d43519cf 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -135,7 +135,7 @@ class ThermalController : public ExtendedControllerBase {
   TempLimits mpaLimits = TempLimits(-40.0, -30.0, -75.0, 80.0, 90.0);
   TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0);
 
-  float sensorTemp;
+  static float sensorTemp;
   bool sensorTempAvailable = true;
   bool heaterAvailable = true;
   bool redSwitchNrInUse = false;
@@ -157,15 +157,12 @@ class ThermalController : public ExtendedControllerBase {
 
   void ctrlComponentTemperature(heater::Switchers switchNr, heater::Switchers redSwitchNr,
                                 const lp_float_t& sensor1, const lp_float_t& sensor2,
-                                const lp_float_t& sensor3, const lp_float_t& sensor4,
-                                const lp_float_t& sensor5, TempLimits* tempLimit,
+                                const lp_float_t& sensor3, TempLimits* tempLimit,
                                 bool moreThan3Sensors = false);
-  void ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr,
-                  struct TempLimits* tempLimit);
+  void ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr, TempLimits* tempLimit);
   void chooseHeater(heater::Switchers& switchNr, heater::Switchers redSwitchNr);
   void chooseSensor(heater::Switchers switchNr, const lp_float_t& sensor1,
-                    const lp_float_t& sensor2, const lp_float_t& sensor3, const lp_float_t& sensor4,
-                    const lp_float_t& sensor5, bool moreThan3Sensors);
+                    const lp_float_t& sensor2, const lp_float_t& sensor3, bool moreThan3Sensors);
 
   void ctrlAcsBoard();
   void ctrlMgt();