better handling for TCS ctrl state

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,13 +1572,12 @@ 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 (heaterCtrlAllowed() and
-        if (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON) {
+          (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) {
         heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
       }
     }
   }
-  }
   resetSensorsArray();
 }
 bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) {
@@ -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);
+    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) {
+    heaterSwitchHelper(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
-      heaterHandler.switchHeater(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF);
     heaterStates[htrCtx.redSwitchNr].switchTransition = true;
-    }
     heaterStates[htrCtx.redSwitchNr].target = HeaterHandler::SwitchState::OFF;
   }
 }
@@ -1649,33 +1648,37 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) {
   if (heaterStates[htrCtx.switchNr].switchTransition) {
     htrCtx.doHeaterHandling = false;
     heaterCtrlCheckUpperLimits(htrCtx);
-  } else {
+    return;
-    // Heater off
+  }
+
   htrCtx.switchState = heaterHandler.getSwitchState(htrCtx.switchNr);
+  // Heater off
   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;
+      heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, 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;
     }
     heaterCtrlCheckUpperLimits(htrCtx);
+    return;
+  }
+
   // Heater on
-    } else if (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::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()) {
-          heaterHandler.switchHeater(htrCtx.switchNr, HeaterHandler::SwitchState::OFF);
         sif::info << "TCS: Heater" << static_cast<int>(thermalComponent) << " OFF" << std::endl;
+        heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent);
         heaterStates[htrCtx.switchNr].switchTransition = true;
         heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF;
-          thermalStates[thermalComponent].heating = false;
       }
-      } else {
+      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);
@@ -1686,8 +1689,6 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) {
       }
     }
   }
-    }
-  }
 }
 
 bool ThermalController::heaterCtrlCheckUpperLimits(HeaterContext& htrCtx) {
@@ -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) {

mission/controller/ThermalController.h

@@ -47,8 +47,13 @@ struct TempLimits {
 
 struct ThermalState {
   uint8_t errorCounter;
-  bool heating;
+  // Is heating on for that thermal module?
-  uint32_t heaterStartTime;
+  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();