diff --git a/CHANGELOG.md b/CHANGELOG.md
index dbe08848..d446bd49 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -27,6 +27,12 @@ will consitute of a breaking change warranting a new major release:
 - Internal error reporter set is now enabled by default and generated every 120 seconds.
 - Persistent TM store dumps are now performed in chronological order.
 
+## Fixed
+
+- Various robustness improvements for the heater handler. The heater handler will now only
+  process the command queue if it is not busy with switch commanding which reduced the amount
+  of possible bugs.
+
 # [v5.0.0] 2023-06-26
 
 v3.3.1 and all following version will now be moved to v5.0.0 with the additional changes listed
diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp
index da14f58c..d9329fc0 100644
--- a/bsp_q7s/fmObjectFactory.cpp
+++ b/bsp_q7s/fmObjectFactory.cpp
@@ -33,8 +33,8 @@ void ObjectFactory::produce(void* args) {
 
   PersistentTmStores stores;
   ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
-                                       *SdCardManager::instance(), &ipcStore, &tmStore, stores,
-                                       200, true);
+                                       *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200,
+                                       true);
 
   LinuxLibgpioIF* gpioComIF = nullptr;
   SerialComIF* uartComIF = nullptr;
diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp
index 14a005aa..c58416f7 100644
--- a/dummies/TemperatureSensorInserter.cpp
+++ b/dummies/TemperatureSensorInserter.cpp
@@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId,
       tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {}
 
 ReturnValue_t TemperatureSensorInserter::initialize() {
-  testCase = TestCase::NONE;
+  testCase = TestCase::COLD_PLOC_CONSECUTIVE;
   return returnvalue::OK;
 }
 
@@ -96,6 +96,25 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) {
       }
       break;
     }
+    case (TestCase::COLD_PLOC_CONSECUTIVE): {
+      if (cycles == 15) {
+        sif::debug << "Setting cold PLOC temperature" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true);
+      }
+      if (cycles == 30) {
+        sif::debug << "Setting warmer PLOC temperature" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true);
+      }
+      if (cycles == 45) {
+        sif::debug << "Setting cold PLOC temperature again" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true);
+      }
+      if (cycles == 60) {
+        sif::debug << "Setting warmer PLOC temperature again" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true);
+      }
+      break;
+    }
     case (TestCase::COLD_CAMERA): {
       if (cycles == 15) {
         sif::debug << "Setting cold CAM temperature" << std::endl;
diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h
index eb6cc1ba..009f5b7d 100644
--- a/dummies/TemperatureSensorInserter.h
+++ b/dummies/TemperatureSensorInserter.h
@@ -32,6 +32,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject
     COLD_STR = 4,
     COLD_STR_CONSECUTIVE = 5,
     COLD_CAMERA = 6,
+    COLD_PLOC_CONSECUTIVE = 7,
   };
   int iteration = 0;
   uint32_t cycles = 0;
diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp
index de4b600d..9f7ba43c 100644
--- a/mission/tcs/HeaterHandler.cpp
+++ b/mission/tcs/HeaterHandler.cpp
@@ -51,9 +51,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
       if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
         waitForSwitchOff = false;
         mode = MODE_OFF;
+        busyWithSwitchCommanding = false;
         modeHelper.modeChanged(mode, submode);
       }
     }
