diff --git a/CHANGELOG.md b/CHANGELOG.md
index 41b24103..0f01acdc 100644
--- 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
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 650e8af1..8e1aa4eb 100644
--- 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)
diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp
index 1686eb03..e3162a07 100644
--- 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);
diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt
index 1b08ad84..17d842ea 100644
--- 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)
 
diff --git a/linux/devices/Max31865RtdLowlevelHandler.cpp b/linux/devices/Max31865RtdPolling.cpp
similarity index 81%
rename from linux/devices/Max31865RtdLowlevelHandler.cpp
rename to linux/devices/Max31865RtdPolling.cpp
index 0af22bfa..e59c2ef2 100644
--- a/linux/devices/Max31865RtdLowlevelHandler.cpp
+++ b/linux/devices/Max31865RtdPolling.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,8 +207,8 @@ ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* sendData,
-                                             size_t sendLen) {
+ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                              size_t sendLen) {
   if (cookie == nullptr) {
     return returnvalue::FAILED;
   }
@@ -308,14 +304,14 @@ 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,
-                                                     size_t* size) {
+ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                      size_t* size) {
   MutexGuard mg(readerMutex);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
@@ -338,13 +334,13 @@ 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,
-                                              uint8_t baseCfg) {
+ReturnValue_t Max31865RtdPolling::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
+                                               uint8_t baseCfg) {
   using namespace MAX31865;
   if (bias == MAX31865::Bias::OFF) {
     baseCfg &= ~(1 << CfgBitPos::BIAS_SEL);
@@ -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,8 +434,8 @@ ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bo
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
-                                              uint8_t** reply) {
+ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
+                                               uint8_t** reply) {
   using namespace MAX31865;
   if (n > 4) {
     return returnvalue::FAILED;
@@ -465,15 +461,15 @@ ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, si
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
-                                                const char* ctx) {
+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
                << std::endl;
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::initialize() {
+ReturnValue_t Max31865RtdPolling::initialize() {
   csLock = comIF->getCsMutex();
   return SystemObject::initialize();
 }
diff --git a/linux/devices/Max31865RtdLowlevelHandler.h b/linux/devices/Max31865RtdPolling.h
similarity index 92%
rename from linux/devices/Max31865RtdLowlevelHandler.h
rename to linux/devices/Max31865RtdPolling.h
index 4854ef3b..a34c8e53 100644
--- a/linux/devices/Max31865RtdLowlevelHandler.h
+++ b/linux/devices/Max31865RtdPolling.h
@@ -35,11 +35,11 @@ struct Max31865ReaderCookie : public CookieIF {
   EiveMax31855::ReadOutStruct db;
 };
 
-class Max31865RtdReader : public SystemObject,
-                          public ExecutableObjectIF,
-                          public DeviceCommunicationIF {
+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;
diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp
index 7dcbddc7..98f65427 100644
--- 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;
   }
diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h
index 6297b175..a161b0eb 100644
--- 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;
 
diff --git a/mission/controller/controllerdefinitions/ThermalControllerDefinitions.h b/mission/controller/controllerdefinitions/ThermalControllerDefinitions.h
index 0c16d3ea..e7c95218 100644
--- 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_ */
diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp
index dcf19a7b..fea6e74a 100644
--- 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) {
diff --git a/mission/devices/HeaterHandler.cpp b/mission/devices/HeaterHandler.cpp
index 9600c0db..2cde7f1e 100644
--- 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);
-        heater.switchState = ON;
+        {
+          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();
-      heater.switchState = OFF;
-      if (result == returnvalue::OK) {
-        heaterMutex->unlockMutex();
+      {
+        MutexGuard mg(heaterHealthAndStateMutex);
+        heater.switchState = OFF;
       }
       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;
diff --git a/mission/devices/HeaterHandler.h b/mission/devices/HeaterHandler.h
index ac4f94f4..ffc38440 100644
--- 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);
diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp
index 3bcf2cec..3955aa84 100644
--- 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);
   }
 
diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp
index 87aa8840..b88032d7 100644
--- 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) {
diff --git a/mission/tmtc/VirtualChannel.cpp b/mission/tmtc/VirtualChannel.cpp
index b0f9391d..64c7b006 100644
--- 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;
 }
diff --git a/tmtc b/tmtc
index 9686701c..4917ddbb 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 9686701c2eef9387952a9efb8f55298ae04dfa3f
+Subproject commit 4917ddbbe4abd03ca7c7bb4ca5c80970e9b3b6bf