From abaa8f781ca56301706050d9703678d5c72a0921 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 31 Aug 2022 16:26:18 +0200
Subject: [PATCH] small fix which allows sending action reply immediately

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

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;
 }