+    if (busyWithSwitchCommanding and heaterCmdBusyCd.hasTimedOut()) {
+      busyWithSwitchCommanding = false;
+    }
   } catch (const std::out_of_range& e) {
     sif::warning << "HeaterHandler::performOperation: "
                     "Out of range error | "
@@ -102,20 +106,22 @@ void HeaterHandler::readCommandQueue() {
   ReturnValue_t result = returnvalue::OK;
   CommandMessage command;
   do {
-    result = commandQueue->receiveMessage(&command);
-    if (result == MessageQueueIF::EMPTY) {
-      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;
+    if (not busyWithSwitchCommanding) {
+      result = commandQueue->receiveMessage(&command);
+      if (result == MessageQueueIF::EMPTY) {
+        break;
+      } else if (result != returnvalue::OK) {
+        sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl;
+        break;
+      }
+      result = modeHelper.handleModeCommand(&command);
+      if (result == returnvalue::OK) {
+        continue;
+      }
+      result = actionHelper.handleActionMessage(&command);
+      if (result == returnvalue::OK) {
+        continue;
+      }
     }
   } while (result == returnvalue::OK);
 }
@@ -167,6 +173,8 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
   heater.action = action;
   heater.cmdActive = true;
   heater.replyQueue = commandedBy;
+  busyWithSwitchCommanding = true;
+  heaterCmdBusyCd.resetTimer();
   return returnvalue::OK;
 }
 
@@ -249,6 +257,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
     sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
                << std::endl;
     heater.cmdActive = false;
+    busyWithSwitchCommanding = false;
     heater.waitMainSwitchOn = false;
     if (heater.replyQueue != commandQueue->getId()) {
       actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT);
@@ -259,27 +268,30 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
   // Check state of main line switch
   ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
   if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
-    if (getSwitchState(heaterIdx) == SwitchState::OFF) {
-      gpioId_t gpioId = heater.gpioId;
-      result = gpioInterface->pullHigh(gpioId);
-      if (result != returnvalue::OK) {
-        sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId
-                   << " high" << std::endl;
-        triggerEvent(GPIO_PULL_HIGH_FAILED, result);
-      } else {
+    gpioId_t gpioId = heater.gpioId;
+    result = gpioInterface->pullHigh(gpioId);
+    if (result != returnvalue::OK) {
+      sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull GPIO with ID " << gpioId
+                 << " high" << std::endl;
+      triggerEvent(GPIO_PULL_HIGH_FAILED, result);
+    }
+    if (result == returnvalue::OK) {
+      if (getSwitchState(heaterIdx) == SwitchState::OFF) {
         triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
-        EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
-                                     MODE_ON, 0);
         {
           MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
           heater.switchState = ON;
         }
+
+      } else {
+        triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
       }
-    } else {
-      triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
+      EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
+                                   MODE_ON, 0);
+      busyWithSwitchCommanding = false;
+      mode = HasModesIF::MODE_ON;
+      modeHelper.modeChanged(mode, submode);
     }
-    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()) {
@@ -312,30 +324,33 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
 void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) {
   ReturnValue_t result = returnvalue::OK;
   auto& heater = heaterVec.at(heaterIdx);
-  // Check whether switch is already off
-  if (getSwitchState(heaterIdx)) {
-    gpioId_t gpioId = heater.gpioId;
-    result = gpioInterface->pullLow(gpioId);
-    if (result != returnvalue::OK) {
-      sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
-                 << " low" << std::endl;
-      triggerEvent(GPIO_PULL_LOW_FAILED, result);
-    } else {
+  gpioId_t gpioId = heater.gpioId;
+  result = gpioInterface->pullLow(gpioId);
+  if (result != returnvalue::OK) {
+    sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
+               << " low" << std::endl;
+    triggerEvent(GPIO_PULL_LOW_FAILED, result);
+  }
+  if (result == returnvalue::OK) {
+    // Check whether switch is already off
+    if (getSwitchState(heaterIdx) == SwitchState::ON) {
       {
         MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
         heater.switchState = OFF;
       }
       triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
-      EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
-                                   MODE_OFF, 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 {
+      triggerEvent(SWITCH_ALREADY_OFF, heaterIdx);
+    }
+    EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
+                                 MODE_OFF, 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 {
+      busyWithSwitchCommanding = false;
     }
-  } else {
-    triggerEvent(SWITCH_ALREADY_OFF, heaterIdx);
   }
   if (heater.replyQueue != NO_COMMANDER) {
     // Report back switch command reply if necessary
diff --git a/mission/tcs/HeaterHandler.h b/mission/tcs/HeaterHandler.h
index 609ac725..0198ab04 100644
--- a/mission/tcs/HeaterHandler.h
+++ b/mission/tcs/HeaterHandler.h
@@ -148,6 +148,7 @@ class HeaterHandler : public ExecutableObjectIF,
   /** Size of command queue */
   size_t cmdQueueSize = 20;
   bool waitForSwitchOff = true;
+  bool busyWithSwitchCommanding = false;
 
   GpioIF* gpioInterface = nullptr;
 
@@ -163,6 +164,7 @@ class HeaterHandler : public ExecutableObjectIF,
   power::Switch_t mainLineSwitch;
 
   ActionHelper actionHelper;
+  Countdown heaterCmdBusyCd = Countdown(2000);
 
   StorageManagerIF* ipcStore = nullptr;