diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index b14a46e9..f73f027e 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1294,19 +1294,24 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, return result; } DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); + MessageQueueId_t previousReplyRecipient = iter->second.sendReplyTo; if (iter == deviceCommandMap.end()) { result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { result = COMMAND_ALREADY_SENT; } else { + // Set this so it can be used to finish a command immediately + iter->second.sendReplyTo = commandedBy; result = buildCommandFromCommand(actionId, data, size); } if (result == RETURN_OK) { - iter->second.sendReplyTo = commandedBy; iter->second.isExecuting = true; cookieInfo.pendingCommand = iter; cookieInfo.state = COOKIE_WRITE_READY; } + else { + iter->second.sendReplyTo = previousReplyRecipient; + } return result; } diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index b068c04f..d028f9f7 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -285,10 +285,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EBADF"); #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "mq_send to: " << sendTo << " sent from " - << sentFrom << "failed" << std::endl; + sif::warning << "mq_send to " << sendTo << " sent from " + << sentFrom << " failed" << std::endl; #else - sif::printWarning("mq_send to: %d sent from %d failed\n", sendTo, sentFrom); + sif::printWarning("mq_send to %d sent from %d failed\n", sendTo, sentFrom); #endif return DESTINATION_INVALID; }