Merge remote-tracking branch 'origin/develop' into add_heater_info_set

mission/controller/ThermalController.cpp

@@ -2,6 +2,7 @@
 
 #include <bsp_q7s/core/CoreDefinitions.h>
 #include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/thermal/ThermalComponentIF.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h>
 #include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
 #include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
@@ -14,12 +15,14 @@
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <objects/systemObjectList.h>
 
-ThermalController::ThermalController(object_id_t objectId)
+ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater)
     : ExtendedControllerBase(objectId),
+      heaterHandler(heater),
       sensorTemperatures(this),
       susTemperatures(this),
       deviceTemperatures(this),
       heaterInfo(this),
+      imtqThermalSet(objects::IMTQ_HANDLER),
       max31865Set0(objects::RTD_0_IC3_PLOC_HEATSPREADER,
                    EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
       max31865Set1(objects::RTD_1_IC4_PLOC_MISSIONBOARD,
@@ -56,7 +59,9 @@ ThermalController::ThermalController(object_id_t objectId)
       susSet8(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
       susSet9(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
       susSet10(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
-      susSet11(objects::SUS_11_R_LOC_XBYMZB_PT_ZB) {}
+      susSet11(objects::SUS_11_R_LOC_XBYMZB_PT_ZB) {
+  resetSensorsArray();
+}
 
 ReturnValue_t ThermalController::initialize() {
   auto result = ExtendedControllerBase::initialize();
@@ -105,6 +110,7 @@ void ThermalController::performControlOperation() {
     deviceTemperatures.commit();
   }
 
+<<<<<<< HEAD
   {
     PoolReadGuard pg(&heaterInfo);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -116,6 +122,32 @@ void ThermalController::performControlOperation() {
       }
     }
   }
+=======
+  ctrlCameraBody();
+  ctrlAcsBoard();
+  ctrlMgt();
+  ctrlRw();
+  ctrlStr();
+  ctrlIfBoard();
+  ctrlAcsBoard();
+  ctrlObc();
+  ctrlObcIfBoard();
+  ctrlSBandTransceiver();
+  ctrlPcduP60Board();
+  ctrlPcduAcu();
+  ctrlPcduPdu();
+  ctrlPlPcduBoard();
+  ctrlPlocMissionBoard();
+  ctrlPlocProcessingBoard();
+  ctrlDac();
+
+  ctrlDro();
+  ctrlX8();
+  ctrlHpa();
+  ctrlTx();
+  ctrlMpa();
+  ctrlScexBoard();
+>>>>>>> origin/develop
 }
 
 ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -763,7 +795,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempSyrlinksPowerAmplifier =
-        lp_var_t<float>(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER);
+        lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER);
     PoolReadGuard pg(&tempSyrlinksPowerAmplifier, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read syrlinks power amplifier temperature"
@@ -778,7 +810,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempSyrlinksBasebandBoard =
-        lp_var_t<float>(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_BASEBAND_BOARD);
+        lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_BASEBAND_BOARD);
     PoolReadGuard pg(&tempSyrlinksBasebandBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read syrlinks baseband board temperature"
@@ -969,3 +1001,450 @@ void ThermalController::copyDevices() {
     }
   }
 }
+
+void ThermalController::ctrlAcsBoard() {
+  // TODO: check
+  heater::Switchers switchNr = heater::HEATER_2_ACS_BRD;
+  heater::Switchers redSwitchNr = heater::HEATER_0_OBC_BRD;
+
+  // A side
+  sensors[0].first = deviceTemperatures.gyro0SideA.isValid();
+  sensors[0].second = deviceTemperatures.gyro0SideA.value;
+  sensors[1].first = deviceTemperatures.mgm0SideA.isValid();
+  sensors[1].second = deviceTemperatures.mgm0SideA.value;
+  sensors[2].first = deviceTemperatures.gyro1SideA.isValid();
+  sensors[2].second = deviceTemperatures.gyro1SideA.value;
+  sensors[3].first = sensorTemperatures.sensor_tcs_board.isValid();
+  sensors[3].second = sensorTemperatures.sensor_tcs_board.value;
+  numSensors = 4;
+  if (selectAndReadSensorTemp()) {
+    if (chooseHeater(switchNr, redSwitchNr)) {
+      ctrlHeater(switchNr, redSwitchNr, acsBoardLimits);
+    }
+    resetSensorsArray();
+    return;
+  }
+
+  // B side
+  sensors[0].first = deviceTemperatures.gyro2SideB.isValid();
+  sensors[0].second = deviceTemperatures.gyro2SideB.value;
+  sensors[1].first = deviceTemperatures.mgm2SideB.isValid();
+  sensors[1].second = deviceTemperatures.mgm2SideB.value;
+  sensors[2].first = deviceTemperatures.gyro3SideB.isValid();
+  sensors[2].second = deviceTemperatures.gyro3SideB.value;
+  sensors[3].first = sensorTemperatures.sensor_tcs_board.isValid();
+  sensors[3].second = sensorTemperatures.sensor_tcs_board.value;
+  if (selectAndReadSensorTemp()) {
+    if (chooseHeater(switchNr, redSwitchNr)) {
+      ctrlHeater(switchNr, redSwitchNr, acsBoardLimits);
+    }
+  } else {
+    if (chooseHeater(switchNr, redSwitchNr)) {
+      if (heaterHandler.checkSwitchState(switchNr)) {
+        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+        sif::info << "ThermalController::ctrlHeater: Heater" << switchNr << " OFF" << std::endl;
+      }
+    }
+  }
+  resetSensorsArray();
+}
+
+void ThermalController::ctrlMgt() {
+  PoolReadGuard pg(&imtqThermalSet);
+  auto heaterReq =
+      static_cast<ThermalComponentIF::StateRequest>(imtqThermalSet.heaterRequest.value);
+
+  if (heaterReq == ThermalComponentIF::STATE_REQUEST_OPERATIONAL) {
+    sensors[0].first = sensorTemperatures.sensor_magnettorquer.isValid();
+    sensors[0].second = sensorTemperatures.sensor_magnettorquer.value;
+    sensors[1].first = deviceTemperatures.mgt.isValid();
+    sensors[1].second = deviceTemperatures.mgt.value;
+    sensors[2].first = sensorTemperatures.sensor_plpcdu_heatspreader.isValid();
+    sensors[2].second = sensorTemperatures.sensor_plpcdu_heatspreader.value;
+    numSensors = 3;
+    ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits);
+  }
+}
+
+void ThermalController::ctrlRw() {
+  // TODO: better solution?
+  // RW1
+  sensors[0].first = sensorTemperatures.sensor_rw1.isValid();
+  sensors[0].second = sensorTemperatures.sensor_rw1.value;
+  sensors[1].first = deviceTemperatures.rw1.isValid();
+  sensors[1].second = deviceTemperatures.rw1.value;
+  sensors[2].first = deviceTemperatures.rw4.isValid();
+  sensors[2].second = deviceTemperatures.rw4.value;
+  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[3].second = sensorTemperatures.sensor_dro.value;
+  numSensors = 4;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
+
+  // RW2
+  sensors[0].first = deviceTemperatures.rw2.isValid();
+  sensors[0].second = deviceTemperatures.rw2.value;
+  sensors[1].first = deviceTemperatures.rw3.isValid();
+  sensors[1].second = deviceTemperatures.rw3.value;
+  sensors[2].first = sensorTemperatures.sensor_rw1.isValid();
+  sensors[2].second = sensorTemperatures.sensor_rw1.value;
+  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[3].second = sensorTemperatures.sensor_dro.value;
+  numSensors = 4;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
+
+  // RW3
+  sensors[0].first = deviceTemperatures.rw3.isValid();
+  sensors[0].second = deviceTemperatures.rw3.value;
+  sensors[1].first = deviceTemperatures.rw4.isValid();
+  sensors[1].second = deviceTemperatures.rw4.value;
+  sensors[2].first = sensorTemperatures.sensor_rw1.isValid();
+  sensors[2].second = sensorTemperatures.sensor_rw1.value;
+  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[3].second = sensorTemperatures.sensor_dro.value;
+  numSensors = 4;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
+
+  // RW4
+  sensors[0].first = deviceTemperatures.rw4.isValid();
+  sensors[0].second = deviceTemperatures.rw4.value;
+  sensors[1].first = deviceTemperatures.rw1.isValid();
+  sensors[1].second = deviceTemperatures.rw1.value;
+  sensors[2].first = sensorTemperatures.sensor_rw1.isValid();
+  sensors[2].second = sensorTemperatures.sensor_rw1.value;
+  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[3].second = sensorTemperatures.sensor_dro.value;
+  numSensors = 4;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
+}
+
+void ThermalController::ctrlStr() {
+  sensors[0].first = sensorTemperatures.sensor_startracker.isValid();
+  sensors[0].second = sensorTemperatures.sensor_startracker.value;
+  sensors[1].first = deviceTemperatures.startracker.isValid();
+  sensors[1].second = deviceTemperatures.startracker.value;
+  sensors[2].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[2].second = sensorTemperatures.sensor_dro.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_5_STR, heater::HEATER_6_DRO, strLimits);
+}
+
+void ThermalController::ctrlIfBoard() {
+  sensors[0].first = sensorTemperatures.tmp1075IfBrd.isValid();
+  sensors[0].second = sensorTemperatures.tmp1075IfBrd.value;
+  sensors[1].first = sensorTemperatures.sensor_magnettorquer.isValid();
+  sensors[1].second = sensorTemperatures.sensor_magnettorquer.value;
+  sensors[2].first = deviceTemperatures.mgm2SideB.isValid();
+  sensors[2].second = deviceTemperatures.mgm2SideB.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits);
+}
+
+void ThermalController::ctrlTcsBoard() {
+  sensors[0].first = sensorTemperatures.sensor_tcs_board.isValid();
+  sensors[0].second = sensorTemperatures.sensor_tcs_board.value;
+  sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid();
+  sensors[1].second = sensorTemperatures.tmp1075Tcs0.value;
+  sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid();
+  sensors[2].second = sensorTemperatures.tmp1075Tcs1.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits);
+}
+
+void ThermalController::ctrlObc() {
+  sensors[0].first = deviceTemperatures.q7s.isValid();
+  sensors[0].second = deviceTemperatures.q7s.value;
+  sensors[1].first = sensorTemperatures.tmp1075Tcs1.isValid();
+  sensors[1].second = sensorTemperatures.tmp1075Tcs1.value;
+  sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid();
+  sensors[2].second = sensorTemperatures.tmp1075Tcs0.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits);
+  if (componentAboveCutOffLimit) {
+    triggerEvent(OBC_OVERHEATING);
+  }
+}
+
+void ThermalController::ctrlObcIfBoard() {
+  sensors[0].first = deviceTemperatures.q7s.isValid();
+  sensors[0].second = deviceTemperatures.q7s.value;
+  sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid();
+  sensors[1].second = sensorTemperatures.tmp1075Tcs0.value;
+  sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid();
+  sensors[2].second = sensorTemperatures.tmp1075Tcs1.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits);
+}
+
+void ThermalController::ctrlSBandTransceiver() {
+  sensors[0].first = deviceTemperatures.syrlinksPowerAmplifier.isValid();
+  sensors[0].second = deviceTemperatures.syrlinksPowerAmplifier.value;
+  sensors[1].first = deviceTemperatures.syrlinksBasebandBoard.isValid();
+  sensors[1].second = deviceTemperatures.syrlinksBasebandBoard.value;
+  sensors[2].first = sensorTemperatures.sensor_4k_camera.isValid();
+  sensors[2].second = sensorTemperatures.sensor_4k_camera.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_7_S_BAND, heater::HEATER_4_CAMERA,
+                           sBandTransceiverLimits);
+  if (componentAboveCutOffLimit) {
+    triggerEvent(SYRLINKS_OVERHEATING);
+  }
+}
+void ThermalController::ctrlPcduP60Board() {
+  sensors[0].first = deviceTemperatures.temp1P60dock.isValid();
+  sensors[0].second = deviceTemperatures.temp1P60dock.value;
+  sensors[1].first = deviceTemperatures.temp2P60dock.isValid();
+  sensors[1].second = deviceTemperatures.temp2P60dock.value;
+  numSensors = 2;
+  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits);
+}
+
+void ThermalController::ctrlPcduAcu() {
+  heater::Switchers switchNr = heater::HEATER_3_PCDU_PDU;
+  heater::Switchers redSwitchNr = heater::HEATER_2_ACS_BRD;
+
+  if (chooseHeater(switchNr, redSwitchNr)) {
+    bool sensorTempAvailable = true;
+
+    if (deviceTemperatures.acu.value[0] != INVALID_TEMPERATURE) {
+      sensorTemp = deviceTemperatures.acu.value[0];
+    } else if (deviceTemperatures.acu.value[1] != INVALID_TEMPERATURE) {
+      sensorTemp = deviceTemperatures.acu.value[1];
+    } else if (deviceTemperatures.acu.value[2] != INVALID_TEMPERATURE) {
+      sensorTemp = deviceTemperatures.acu.value[2];
+    } else if (sensorTemperatures.sensor_acu.isValid()) {
+      sensorTemp = sensorTemperatures.sensor_acu.value;
+    } else {
+      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
+      sensorTempAvailable = false;
+    }
+    if (sensorTempAvailable) {
+      ctrlHeater(switchNr, redSwitchNr, pcduAcuLimits);
+    }
+  }
+}
+
+void ThermalController::ctrlPcduPdu() {
+  sensors[0].first = deviceTemperatures.pdu1.isValid();
+  sensors[0].second = deviceTemperatures.pdu1.value;
+  sensors[1].first = deviceTemperatures.pdu2.isValid();
+  sensors[1].second = deviceTemperatures.pdu2.value;
+  sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid();
+  sensors[2].second = sensorTemperatures.tmp1075Tcs0.value;
+  numSensors = 2;
+  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits);
+}
+
+void ThermalController::ctrlPlPcduBoard() {
+  sensors[0].first = sensorTemperatures.tmp1075PlPcdu0.isValid();
+  sensors[0].second = sensorTemperatures.tmp1075PlPcdu0.value;
+  sensors[1].first = sensorTemperatures.tmp1075PlPcdu1.isValid();
+  sensors[1].second = sensorTemperatures.tmp1075PlPcdu1.value;
+  sensors[2].first = deviceTemperatures.adcPayloadPcdu.isValid();
+  sensors[2].second = deviceTemperatures.adcPayloadPcdu.value;
+  sensors[3].first = sensorTemperatures.sensor_plpcdu_heatspreader.isValid();
+  sensors[3].second = sensorTemperatures.sensor_plpcdu_heatspreader.value;
+  numSensors = 4;
+  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits);
+  if (componentAboveCutOffLimit) {
+    triggerEvent(PLPCDU_OVERHEATING);
+  }
+}
+
+void ThermalController::ctrlPlocMissionBoard() {
+  sensors[0].first = sensorTemperatures.sensor_ploc_heatspreader.isValid();
+  sensors[0].second = sensorTemperatures.sensor_ploc_heatspreader.value;
+  sensors[1].first = sensorTemperatures.sensor_ploc_missionboard.isValid();
+  sensors[1].second = sensorTemperatures.sensor_ploc_missionboard.value;
+  sensors[2].first = sensorTemperatures.sensor_dac_heatspreader.isValid();
+  sensors[2].second = sensorTemperatures.sensor_dac_heatspreader.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
+                           plocMissionBoardLimits);
+  if (componentAboveCutOffLimit) {
+    triggerEvent(PLOC_OVERHEATING);
+  }
+}
+
+void ThermalController::ctrlPlocProcessingBoard() {
+  sensors[0].first = sensorTemperatures.sensor_ploc_missionboard.isValid();
+  sensors[0].second = sensorTemperatures.sensor_ploc_missionboard.value;
+  sensors[1].first = sensorTemperatures.sensor_ploc_heatspreader.isValid();
+  sensors[1].second = sensorTemperatures.sensor_ploc_heatspreader.value;
+  sensors[2].first = sensorTemperatures.sensor_dac_heatspreader.isValid();
+  sensors[2].second = sensorTemperatures.sensor_dac_heatspreader.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
+                           plocProcessingBoardLimits);
+}
+
+void ThermalController::ctrlDac() {
+  sensors[0].first = sensorTemperatures.sensor_dac_heatspreader.isValid();
+  sensors[0].second = sensorTemperatures.sensor_dac_heatspreader.value;
+  sensors[1].first = sensorTemperatures.sensor_ploc_missionboard.isValid();
+  sensors[1].second = sensorTemperatures.sensor_ploc_missionboard.value;
+  sensors[2].first = sensorTemperatures.sensor_ploc_heatspreader.isValid();
+  sensors[2].second = sensorTemperatures.sensor_ploc_heatspreader.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits);
+}
+
+void ThermalController::ctrlCameraBody() {
+  sensors[0].first = sensorTemperatures.sensor_4k_camera.isValid();
+  sensors[0].second = sensorTemperatures.sensor_4k_camera.value;
+  sensors[1].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[1].second = sensorTemperatures.sensor_dro.value;
+  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
+  sensors[2].second = sensorTemperatures.sensor_mpa.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_4_CAMERA, heater::HEATER_6_DRO, cameraLimits);
+}
+
+void ThermalController::ctrlDro() {
+  sensors[0].first = sensorTemperatures.sensor_dro.isValid();
+  sensors[0].second = sensorTemperatures.sensor_dro.value;
+  sensors[1].first = sensorTemperatures.sensor_4k_camera.isValid();
+  sensors[1].second = sensorTemperatures.sensor_4k_camera.value;
+  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
+  sensors[2].second = sensorTemperatures.sensor_mpa.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, droLimits);
+}
+
+void ThermalController::ctrlX8() {
+  sensors[0].first = sensorTemperatures.sensor_x8.isValid();
+  sensors[0].second = sensorTemperatures.sensor_x8.value;
+  sensors[1].first = sensorTemperatures.sensor_hpa.isValid();
+  sensors[1].second = sensorTemperatures.sensor_hpa.value;
+  sensors[2].first = sensorTemperatures.sensor_tx_modul.isValid();
+  sensors[2].second = sensorTemperatures.sensor_tx_modul.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, x8Limits);
+}
+
+void ThermalController::ctrlTx() {
+  sensors[0].first = sensorTemperatures.sensor_tx_modul.isValid();
+  sensors[0].second = sensorTemperatures.sensor_tx_modul.value;
+  sensors[1].first = sensorTemperatures.sensor_x8.isValid();
+  sensors[1].second = sensorTemperatures.sensor_x8.value;
+  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
+  sensors[2].second = sensorTemperatures.sensor_mpa.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, txLimits);
+}
+
+void ThermalController::ctrlMpa() {
+  sensors[0].first = sensorTemperatures.sensor_mpa.isValid();
+  sensors[0].second = sensorTemperatures.sensor_mpa.value;
+  sensors[1].first = sensorTemperatures.sensor_hpa.isValid();
+  sensors[1].second = sensorTemperatures.sensor_hpa.value;
+  sensors[2].first = sensorTemperatures.sensor_tx_modul.isValid();
+  sensors[2].second = sensorTemperatures.sensor_tx_modul.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, mpaLimits);
+}
+
+void ThermalController::ctrlHpa() {
+  sensors[0].first = sensorTemperatures.sensor_hpa.isValid();
+  sensors[0].second = sensorTemperatures.sensor_hpa.value;
+  sensors[1].first = sensorTemperatures.sensor_x8.isValid();
+  sensors[1].second = sensorTemperatures.sensor_x8.value;
+  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
+  sensors[2].second = sensorTemperatures.sensor_mpa.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, hpaLimits);
+  if (componentAboveCutOffLimit) {
+    triggerEvent(HPA_OVERHEATING);
+  }
+}
+
+void ThermalController::ctrlScexBoard() {
+  sensors[0].first = sensorTemperatures.sensor_scex.isValid();
+  sensors[0].second = sensorTemperatures.sensor_scex.value;
+  sensors[1].first = sensorTemperatures.sensor_x8.isValid();
+  sensors[1].second = sensorTemperatures.sensor_x8.value;
+  sensors[2].first = sensorTemperatures.sensor_hpa.isValid();
+  sensors[2].second = sensorTemperatures.sensor_hpa.value;
+  numSensors = 3;
+  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_5_STR, scexBoardLimits);
+}
+
+void ThermalController::ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr,
+                                   struct TempLimits& tempLimit) {
+  componentAboveCutOffLimit = false;
+  // Heater off
+  if (not heaterHandler.checkSwitchState(switchNr)) {
+    if (sensorTemp < tempLimit.opLowerLimit) {
+      heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::ON);
+      sif::info << "ThermalController::ctrlHeater: Heater" << switchNr << " ON" << std::endl;
+    }
+    // Heater on
+  } else if (heaterHandler.checkSwitchState(switchNr)) {
+    if (sensorTemp >= tempLimit.opLowerLimit + TEMP_OFFSET) {
+      heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+      sif::info << "ThermalController::ctrlHeater: Heater" << switchNr << " OFF" << std::endl;
+    }
+  } else if (not redSwitchNrInUse) {
+    if (heaterHandler.checkSwitchState(redSwitchNr)) {
+      if (sensorTemp >= tempLimit.cutOffLimit) {
+        componentAboveCutOffLimit = true;
+        heaterHandler.switchHeater(redSwitchNr, HeaterHandler::SwitchState::OFF);
+        sif::info << "ThermalController::ctrlHeater: Heater" << redSwitchNr << " OFF" << std::endl;
+      }
+    }
+  }
+}
+
+bool ThermalController::chooseHeater(heater::Switchers& switchNr, heater::Switchers redSwitchNr) {
+  bool heaterAvailable = true;
+
+  if (heaterHandler.getHealth(switchNr) != HasHealthIF::HEALTHY) {
+    if (heaterHandler.getHealth(redSwitchNr) == HasHealthIF::HEALTHY) {
+      switchNr = redSwitchNr;
+      redSwitchNrInUse = true;
+    } else {
+      heaterAvailable = false;
+      triggerEvent(NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr);
+    }
+  } else {
+    redSwitchNrInUse = false;
+  }
+  return heaterAvailable;
+}
+
+bool ThermalController::selectAndReadSensorTemp() {
+  for (unsigned i = 0; i < numSensors; i++) {
+    if (sensors[i].first and sensors[i].second != INVALID_TEMPERATURE) {
+      sensorTemp = sensors[i].second;
+      return true;
+    }
+  }
+  triggerEvent(NO_VALID_SENSOR_TEMPERATURE);
+  return false;
+}
+
+void ThermalController::ctrlComponentTemperature(heater::Switchers switchNr,
+                                                 heater::Switchers redSwitchNr,
+                                                 TempLimits& tempLimit) {
+  if (selectAndReadSensorTemp()) {
+    if (chooseHeater(switchNr, redSwitchNr)) {
+      ctrlHeater(switchNr, redSwitchNr, tempLimit);
+    }
+  } else {
+    if (chooseHeater(switchNr,
+                     redSwitchNr)) {  // TODO: muss der Heater dann wirklich abgeschalten werden?
+      if (heaterHandler.checkSwitchState(switchNr)) {
+        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+        sif::info << "ThermalController::ctrlHeater: Heater" << switchNr << " OFF" << std::endl;
+      }
+    }
+  }
+  resetSensorsArray();
+}
+
+void ThermalController::resetSensorsArray() {
+  //TODO: müssen auch andere Variablen resettet werden? senstemp?
+  for (auto& validValuePair : sensors) {
+    validValuePair.first = false;
+    validValuePair.second = INVALID_TEMPERATURE;
+  }
+}