diff --git a/mission/devices/HeaterHandler.cpp b/mission/devices/HeaterHandler.cpp
index 06beb0bc..9bb8e6d8 100644
--- a/mission/devices/HeaterHandler.cpp
+++ b/mission/devices/HeaterHandler.cpp
@@ -52,6 +52,7 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
       if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
         waitForSwitchOff = false;
         mode = MODE_OFF;
+        modeHelper.modeChanged(mode, submode);
       }
     }
   } catch (const std::out_of_range& e) {
@@ -112,6 +113,10 @@ void HeaterHandler::readCommandQueue() {
     if (result == returnvalue::OK) {
       continue;
     }
+    result = modeHelper.handleModeCommand(&command);
+    if(result == returnvalue::OK) {
+      continue;
+    }
   } while (result == returnvalue::OK);
 }
 
@@ -250,7 +255,6 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
   // Check state of main line switch
   ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
   if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
-    mode = HasModesIF::MODE_ON;
     if (checkSwitchState(heaterIdx) == SwitchState::OFF) {
       gpioId_t gpioId = heater.gpioId;
       result = gpioInterface->pullHigh(gpioId);
@@ -265,6 +269,8 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
     } 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()) {