From 9cf65be1be3e8e74b53e8d6e401b4b5fb57ed04e Mon Sep 17 00:00:00 2001
From: Irini Kosmidou <kosmidoui@irs.uni-stuttgart.de>
Date: Tue, 7 Feb 2023 14:28:42 +0100
Subject: [PATCH] ThermalController upgrade

---
 mission/controller/ThermalController.cpp | 230 +++++++++++------------
 mission/controller/ThermalController.h   |   9 +-
 2 files changed, 114 insertions(+), 125 deletions(-)

diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index aa49ecb5..a0b710fc 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -984,23 +984,50 @@ void ThermalController::copyDevices() {
 }
 
 void ThermalController::ctrlAcsBoard() {
-  // TODO: fix
+  // TODO: check
   heater::Switchers switchNr = heater::HEATER_2_ACS_BRD;
   heater::Switchers redSwitchNr = heater::HEATER_0_OBC_BRD;
 
-  if (chooseHeater(switchNr, redSwitchNr)) {
-    // A side
-    if (chooseOf4Sensors(switchNr, deviceTemperatures.gyro0SideA, deviceTemperatures.mgm0SideA,
-                         deviceTemperatures.gyro1SideA, sensorTemperatures.sensor_tcs_board)) {
+  // 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);
-      return;
     }
-    // B side
-    if (chooseOf4Sensors(switchNr, deviceTemperatures.gyro2SideB, deviceTemperatures.mgm2SideB,
-                         deviceTemperatures.gyro3SideB, sensorTemperatures.sensor_tcs_board)) {
+    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() {
@@ -1016,100 +1043,59 @@ void ThermalController::ctrlMgt() {
     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);
+    ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits);
   }
 }
 
 void ThermalController::ctrlRw() {
-  //TODO: better solution?
-  heater::Switchers switchNr = heater::HEATER_6_DRO;
-  heater::Switchers redSwitchNr = heater::HEATER_6_DRO;
-  redSwitchNrInUse = false;
+  // 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);
 
-  if (heaterHandler.getHealth(switchNr) == HasHealthIF::HEALTHY) {
-    // RW1
-    bool sensorTempAvailable = true;
+  // 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);
 
-    if (sensorTemperatures.sensor_rw1.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_rw1.value;
-    } else if (deviceTemperatures.rw1.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw1.value);
-    } else if (deviceTemperatures.rw4.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw4.value);
-    } else if (sensorTemperatures.sensor_dro.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_dro.value;
-    } else {
-      if (heaterHandler.checkSwitchState(switchNr)) {
-        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
-      }
-      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
-      sensorTempAvailable = false;
-    }
-    if (sensorTempAvailable) {
-      ctrlHeater(switchNr, redSwitchNr, rwLimits);
-    }
-    // RW2
-    sensorTempAvailable = true;
-    if (deviceTemperatures.rw2.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw2.value);
-    } else if (deviceTemperatures.rw3.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw3.value);
-    } else if (sensorTemperatures.sensor_rw1.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_rw1.value;
-    } else if (sensorTemperatures.sensor_dro.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_dro.value;
-    } else {
-      if (heaterHandler.checkSwitchState(switchNr)) {
-        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
-      }
-      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
-      sensorTempAvailable = false;
-    }
-    if (sensorTempAvailable) {
-      ctrlHeater(switchNr, redSwitchNr, rwLimits);
-    }
-    // RW3
-    sensorTempAvailable = true;
-    if (deviceTemperatures.rw3.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw3.value);
-    } else if (deviceTemperatures.rw4.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw4.value);
-    } else if (sensorTemperatures.sensor_rw1.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_rw1.value;
-    } else if (sensorTemperatures.sensor_dro.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_dro.value;
-    } else {
-      if (heaterHandler.checkSwitchState(switchNr)) {
-        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
-      }
-      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
-      sensorTempAvailable = false;
-    }
-    if (sensorTempAvailable) {
-      ctrlHeater(switchNr, redSwitchNr, rwLimits);
-    }
-    // RW4
-    sensorTempAvailable = true;
-    if (deviceTemperatures.rw4.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw4.value);
-    } else if (deviceTemperatures.rw1.isValid()) {
-      sensorTemp = static_cast<float>(deviceTemperatures.rw1.value);
-    } else if (sensorTemperatures.sensor_rw1.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_rw1.value;
-    } else if (sensorTemperatures.sensor_dro.isValid()) {
-      sensorTemp = sensorTemperatures.sensor_dro.value;
-    } else {
-      if (heaterHandler.checkSwitchState(switchNr)) {
-        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
-      }
-      triggerEvent(NO_VALID_SENSOR_TEMPERATURE, switchNr);
-      sensorTempAvailable = false;
-    }
-    if (sensorTempAvailable) {
-      ctrlHeater(switchNr, redSwitchNr, 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() {
@@ -1120,18 +1106,17 @@ void ThermalController::ctrlStr() {
   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);
+  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;
+  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);
 }
 
@@ -1179,7 +1164,8 @@ void ThermalController::ctrlSBandTransceiver() {
   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);
+  ctrlComponentTemperature(heater::HEATER_7_S_BAND, heater::HEATER_4_CAMERA,
+                           sBandTransceiverLimits);
   if (componentAboveCutOffLimit) {
     triggerEvent(SYRLINKS_OVERHEATING);
   }
@@ -1253,7 +1239,8 @@ void ThermalController::ctrlPlocMissionBoard() {
   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);
+  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
+                           plocMissionBoardLimits);
   if (componentAboveCutOffLimit) {
     triggerEvent(PLOC_OVERHEATING);
   }
