diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 525a3dcc..c4ce7781 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1320,18 +1320,22 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue return result; } DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); + MessageQueueId_t prevRecipient = MessageQueueIF::NO_QUEUE; if (iter == deviceCommandMap.end()) { result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { result = COMMAND_ALREADY_SENT; } else { + prevRecipient = iter->second.sendReplyTo; + iter->second.sendReplyTo = commandedBy; result = buildCommandFromCommand(actionId, data, size); } if (result == returnvalue::OK) { - iter->second.sendReplyTo = commandedBy; iter->second.isExecuting = true; cookieInfo.pendingCommand = iter; cookieInfo.state = COOKIE_WRITE_READY; + } else { + iter->second.sendReplyTo = prevRecipient; } return result; }