diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index e06e568d..738b9e14 100644
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@@ -183,6 +183,7 @@ void ThermalController::performControlOperation() {
     if (allSwitchersOff or transitionWhenHeatersOffCycles == 6) {
       // Finish the transition
       transitionWhenHeatersOff = false;
+      resetThermalStates();
       setMode(targetMode, targetSubmode);
     } else {
       transitionWhenHeatersOffCycles++;
@@ -1571,10 +1572,9 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) {
     // No sensors available, so switch the heater off. We can not perform control tasks if we
     // are blind..
     if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) {
-      if (submode != SUBMODE_NO_HEATER_CTRL) {
-        if (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON) {
-          heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
-        }
+      if (heaterCtrlAllowed() and
+          (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) {
+        heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
       }
     }
   }
@@ -1622,20 +1622,19 @@ bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch re
 }
 
 void ThermalController::heaterCtrlTempTooHighHandler(HeaterContext& htrCtx, const char* whatLimit) {
+  if (not heaterCtrlAllowed()) {
+    return;
+  }
   if (htrCtx.switchState == HeaterHandler::SwitchState::ON) {
-    if (submode != SUBMODE_NO_HEATER_CTRL) {
-      sif::info << "TCS: Component " << static_cast<int>(thermalComponent) << " too warm, above "
-                << whatLimit << ", switching off heater" << std::endl;
-      heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
-      heaterStates[htrCtx.switchNr].switchTransition = true;
-    }
+    sif::info << "TCS: Component " << static_cast<int>(thermalComponent) << " too warm, above "
+              << whatLimit << ", switching off heater" << std::endl;
+    heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
+    heaterStates[htrCtx.switchNr].switchTransition = true;
     heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF;
   }
   if (heaterHandler.getSwitchState(htrCtx.redSwitchNr) == HeaterHandler::SwitchState::ON) {
-    if (submode != SUBMODE_NO_HEATER_CTRL) {
-      heaterHandler.switchHeater(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF);
-      heaterStates[htrCtx.redSwitchNr].switchTransition = true;
-    }
+    heaterSwitchHelper(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
+    heaterStates[htrCtx.redSwitchNr].switchTransition = true;
     heaterStates[htrCtx.redSwitchNr].target = HeaterHandler::SwitchState::OFF;
   }
 }
@@ -1649,42 +1648,44 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) {
   if (heaterStates[htrCtx.switchNr].switchTransition) {
     htrCtx.doHeaterHandling = false;
     heaterCtrlCheckUpperLimits(htrCtx);
-  } else {
-    // Heater off
-    htrCtx.switchState = heaterHandler.getSwitchState(htrCtx.switchNr);
-    if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) {
-      if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) {
-        heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::ON);
-        sif::info << "TCS: Heater " << static_cast<int>(thermalComponent) << " ON" << std::endl;
+    return;
+  }
+
+  htrCtx.switchState = heaterHandler.getSwitchState(htrCtx.switchNr);
+  // Heater off
+  if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) {
+    if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) {
+      sif::info << "TCS: Heater " << static_cast<int>(thermalComponent) << " ON" << std::endl;
+      heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent);
+      heaterStates[htrCtx.switchNr].switchTransition = true;
+      heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON;
+    } else {
+      // Even if heater control is now allowed, we can update the state.
+      thermalStates[thermalComponent].heating = false;
+    }
+    heaterCtrlCheckUpperLimits(htrCtx);
+    return;
+  }
+
+  // Heater on
+  if (htrCtx.switchState == HeaterHandler::SwitchState::ON) {
+    if (thermalStates[thermalComponent].heating) {
+      // We are already in a heating cycle, so need to check whether heating task is complete.
+      if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) {
+        sif::info << "TCS: Heater" << static_cast<int>(thermalComponent) << " OFF" << std::endl;
+        heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
         heaterStates[htrCtx.switchNr].switchTransition = true;
-        thermalStates[thermalComponent].heating = true;
-        heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON;
-      } else {
-        // Even if heater control is now allowed, we can update the state.
-        thermalStates[thermalComponent].heating = false;
+        heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF;
       }
-      heaterCtrlCheckUpperLimits(htrCtx);
-      // Heater on
-    } else if (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON) {
-      if (thermalStates[thermalComponent].heating) {
-        // We are already in a heating cycle, so need to check whether heating task is complete.
-        if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) {
-          heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
-          sif::info << "TCS: Heater" << static_cast<int>(thermalComponent) << " OFF" << std::endl;
-          heaterStates[htrCtx.switchNr].switchTransition = true;
-          heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF;
-          thermalStates[thermalComponent].heating = false;
-        }
-      } else {
-        // This can happen if heater is used as alternative heater (no regular heating cycle), so we
-        // should still check the upper limits.
-        bool tooHighHandlerAlreadyCalled = heaterCtrlCheckUpperLimits(htrCtx);
-        if (sensorTemp >= htrCtx.tempLimit.cutOffLimit) {
-          componentAboveCutOffLimit = true;
-          if (not tooHighHandlerAlreadyCalled) {
-            heaterCtrlTempTooHighHandler(htrCtx, "CutOff-Limit");
-          }
-        }
+      return;
+    }
+    // This can happen if heater is used as alternative heater (no regular heating cycle), so we
+    // should still check the upper limits.
+    bool tooHighHandlerAlreadyCalled = heaterCtrlCheckUpperLimits(htrCtx);
+    if (sensorTemp >= htrCtx.tempLimit.cutOffLimit) {
+      componentAboveCutOffLimit = true;
+      if (not tooHighHandlerAlreadyCalled) {
+        heaterCtrlTempTooHighHandler(htrCtx, "CutOff-Limit");
       }
     }
   }