@@ -1267,7 +1254,8 @@ void ThermalController::ctrlPlocProcessingBoard() {
   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);
+  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
+                           plocProcessingBoardLimits);
 }
 
 void ThermalController::ctrlDac() {
@@ -1404,37 +1392,39 @@ bool ThermalController::chooseHeater(heater::Switchers& switchNr, heater::Switch
   return heaterAvailable;
 }
 
-bool ThermalController::selectAndReadSensorTemp(heater::Switchers switchNr) {
- for(unsigned i= 0; i<numSensors; i++) {
+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, switchNr);
-    return false;
+  triggerEvent(NO_VALID_SENSOR_TEMPERATURE);
+  return false;
 }
 
 void ThermalController::ctrlComponentTemperature(heater::Switchers switchNr,
                                                  heater::Switchers redSwitchNr,
                                                  TempLimits& tempLimit) {
-
-  if (selectAndReadSensorTemp(switchNr)) {
+  if (selectAndReadSensorTemp()) {
     if (chooseHeater(switchNr, redSwitchNr)) {
       ctrlHeater(switchNr, redSwitchNr, tempLimit);
     }
   } else {
-    chooseHeater(switchNr, redSwitchNr);
-        if (heaterHandler.checkSwitchState(switchNr)) {
-      heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
-      sif::info << "ThermalController::ctrlHeater: Heater" << switchNr << " OFF" << std::endl;
+    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() {
-  for(auto& validValuePair:sensors){
+  //TODO: müssen auch andere Variablen resettet werden? senstemp?
+  for (auto& validValuePair : sensors) {
     validValuePair.first = false;
     validValuePair.second = INVALID_TEMPERATURE;
   }
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index 80b43ad2..79ef4a95 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -47,12 +47,12 @@ class ThermalController : public ExtendedControllerBase {
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
   void performControlOperation() override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
-                                                LocalDataPoolManager& poolManager) override;
+                                        LocalDataPoolManager& poolManager) override;
   LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
 
   // Mode abstract functions
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
-                                         uint32_t* msToReachTheMode) override;
+                                 uint32_t* msToReachTheMode) override;
 
  private:
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::TCS_CONTROLLER;
@@ -151,8 +151,7 @@ class ThermalController : public ExtendedControllerBase {
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(DELAY);
 
-
-  std::array<std::pair<bool, double>, 5>  sensors;
+  std::array<std::pair<bool, double>, 5> sensors;
   uint8_t numSensors = 0;
 
   PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>(10.0);
@@ -172,7 +171,7 @@ class ThermalController : public ExtendedControllerBase {
                                 TempLimits& tempLimit);
   void ctrlHeater(heater::Switchers switchNr, heater::Switchers redSwitchNr, TempLimits& tempLimit);
   bool chooseHeater(heater::Switchers& switchNr, heater::Switchers redSwitchNr);
-  bool selectAndReadSensorTemp(heater::Switchers switchNr);
+  bool selectAndReadSensorTemp();
 
   void ctrlAcsBoard();
   void ctrlMgt();