diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 69baf54f..f6e8578b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/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); @@ -1567,3 +1570,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; + } + } +} diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 34171067..58e54da0 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/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_ */