From ec12ab5daaa391232e610567fbbbf849594e52e2 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 01:20:28 +0100
Subject: [PATCH] mode service fixes

---
 src/fsfw/modes/ModeMessage.cpp             |  2 +
 src/fsfw/modes/ModeMessage.h               |  1 +
 src/fsfw/pus/CService200ModeCommanding.cpp | 53 +++++++++++++---------
 3 files changed, 35 insertions(+), 21 deletions(-)

diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp
index 0d7eaeea..36cbb971 100644
--- a/src/fsfw/modes/ModeMessage.cpp
+++ b/src/fsfw/modes/ModeMessage.cpp
@@ -38,3 +38,5 @@ void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive
 void ModeMessage::setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) {
   setModeMessage(&message, CMD_MODE_COMMAND, mode, submode);
 }
+
+void ModeMessage::setModeReadMessage(CommandMessage& message) { message.setCommand(CMD_MODE_READ); }
diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h
index 89203f6e..4e75d156 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -46,6 +46,7 @@ class ModeMessage {
 
   static void setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
   static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
+  static void setModeReadMessage(CommandMessage& message);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);
 };
diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index f5928be7..814c1c92 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -54,27 +54,36 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
-  ReturnValue_t result = returnvalue::OK;
-  if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or
-      subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) {
-    bool recursive = true;
-    if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
-      recursive = false;
-    }
-    ModeMessage::setModeAnnounceMessage(*message, recursive);
-  } else {
-    ModePacket modeCommandPacket;
-    ReturnValue_t result =
-        modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-    if (result != returnvalue::OK) {
-      return result;
-    }
+  switch (subservice) {
+    case (Subservice::COMMAND_MODE_COMMAND): {
+      ModePacket modeCommandPacket;
+      ReturnValue_t result =
+          modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+      if (result != returnvalue::OK) {
+        return result;
+      }
 
-    ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
-                                modeCommandPacket.getSubmode());
+      ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND,
+                                  modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
+      return returnvalue::OK;
+    }
+    case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): {
+      bool recursive = true;
+      if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
+        recursive = false;
+      }
+      ModeMessage::setModeAnnounceMessage(*message, recursive);
+      return EXECUTION_COMPLETE;
+    }
+    case (Subservice::COMMAND_MODE_READ): {
+      ModeMessage::setModeReadMessage(*message);
+      return returnvalue::OK;
+    }
+    default: {
+      return CommandingServiceBase::INVALID_SUBSERVICE;
+    }
   }
-
-  return result;
 }
 
 ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@@ -85,8 +94,10 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply
   ReturnValue_t result = returnvalue::FAILED;
   switch (replyId) {
     case (ModeMessage::REPLY_MODE_REPLY): {
-      result = prepareModeReply(reply, objectId);
-      break;
+      if (previousCommand != ModeMessage::CMD_MODE_COMMAND) {
+        return prepareModeReply(reply, objectId);
+      }
+      return returnvalue::OK;
     }
     case (ModeMessage::REPLY_WRONG_MODE_REPLY): {
       result = prepareWrongModeReply(reply, objectId);