@@ -1763,6 +1764,28 @@ bool ThermalController::tooHotHandler(object_id_t object, bool& oneShotFlag) {
 
 bool ThermalController::heaterCtrlAllowed() const { return submode != SUBMODE_NO_HEATER_CTRL; }
 
+void ThermalController::resetThermalStates() {
+  for (auto& thermalState : thermalStates) {
+    thermalState.heating = false;
+  }
+}
+
+void ThermalController::heaterSwitchHelper(heater::Switch switchNr,
+                                           HeaterHandler::SwitchState state,
+                                           unsigned componentIdx) {
+  timeval currentTime;
+  Clock::getClockMonotonic(&currentTime);
+  if (state == HeaterHandler::SwitchState::ON) {
+    heaterHandler.switchHeater(switchNr, state);
+    thermalStates[componentIdx].heating = true;
+    thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec;
+  } else {
+    heaterHandler.switchHeater(switchNr, state);
+    thermalStates[componentIdx].heating = false;
+    thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec;
+  }
+}
+
 void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag) {
   // Clear the one shot flag is the component is in acceptable temperature range.
   if (not tooHotHandler(object, oneShotFlag) and not componentAboveUpperLimit) {
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index 34ffcd49..af27dd29 100644
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@@ -47,8 +47,13 @@ struct TempLimits {
 
 struct ThermalState {
   uint8_t errorCounter;
-  bool heating;
-  uint32_t heaterStartTime;
+  // Is heating on for that thermal module?
+  bool heating = false;
+  heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES;
+  // Heater start time and end times as UNIX seconds. Please note that these times will be updated
+  // when a switch command is sent, with no guarantess that the heater actually went on.
+  uint32_t heaterStartTime = 0;
+  uint32_t heaterEndTime = 0;
 };
 
 struct HeaterState {
@@ -295,6 +300,7 @@ class ThermalController : public ExtendedControllerBase {
   void startTransition(Mode_t mode, Submode_t submode) override;
 
   bool heaterCtrlAllowed() const;
+  void resetThermalStates();
 
   void resetSensorsArray();
   void copySensors();
@@ -309,6 +315,9 @@ class ThermalController : public ExtendedControllerBase {
   bool chooseHeater(heater::Switch& switchNr, heater::Switch redSwitchNr);
   bool selectAndReadSensorTemp(HeaterContext& htrCtx);
 
+  void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state,
+                          unsigned componentIdx);
+
   void ctrlAcsBoard();
   void ctrlMgt();
   void ctrlRw();