2023-02-24 19:03:39 +01:00
16 changed files with 347 additions and 188 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -17,6 +17,39 @@ change warranting a new major release:

 # [unreleased]

+# [v1.29.1]
+
+## Fixed
+
+- Limit number of handled messages for core TM handlers:
+  - https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/391
+  - https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/390
+  - https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/389
+- HeaterHandler better handling for faulty message reception
+  Issue: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/388
+- Disable stopwatch in MAX31865 polling task
+
+# [v1.29.0]
+
+eive-tmtc: v2.13.0
+
+## Changed
+
+- Refactored IMTQ handlers to also perform low level I2C communication tasks in separate thread.
+  This avoids the various delays needed for I2C communication with that device inside the ACS PST.
+  (e.g. 1 ms delay between each transfer, or 10 ms integration delay for MGM measurements).
+
+## Added
+
+- Added new heater info set for the TCS controller. This set contains the heater switch states
+  and the current draw.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
+- The HeaterHandler now exposes a mode which reflects whether the heater power
+  is on or off. It also triggers mode events for its heater children objects
+  which show whether the specific heaters are on or off. The heater handler
+  will be part of the TCS tree.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
+
 # [v1.28.1]

 ## Fixed
@ -24,13 +57,14 @@ change warranting a new major release:
 - Patch version which compiles for EM
 - CFDP Funnel bugfix: CCSDS wrapping was buggy and works properly now.
 - CMakeLists.txt fix which broke CI/CD builds when server could not retrieve full git SHA.
+- Possible regression in the MAX31865 polling task: Using a `ManualCsLockGuard` for reconfiguring
+  and then polling the sensor is problematic, invalid sensor values will be read.
+  CS probably needs to be de-asserted or some other HW/SPI specific issue. Letting the SPI ComIF
+  do the locking does the job.

 ## Changed

 - Add `-Wshadow=local` shadowing warnings and fixed all of them
- Refactored IMTQ handlers to also perform low level I2C communication tasks in separate thread.
-  This avoids the various delays needed for I2C communication with that device inside the ACS PST.
-  (e.g. 1 ms delay between each transfer, or 10 ms integration delay for MGM measurements).
 - Updated generated CSV files: Support for skip directive and explicit
  "No description" info string
 - The polling threads for actuator polling now have a slightly higher priority than the ACS PST
@ -165,6 +199,7 @@ eive-tmtc: v2.12.2
 ## Changed

 - Reworked dummy handling for the TCS controller.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/325
 - Generator scripts now generate files for hosted and for Q7S build.

 ## Fixed
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)

 set(OBSW_VERSION_MAJOR 1)
-set(OBSW_VERSION_MINOR 28)
+set(OBSW_VERSION_MINOR 29)
 set(OBSW_VERSION_REVISION 1)

 # set(CMAKE_VERBOSE TRUE)
--- a/linux/ObjectFactory.cpp
+++ b/linux/ObjectFactory.cpp
@ -9,7 +9,7 @@
 #include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
 #include <linux/callbacks/gpioCallbacks.h>
-#include <linux/devices/Max31865RtdLowlevelHandler.h>
+#include <linux/devices/Max31865RtdPolling.h>
 #include <mission/controller/AcsController.h>
 #include <mission/core/GenericFactory.h>
 #include <mission/devices/Max31865EiveHandler.h>
@ -278,7 +278,7 @@ void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF,
  TcsBoardAssembly* tcsBoardAss = ObjectFactory::createTcsBoardAssy(*pwrSwitcher);

  // Create special low level reader communication interface
-  new Max31865RtdReader(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
+  new Max31865RtdPolling(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
  for (uint8_t idx = 0; idx < NUM_RTDS; idx++) {
    rtdCookies[idx] = new SpiCookie(cookieArgs[idx].first, cookieArgs[idx].second,
                                    MAX31865::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
--- a/linux/devices/CMakeLists.txt
+++ b/linux/devices/CMakeLists.txt
@ -3,9 +3,8 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER)
 endif()

 target_sources(
-  ${OBSW_NAME}
-  PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp ScexDleParser.cpp
-          ScexHelper.cpp RwPollingTask.cpp ImtqPollingTask.cpp)
+  ${OBSW_NAME} PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp
+                       ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)

 add_subdirectory(ploc)

--- a/linux/devices/Max31865RtdLowlevelHandler.cpp
+++ b/linux/devices/Max31865RtdLowlevelHandler.cpp
@ -1,8 +1,7 @@
-#include "Max31865RtdLowlevelHandler.h"
-
 #include <fsfw/tasks/TaskFactory.h>
 #include <fsfw/timemanager/Stopwatch.h>
 #include <fsfw_hal/linux/spi/ManualCsLockGuard.h>
+#include <linux/devices/Max31865RtdPolling.h>

 #define OBSW_RTD_AUTO_MODE 1

@ -17,12 +16,13 @@ static constexpr uint8_t BASE_CFG =
    (MAX31865::ConvMode::NORM_OFF << MAX31865::CfgBitPos::CONV_MODE);
 #endif

-Max31865RtdReader::Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF)
+Max31865RtdPolling::Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF,
+                                       GpioIF* gpioIF)
    : SystemObject(objectId), rtds(EiveMax31855::NUM_RTDS), comIF(lowLevelComIF), gpioIF(gpioIF) {
  readerMutex = MutexFactory::instance()->createMutex();
 }

-ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
+ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) {
  using namespace MAX31865;
  ReturnValue_t result = returnvalue::OK;
  static_cast<void>(result);
@ -49,17 +49,16 @@ ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
  return periodicReadHandling();
 }

-bool Max31865RtdReader::rtdIsActive(uint8_t idx) {
+bool Max31865RtdPolling::rtdIsActive(uint8_t idx) {
  if (rtds[idx]->on and rtds[idx]->db.active and rtds[idx]->db.configured) {
    return true;
  }
  return false;
 }

-bool Max31865RtdReader::periodicInitHandling() {
+bool Max31865RtdPolling::periodicInitHandling() {
  using namespace MAX31865;
  ReturnValue_t result = returnvalue::OK;
-
  for (auto& rtd : rtds) {
    if (rtd == nullptr) {
      continue;
@ -70,11 +69,9 @@ bool Max31865RtdReader::periodicInitHandling() {
      return false;
    }
    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
-      ManualCsLockWrapper mg1(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
-      if (mg1.lockResult != returnvalue::OK or mg1.gpioResult != returnvalue::OK) {
-        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        continue;
-      }
+      // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
+      // or hardware specific issue where the CS needs to be pulled high and then low again
+      // between transfers
      result = writeCfgReg(rtd->spiCookie, BASE_CFG);
      if (result != returnvalue::OK) {
        handleSpiError(rtd, result, "writeCfgReg");
@ -115,7 +112,7 @@ bool Max31865RtdReader::periodicInitHandling() {
  return someRtdUsable;
 }

-ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
+ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
  using namespace MAX31865;
  // Now request one shot config for all active RTDs
  for (auto& rtd : rtds) {
@ -139,7 +136,7 @@ ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::periodicReadHandling() {
+ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
  using namespace MAX31865;
  auto result = returnvalue::OK;
  // Now read the RTD values
@ -153,11 +150,9 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
      return returnvalue::FAILED;
    }
    if (rtdIsActive(rtd->idx)) {
-      ManualCsLockWrapper mg1(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
-      if (mg1.lockResult != returnvalue::OK or mg1.gpioResult != returnvalue::OK) {
-        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        continue;
-      }
+      // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
+      // or hardware specific issue where the CS needs to be pulled high and then low again
+      // between transfers
      uint16_t rtdVal = 0;
      bool faultBitSet = false;
      result = writeCfgReg(rtd->spiCookie, BASE_CFG);
@ -166,6 +161,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
        continue;
      }
      result = readRtdVal(rtd->spiCookie, rtdVal, faultBitSet);
+      // sif::debug << "RTD Val: " << rtdVal << std::endl;
      if (result != returnvalue::OK) {
        handleSpiError(rtd, result, "readRtdVal");
        continue;
@ -191,7 +187,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
+ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) {
  if (cookie == nullptr) {
    throw std::invalid_argument("Invalid MAX31865 Reader Cookie");
  }
@ -211,7 +207,7 @@ ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                              size_t sendLen) {
  if (cookie == nullptr) {
    return returnvalue::FAILED;
@ -308,13 +304,13 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+ReturnValue_t Max31865RtdPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }

-ReturnValue_t Max31865RtdReader::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                      size_t* size) {
  MutexGuard mg(readerMutex);
  if (mg.getLockResult() != returnvalue::OK) {
@ -338,12 +334,12 @@ ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t**
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::writeCfgReg(SpiCookie* cookie, uint8_t cfg) {
+ReturnValue_t Max31865RtdPolling::writeCfgReg(SpiCookie* cookie, uint8_t cfg) {
  using namespace MAX31865;
  return writeNToReg(cookie, CONFIG, 1, &cfg, nullptr);
 }

-ReturnValue_t Max31865RtdReader::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
+ReturnValue_t Max31865RtdPolling::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
                                               uint8_t baseCfg) {
  using namespace MAX31865;
  if (bias == MAX31865::Bias::OFF) {
@ -354,7 +350,7 @@ ReturnValue_t Max31865RtdReader::writeBiasSel(MAX31865::Bias bias, SpiCookie* co
  return writeCfgReg(cookie, baseCfg);
 }

-ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) {
+ReturnValue_t Max31865RtdPolling::clearFaultStatus(SpiCookie* cookie) {
  using namespace MAX31865;
  // Read back the current configuration to avoid overwriting it when clearing te fault status
  uint8_t currentCfg = 0;
@ -368,7 +364,7 @@ ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) {
  return writeCfgReg(cookie, currentCfg);
 }

-ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
+ReturnValue_t Max31865RtdPolling::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
  using namespace MAX31865;
  uint8_t* replyPtr = nullptr;
  auto result = readNFromReg(cookie, CONFIG, 1, &replyPtr);
@ -378,19 +374,19 @@ ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
  return result;
 }

-ReturnValue_t Max31865RtdReader::writeLowThreshold(SpiCookie* cookie, uint16_t val) {
+ReturnValue_t Max31865RtdPolling::writeLowThreshold(SpiCookie* cookie, uint16_t val) {
  using namespace MAX31865;
  uint8_t cmd[2] = {static_cast<uint8_t>((val >> 8) & 0xff), static_cast<uint8_t>(val & 0xff)};
  return writeNToReg(cookie, LOW_THRESHOLD, 2, cmd, nullptr);
 }

-ReturnValue_t Max31865RtdReader::writeHighThreshold(SpiCookie* cookie, uint16_t val) {
+ReturnValue_t Max31865RtdPolling::writeHighThreshold(SpiCookie* cookie, uint16_t val) {
  using namespace MAX31865;
  uint8_t cmd[2] = {static_cast<uint8_t>((val >> 8) & 0xff), static_cast<uint8_t>(val & 0xff)};
  return writeNToReg(cookie, HIGH_THRESHOLD, 2, cmd, nullptr);
 }

-ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& lowThreshold) {
+ReturnValue_t Max31865RtdPolling::readLowThreshold(SpiCookie* cookie, uint16_t& lowThreshold) {
  using namespace MAX31865;
  uint8_t* replyPtr = nullptr;
  auto result = readNFromReg(cookie, LOW_THRESHOLD, 2, &replyPtr);
@ -400,7 +396,7 @@ ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& l
  return result;
 }

-ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t& highThreshold) {
+ReturnValue_t Max31865RtdPolling::readHighThreshold(SpiCookie* cookie, uint16_t& highThreshold) {
  using namespace MAX31865;
  uint8_t* replyPtr = nullptr;
  auto result = readNFromReg(cookie, HIGH_THRESHOLD, 2, &replyPtr);
@ -410,8 +406,8 @@ ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t&
  return result;
 }

-ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, size_t n, uint8_t* cmd,
-                                             uint8_t** reply) {
+ReturnValue_t Max31865RtdPolling::writeNToReg(SpiCookie* cookie, uint8_t reg, size_t n,
+                                              uint8_t* cmd, uint8_t** reply) {
  using namespace MAX31865;
  if (n > cmdBuf.size() - 1) {
    return returnvalue::FAILED;
@ -423,7 +419,7 @@ ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, siz
  return comIF->sendMessage(cookie, cmdBuf.data(), n + 1);
 }

-ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bool& faultBitSet) {
+ReturnValue_t Max31865RtdPolling::readRtdVal(SpiCookie* cookie, uint16_t& val, bool& faultBitSet) {
  using namespace MAX31865;
  uint8_t* replyPtr = nullptr;
  auto result = readNFromReg(cookie, RTD, 2, &replyPtr);
@ -438,7 +434,7 @@ ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bo
  return result;
 }

-ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
+ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
                                               uint8_t** reply) {
  using namespace MAX31865;
  if (n > 4) {
@ -465,7 +461,7 @@ ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, si
  return returnvalue::OK;
 }

-ReturnValue_t Max31865RtdReader::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
+ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
                                                 const char* ctx) {
  cookie->db.spiErrorCount.value += 1;
  sif::warning << "Max31865RtdReader::handleSpiError: " << ctx << " | Failed with result " << result
@ -473,7 +469,7 @@ ReturnValue_t Max31865RtdReader::handleSpiError(Max31865ReaderCookie* cookie, Re
  return result;
 }

-ReturnValue_t Max31865RtdReader::initialize() {
+ReturnValue_t Max31865RtdPolling::initialize() {
  csLock = comIF->getCsMutex();
  return SystemObject::initialize();
 }
--- a/linux/devices/Max31865RtdLowlevelHandler.h
+++ b/linux/devices/Max31865RtdLowlevelHandler.h
@ -35,11 +35,11 @@ struct Max31865ReaderCookie : public CookieIF {
  EiveMax31855::ReadOutStruct db;
 };

-class Max31865RtdReader : public SystemObject,
+class Max31865RtdPolling : public SystemObject,
                           public ExecutableObjectIF,
                           public DeviceCommunicationIF {
 public:
-  Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF);
+  Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF);

  ReturnValue_t performOperation(uint8_t operationCode) override;
  ReturnValue_t initialize() override;
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@ -21,6 +21,7 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater
      sensorTemperatures(this),
      susTemperatures(this),
      deviceTemperatures(this),
+      heaterInfo(this),
      imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()),
      max31865Set0(objects::RTD_0_IC3_PLOC_HEATSPREADER,
                   EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
@ -112,108 +113,88 @@ void ThermalController::performControlOperation() {
    deviceTemperatures.commit();
  }

+  std::array<HeaterHandler::SwitchState, 8> heaterStates;
+  heaterHandler.getAllSwitchStates(heaterStates);
+  {
+    PoolReadGuard pg(&heaterInfo);
+    std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8);
+    {
+      PoolReadGuard pg2(&currentVecPdu2);
+      if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) {
+        heaterInfo.heaterCurrent.value = currentVecPdu2.value[PDU2::Channels::TCS_HEATER_IN];
+      }
+    }
+  }
+
  // performThermalModuleCtrl();
 }

 ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                         LocalDataPoolManager& poolManager) {
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_HEATSPREADER,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_MISSIONBOARD,
-                           new PoolEntry<float>({1.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_4K_CAMERA,
-                           new PoolEntry<float>({2.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DAC_HEATSPREADER,
-                           new PoolEntry<float>({3.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_STARTRACKER,
-                           new PoolEntry<float>({4.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_RW1, new PoolEntry<float>({5.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DRO, new PoolEntry<float>({6.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_SCEX, new PoolEntry<float>({7.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_X8, new PoolEntry<float>({8.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_HPA, new PoolEntry<float>({9.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TX_MODUL,
-                           new PoolEntry<float>({10.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MPA, new PoolEntry<float>({11.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_ACU, new PoolEntry<float>({12.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLPCDU_HEATSPREADER,
-                           new PoolEntry<float>({13.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TCS_BOARD,
-                           new PoolEntry<float>({14.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MAGNETTORQUER,
-                           new PoolEntry<float>({15.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_HEATSPREADER, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_MISSIONBOARD, new PoolEntry<float>({1.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_4K_CAMERA, new PoolEntry<float>({2.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_DAC_HEATSPREADER, new PoolEntry<float>({3.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_STARTRACKER, new PoolEntry<float>({4.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_RW1, new PoolEntry<float>({5.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_DRO, new PoolEntry<float>({6.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_SCEX, new PoolEntry<float>({7.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_X8, new PoolEntry<float>({8.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_HPA, new PoolEntry<float>({9.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TX_MODUL, new PoolEntry<float>({10.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_MPA, new PoolEntry<float>({11.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_ACU, new PoolEntry<float>({12.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLPCDU_HEATSPREADER, new PoolEntry<float>({13.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TCS_BOARD, new PoolEntry<float>({14.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_MAGNETTORQUER, new PoolEntry<float>({15.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);

-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_0_N_LOC_XFYFZM_PT_XF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_6_R_LOC_XFYBZM_PT_XF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_1_N_LOC_XBYFZM_PT_XB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_7_R_LOC_XBYBZM_PT_XB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_2_N_LOC_XFYBZB_PT_YB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_8_R_LOC_XBYBZB_PT_YB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_3_N_LOC_XFYBZF_PT_YF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_9_R_LOC_XBYBZB_PT_YF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_0_N_LOC_XFYFZM_PT_XF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_6_R_LOC_XFYBZM_PT_XF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_1_N_LOC_XBYFZM_PT_XB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_7_R_LOC_XBYBZM_PT_XB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_2_N_LOC_XFYBZB_PT_YB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_8_R_LOC_XBYBZB_PT_YB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_3_N_LOC_XFYBZF_PT_YF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_9_R_LOC_XBYBZB_PT_YF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_4_N_LOC_XMYFZF_PT_ZF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_10_N_LOC_XMYBZF_PT_ZF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_5_N_LOC_XFYMZB_PT_ZB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_11_R_LOC_XBYMZB_PT_ZB, new PoolEntry<float>({0.0}));

-  localDataPoolMap.emplace(thermalControllerDefinitions::COMPONENT_RW, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::COMPONENT_RW, new PoolEntry<float>({0.0}));

-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_Q7S, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_1,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_2,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_3,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_4,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW1, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW2, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW3, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW4, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_STAR_TRACKER,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_POWER_AMPLIFIER,
-                           new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_BASEBAND_BOARD,
-                           new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGT, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ACU, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU1, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU2, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_0_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_1_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_2_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_3_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_0_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_2_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ADC_PAYLOAD_PCDU,
-                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_Q7S, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_1, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_2, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_3, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_4, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW1, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW2, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW3, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW4, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_STAR_TRACKER, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_POWER_AMPLIFIER, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_BASEBAND_BOARD, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGT, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_ACU, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU1, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU2, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_0_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_1_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_2_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_3_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_0_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_2_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates);
+  localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent);

  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), false, 10.0));
@ -221,17 +202,21 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo
      subdp::RegularHkPeriodicParams(susTemperatures.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), false, 10.0));
+  poolManager.subscribeForDiagPeriodicPacket(
+      subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), false, 10.0));
  return returnvalue::OK;
 }

 LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
-    case thermalControllerDefinitions::SENSOR_TEMPERATURES:
+    case tcsCtrl::SENSOR_TEMPERATURES:
      return &sensorTemperatures;
-    case thermalControllerDefinitions::SUS_TEMPERATURES:
+    case tcsCtrl::SUS_TEMPERATURES:
      return &susTemperatures;
-    case thermalControllerDefinitions::DEVICE_TEMPERATURES:
+    case tcsCtrl::DEVICE_TEMPERATURES:
      return &deviceTemperatures;
+    case tcsCtrl::HEATER_SET:
+      return &heaterInfo;
    default:
      return nullptr;
  }
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@ -12,6 +12,7 @@
 #include <list>

 #include "mission/devices/HeaterHandler.h"
+#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
 #include "mission/trace.h"

 /**
@ -76,9 +77,12 @@ class ThermalController : public ExtendedControllerBase {

  HeaterHandler& heaterHandler;

-  thermalControllerDefinitions::SensorTemperatures sensorTemperatures;
-  thermalControllerDefinitions::SusTemperatures susTemperatures;
-  thermalControllerDefinitions::DeviceTemperatures deviceTemperatures;
+  tcsCtrl::SensorTemperatures sensorTemperatures;
+  tcsCtrl::SusTemperatures susTemperatures;
+  tcsCtrl::DeviceTemperatures deviceTemperatures;
+  tcsCtrl::HeaterInfo heaterInfo;
+  lp_vec_t<int16_t, 9> currentVecPdu2 =
+      lp_vec_t<int16_t, 9>(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS));

  DeviceHandlerThermalSet imtqThermalSet;

@ -160,11 +164,13 @@ class ThermalController : public ExtendedControllerBase {
  std::array<std::pair<bool, double>, 5> sensors;
  uint8_t numSensors = 0;

-  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>(10.0);
+  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>({10.0});
+  PoolEntry<uint8_t> heaterSwitchStates = PoolEntry<uint8_t>(heater::NUMBER_OF_SWITCHES);
+  PoolEntry<int16_t> heaterCurrent = PoolEntry<int16_t>();

  static constexpr dur_millis_t MUTEX_TIMEOUT = 50;

--- a/mission/controller/controllerdefinitions/ThermalControllerDefinitions.h
+++ b/mission/controller/controllerdefinitions/ThermalControllerDefinitions.h
@ -4,13 +4,16 @@
 #include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>

-namespace thermalControllerDefinitions {
+#include "devices/heaterSwitcherList.h"

-enum SetIds : uint32_t {
-  SENSOR_TEMPERATURES,
-  DEVICE_TEMPERATURES,
-  SUS_TEMPERATURES,
-  COMPONENT_TEMPERATURES
+namespace tcsCtrl {
+
+enum SetId : uint32_t {
+  SENSOR_TEMPERATURES = 0,
+  DEVICE_TEMPERATURES = 1,
+  SUS_TEMPERATURES = 2,
+  COMPONENT_TEMPERATURES = 3,
+  HEATER_SET = 4,
 };

 enum PoolIds : lp_id_t {
@ -75,7 +78,10 @@ enum PoolIds : lp_id_t {
  TEMP_GYRO_3_SIDE_B,
  TEMP_MGM_0_SIDE_A,
  TEMP_MGM_2_SIDE_B,
-  TEMP_ADC_PAYLOAD_PCDU
+  TEMP_ADC_PAYLOAD_PCDU,
+
+  HEATER_SWITCH_LIST,
+  HEATER_CURRENT
 };

 static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25;
@ -202,6 +208,17 @@ class SusTemperatures : public StaticLocalDataSet<ENTRIES_SUS_TEMPERATURE_SET> {
      lp_var_t<float>(sid.objectId, PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, this);
 };

-}  // namespace thermalControllerDefinitions
+class HeaterInfo : public StaticLocalDataSet<3> {
+ public:
+  HeaterInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HEATER_SET) {}
+  HeaterInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HEATER_SET)) {}
+
+  lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES> heaterSwitchState =
+      lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES>(sid.objectId, PoolIds::HEATER_SWITCH_LIST,
+                                                    this);
+  lp_var_t<int16_t> heaterCurrent = lp_var_t<int16_t>(sid.objectId, PoolIds::HEATER_CURRENT, this);
+};
+
+}  // namespace tcsCtrl

 #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@ -20,6 +20,7 @@
 #include <fsfw/pus/Service8FunctionManagement.h>
 #include <fsfw/pus/Service9TimeManagement.h>
 #include <fsfw/storagemanager/PoolManager.h>
+#include <fsfw/subsystem/SubsystemBase.h>
 #include <fsfw/tcdistribution/CcsdsDistributor.h>
 #include <fsfw/tcdistribution/PusDistributor.h>
 #include <fsfw/timemanager/CdsShortTimeStamper.h>
@ -224,6 +225,7 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF&
  }});
  heaterHandler = new HeaterHandler(objects::HEATER_HANDLER, &gpioIF, helper, &pwrSwitcher,
                                    pcdu::Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V);
+  heaterHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
 }

 void ObjectFactory::createThermalController(HeaterHandler& heaterHandler) {
--- a/mission/devices/HeaterHandler.cpp
+++ b/mission/devices/HeaterHandler.cpp
@ -8,11 +8,13 @@
 #include <stdexcept>

 #include "devices/powerSwitcherList.h"
+#include "fsfw/subsystem/helper.h"

 HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
                             PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
    : SystemObject(setObjectId_),
      helper(helper),
+      modeHelper(this),
      gpioInterface(gpioInterface_),
      mainLineSwitcher(mainLineSwitcher_),
      mainLineSwitch(mainLineSwitch_),
@ -28,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
  if (mainLineSwitcher == nullptr) {
    throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
  }
-  heaterMutex = MutexFactory::instance()->createMutex();
-  if (heaterMutex == nullptr) {
+  heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
+  if (heaterHealthAndStateMutex == nullptr) {
    throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
  }
  auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@ -46,6 +48,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
      heater.first->performOperation(0);
    }
    handleSwitchHandling();
+    if (waitForSwitchOff) {
+      if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
+        waitForSwitchOff = false;
+        mode = MODE_OFF;
+        modeHelper.modeChanged(mode, submode);
+      }
+    }
  } catch (const std::out_of_range& e) {
    sif::warning << "HeaterHandler::performOperation: "
                    "Out of range error | "
@ -76,6 +85,10 @@ ReturnValue_t HeaterHandler::initialize() {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

+  result = modeHelper.initialize();
+  if (result != returnvalue::OK) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
  return returnvalue::OK;
 }

@ -95,11 +108,16 @@ void HeaterHandler::readCommandQueue() {
      break;
    } else if (result != returnvalue::OK) {
      sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl;
+      break;
    }
    result = actionHelper.handleActionMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }
+    result = modeHelper.handleModeCommand(&command);
+    if (result == returnvalue::OK) {
+      continue;
+    }
  } while (result == returnvalue::OK);
 }

@ -124,7 +142,11 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
  auto action = static_cast<SwitchAction>(data[1]);
  // Always accepts OFF commands
  if (action == SwitchAction::SET_SWITCH_ON) {
-    HasHealthIF::HealthState health = heater.healthDevice->getHealth();
+    HasHealthIF::HealthState health;
+    {
+      MutexGuard mg(heaterHealthAndStateMutex);
+      health = heater.healthDevice->getHealth();
+    }
    if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
        health == HasHealthIF::NEEDS_RECOVERY) {
      return HasHealthIF::OBJECT_NOT_HEALTHY;
@ -218,6 +240,9 @@ void HeaterHandler::handleSwitchHandling() {
 void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
  ReturnValue_t result = returnvalue::OK;
  auto& heater = heaterVec.at(heaterIdx);
+  if (waitForSwitchOff) {
+    waitForSwitchOff = false;
+  }
  /* Check if command waits for main switch being set on and whether the timeout has expired */
  if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) {
    // TODO - This requires the initiation of an FDIR procedure
@ -244,11 +269,16 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
        triggerEvent(GPIO_PULL_HIGH_FAILED, result);
      } else {
        triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
+        {
+          MutexGuard mg(heaterHealthAndStateMutex);
          heater.switchState = ON;
        }
+      }
    } else {
      triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
    }
+    mode = HasModesIF::MODE_ON;
+    modeHelper.modeChanged(mode, submode);
    // There is no need to send action finish replies if the sender was the
    // HeaterHandler itself
    if (heater.replyQueue != commandQueue->getId()) {
@ -289,15 +319,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
                 << " low" << std::endl;
      triggerEvent(GPIO_PULL_LOW_FAILED, result);
    } else {
-      result = heaterMutex->lockMutex();
+      {
+        MutexGuard mg(heaterHealthAndStateMutex);
        heater.switchState = OFF;
-      if (result == returnvalue::OK) {
-        heaterMutex->unlockMutex();
      }
      triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
      // When all switches are off, also main line switch will be turned off
      if (allSwitchesOff()) {
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+        waitForSwitchOff = true;
      }
    }
  } else {
@ -316,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
 }

 HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
  return heaterVec.at(switchNr).switchState;
 }

@ -329,9 +359,57 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switchers heater, SwitchState
  return returnvalue::FAILED;
 }

+void HeaterHandler::announceMode(bool recursive) {
+  triggerEvent(MODE_INFO, mode, submode);
+
+  std::array<SwitchState, 8> states;
+  getAllSwitchStates(states);
+  for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+    if (states[idx] == ON) {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0);
+    } else {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF,
+                                   0);
+    }
+  }
+}
+
+void HeaterHandler::getMode(Mode_t* mode, Submode_t* submode) {
+  if (!mode || !submode) {
+    return;
+  }
+  *mode = this->mode;
+  *submode = this->submode;
+}
+
+const HasHealthIF* HeaterHandler::getOptHealthIF() const { return nullptr; }
+
+const HasModesIF& HeaterHandler::getModeIF() const { return *this; }
+
+ReturnValue_t HeaterHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
+}
+
+ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; }
+
+object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); }
+
+ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
+  {
+    MutexGuard mg(heaterHealthAndStateMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      return returnvalue::FAILED;
+    }
+    for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+      statesBuf[idx] = heaterVec[idx].switchState;
+    }
+  }
+  return returnvalue::OK;
+}
+
 bool HeaterHandler::allSwitchesOff() {
  bool allSwitchesOrd = false;
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
  /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
  for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
    allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
@ -364,7 +442,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
 HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) {
  auto* healthDev = heaterVec.at(heater).healthDevice;
  if (healthDev != nullptr) {
-    MutexGuard mg(heaterMutex);
+    MutexGuard mg(heaterHealthAndStateMutex);
    return healthDev->getHealth();
  }
  return HasHealthIF::HealthState::FAULTY;
--- a/mission/devices/HeaterHandler.h
+++ b/mission/devices/HeaterHandler.h
@ -10,6 +10,8 @@
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/power/PowerSwitchIF.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw/subsystem/ModeTreeChildIF.h>
+#include <fsfw/subsystem/ModeTreeConnectionIF.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
 #include <fsfw/timemanager/Countdown.h>
 #include <fsfw_hal/common/gpio/GpioIF.h>
@ -40,6 +42,9 @@ struct HeaterHelper {
 */
 class HeaterHandler : public ExecutableObjectIF,
                      public PowerSwitchIF,
+                      public HasModesIF,
+                      public ModeTreeChildIF,
+                      public ModeTreeConnectionIF,
                      public SystemObject,
                      public HasActionsIF {
  friend class ThermalController;
@ -54,6 +59,7 @@ class HeaterHandler : public ExecutableObjectIF,
  static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5);

  enum CmdSourceParam : uint8_t { INTERNAL = 0, EXTERNAL = 1 };
+  enum SwitchState : uint8_t { ON = 1, OFF = 0 };

  /** Device command IDs */
  static const DeviceCommandId_t SWITCH_HEATER = 0x0;
@ -61,10 +67,12 @@ class HeaterHandler : public ExecutableObjectIF,
  HeaterHandler(object_id_t setObjectId, GpioIF* gpioInterface_, HeaterHelper helper,
                PowerSwitchIF* mainLineSwitcherObjectId, power::Switch_t mainLineSwitch);

+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
+  ReturnValue_t getAllSwitchStates(std::array<SwitchState, 8>& statesBuf);
+
  virtual ~HeaterHandler();

 protected:
-  enum SwitchState : bool { ON = true, OFF = false };
  enum SwitchAction : uint8_t { SET_SWITCH_OFF, SET_SWITCH_ON, NONE };

  ReturnValue_t switchHeater(heater::Switchers heater, SwitchState switchState);
@ -128,12 +136,14 @@ class HeaterHandler : public ExecutableObjectIF,

  HeaterMap heaterVec = {};

-  MutexIF* heaterMutex = nullptr;
+  MutexIF* heaterHealthAndStateMutex = nullptr;

  HeaterHelper helper;
+  ModeHelper modeHelper;

  /** Size of command queue */
  size_t cmdQueueSize = 20;
+  bool waitForSwitchOff = true;

  GpioIF* gpioInterface = nullptr;

@ -152,6 +162,9 @@ class HeaterHandler : public ExecutableObjectIF,

  StorageManagerIF* ipcStore = nullptr;

+  Mode_t mode = HasModesIF::MODE_OFF;
+  Submode_t submode = 0;
+
  void readCommandQueue();

  /**
@ -172,6 +185,16 @@ class HeaterHandler : public ExecutableObjectIF,
   */
  void setInitialSwitchStates();

+  // HasModesIF implementation
+  void announceMode(bool recursive) override;
+  void getMode(Mode_t* mode, Submode_t* submode) override;
+
+  // Mode Tree helper overrides
+  object_id_t getObjectId() const override;
+  const HasHealthIF* getOptHealthIF() const override;
+  const HasModesIF& getModeIF() const override;
+  ModeTreeChildIF& getModeTreeChildIF() override;
+
  void handleSwitchOnCommand(heater::Switchers heaterIdx);

  void handleSwitchOffCommand(heater::Switchers heaterIdx);
--- a/mission/tmtc/CfdpTmFunnel.cpp
+++ b/mission/tmtc/CfdpTmFunnel.cpp
@ -11,6 +11,7 @@ const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }

 ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
  TmTcMessage currentMessage;
+  unsigned int count = 0;
  ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
  while (status == returnvalue::OK) {
    status = handlePacket(currentMessage);
@ -18,6 +19,11 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
      sif::warning << "CfdpTmFunnel packet handling failed" << std::endl;
      break;
    }
+    count++;
+    if(count == 500) {
+      sif::error << "CfdpTmFunnel: Possible message storm detected" << std::endl;
+      break;
+    }
    status = tmQueue->receiveMessage(&currentMessage);
  }

--- a/mission/tmtc/PusTmFunnel.cpp
+++ b/mission/tmtc/PusTmFunnel.cpp
@ -58,15 +58,22 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
      sif::error << "PusTmFunnel::performOperation: Error handling TC request" << std::endl;
    }
  }
-  TmTcMessage tmMessage;
-  status = tmQueue->receiveMessage(&tmMessage);
+
+  TmTcMessage currentMessage;
+  unsigned int count = 0;
+  ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
  while (status == returnvalue::OK) {
    status = handleTmPacket(tmMessage);
    if (status != returnvalue::OK) {
      sif::warning << "TmFunnel packet handling failed" << std::endl;
      break;
    }
-    status = tmQueue->receiveMessage(&tmMessage);
+    count++;
+    if(count == 500) {
+      sif::error << "PusTmFunnel: Possible message storm detected" << std::endl;
+      break;
+    }
+    status = tmQueue->receiveMessage(&currentMessage);
  }

  if (status == MessageQueueIF::EMPTY) {
--- a/mission/tmtc/VirtualChannel.cpp
+++ b/mission/tmtc/VirtualChannel.cpp
@ -28,6 +28,7 @@ ReturnValue_t VirtualChannel::performOperation() {
  ReturnValue_t result = returnvalue::OK;
  TmTcMessage message;

+  unsigned int count = 0;
  while (tmQueue->receiveMessage(&message) == returnvalue::OK) {
    store_address_t storeId = message.getStorageId();
    const uint8_t* data = nullptr;
@ -43,12 +44,16 @@ ReturnValue_t VirtualChannel::performOperation() {
    if (linkIsUp) {
      result = ptme->writeToVc(vcId, data, size);
    }
-
    tmStore->deleteData(storeId);
-
    if (result != returnvalue::OK) {
      return result;
    }
+
+    count++;
+    if(count == 500) {
+      sif::error << "VirtualChannel: Possible message storm detected" << std::endl;
+      break;
+    }
  }
  return result;
 }
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 9686701c2eef9387952a9efb8f55298ae04dfa3f
+Subproject commit 4917ddbbe4abd03ca7c7bb4ca5c80970e9b3b6bf