src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -572,6 +572,9 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
   mode = newMode;
   modeChanged();
   setNormalDatapoolEntriesInvalid();
+  if (newMode == MODE_OFF) {
+    disableCommandsAndReplies();
+  }
   if (!isTransitionalMode()) {
     modeHelper.modeChanged(newMode, newSubmode);
     announceMode(false);
@@ -1482,6 +1485,9 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
 
 LocalPoolDataSetBase* DeviceHandlerBase::getDataSetHandle(sid_t sid) {
   auto iter = deviceReplyMap.find(sid.ownerSetId);
+  if (sid.objectId == 0x44140014) {
+    sif::debug << "HK message for IMTQ" << std::endl;
+  }
   if (iter != deviceReplyMap.end()) {
     return iter->second.dataSet;
   } else {
@@ -1567,3 +1573,22 @@ void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; }
 void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) {
   this->powerSwitcher = switcher;
 }
+
+void DeviceHandlerBase::disableCommandsAndReplies() {
+  for (auto& command : deviceCommandMap) {
+    if (command.second.isExecuting) {
+      command.second.isExecuting = false;
+    }
+  }
+  for (auto& reply : deviceReplyMap) {
+    if (!reply.second.periodic) {
+      if (reply.second.countdown != nullptr) {
+        reply.second.countdown->timeOut();
+      }
+      else {
+        reply.second.delayCycles = 0;
+      }
+      reply.second.active = false;
+    }
+  }
+}

src/fsfw/devicehandlers/DeviceHandlerBase.h

@@ -1325,6 +1325,11 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   void printWarningOrError(sif::OutputTypes errorType, const char *functionName,
                            ReturnValue_t errorCode = HasReturnvaluesIF::RETURN_FAILED,
                            const char *errorPrint = nullptr);
+
+  /**
+   * @brief Disables all commands and replies when device is set to MODE_OFF
+   */
+  void disableCommandsAndReplies();
 };
 
 #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ */