From 6c1db8473bdd7639065e230341d52007c58ecf49 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meier-jakob@gmx.de>
Date: Thu, 31 Mar 2022 14:36:45 +0200
Subject: [PATCH] get alternative reply from device command info

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 31 ++++++++++++++-----
 src/fsfw/devicehandlers/DeviceHandlerBase.h   |  6 +++-
 2 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 08a15d5ff..0e2802ac1 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -410,7 +410,7 @@ ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap(
     DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase* replyDataSet,
     size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId) {
   // No need to check, as we may try to insert multiple times.
-  insertInCommandMap(deviceCommand);
+  insertInCommandMap(deviceCommand, hasDifferentReplyId, replyId);
   if (hasDifferentReplyId) {
     return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic);
   } else {
@@ -437,11 +437,15 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId,
   }
 }
 
-ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand) {
+ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand,
+                                                    bool useAlternativeReply,
+                                                    DeviceCommandId_t alternativeReplyId) {
   DeviceCommandInfo info;
   info.expectedReplies = 0;
   info.isExecuting = false;
   info.sendReplyTo = NO_COMMANDER;
+  info.useAlternativeReplyId = alternativeReplyId;
+  info.alternativeReplyId = alternativeReplyId;
   auto resultPair = deviceCommandMap.emplace(deviceCommand, info);
   if (resultPair.second) {
     return RETURN_OK;
@@ -451,12 +455,21 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm
 }
 
 size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) {
-  DeviceReplyIter iter = deviceReplyMap.find(commandId);
-  if (iter != deviceReplyMap.end()) {
-    return iter->second.replyLen;
-  } else {
-    return 0;
+  DeviceCommandId_t replyId = NO_COMMAND_ID;
+  DeviceCommandMap::iterator command = cookieInfo.pendingCommand;
+  if (command->second.useAlternativeReplyId) {
+	replyId = command->second.alternativeReplyId;
   }
+  else {
+	replyId = commandId;
+  }
+  DeviceReplyIter iter = deviceReplyMap.find(replyId);
+  if (iter != deviceReplyMap.end()) {
+	if (iter->second.delayCycles != 0) {
+		return iter->second.replyLen;
+	}
+  }
+  return 0;
 }
 
 ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply,
@@ -651,7 +664,9 @@ void DeviceHandlerBase::doGetWrite() {
 
     // We need to distinguish here, because a raw command never expects a reply.
     //(Could be done in eRIRM, but then child implementations need to be careful.
-    result = enableReplyInReplyMap(cookieInfo.pendingCommand);
+    DeviceCommandMap::iterator command = cookieInfo.pendingCommand;
+    result = enableReplyInReplyMap(command, 1, command->second.useAlternativeReplyId,
+                                   command->second.alternativeReplyId);
   } else {
     // always generate a failure event, so that FDIR knows what's up
     triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, cookieInfo.pendingCommand->first);
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index c8b8b86d7..5e974831e 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@@ -478,7 +478,9 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * @return - @c RETURN_OK when the command was successfully inserted,
    *         - @c RETURN_FAILED else.
    */
-  ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand);
+  ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand,
+                                   bool useAlternativeReply = false,
+                                   DeviceCommandId_t alternativeReplyId = 0);
 
   /**
    * Enables a periodic reply for a given command. It sets to delay cycles to the specified
@@ -751,6 +753,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
     //! if this is != NO_COMMANDER, DHB was commanded externally and shall
     //! report everything to commander.
     MessageQueueId_t sendReplyTo;
+    bool useAlternativeReplyId;
+    DeviceCommandId_t alternativeReplyId;
   };
   using DeviceCommandMap = std::map<DeviceCommandId_t, DeviceCommandInfo>;
   /**