From 8014e4adf90bfcece6267294436af804d657a094 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 1/2] mode service fixes

---
 src/fsfw/modes/ModeMessage.cpp             | 16 ++++++++
 src/fsfw/modes/ModeMessage.h               |  3 ++
 src/fsfw/pus/CService200ModeCommanding.cpp | 44 ++++++++++++++++------
 3 files changed, 52 insertions(+), 11 deletions(-)

diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp
index ecc52c94..fbfb71aa 100644
--- a/src/fsfw/modes/ModeMessage.cpp
+++ b/src/fsfw/modes/ModeMessage.cpp
@@ -24,3 +24,19 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason
   message->setParameter(reason);
   message->setParameter2(0);
 }
+
+void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) {
+  Command_t cmd;
+  if (recursive) {
+    cmd = CMD_MODE_ANNOUNCE_RECURSIVELY;
+  } else {
+    cmd = CMD_MODE_ANNOUNCE;
+  }
+  message.setCommand(cmd);
+}
+
+void ModeMessage::setCmdModeMessage(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 84429e84..c00e6c9e 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -45,6 +45,9 @@ class ModeMessage {
 
   static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
                              Submode_t submode);
+  static void setCmdModeMessage(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 d28df59b..a82e7f5c 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -53,16 +53,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) {
-  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());
-  return result;
+      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;
+    }
+  }
 }
 
 ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@@ -73,8 +93,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);

From 6445debfa182475baefa77ce95c8489f3e0cd7f5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 01:31:32 +0100
Subject: [PATCH 2/2] bump changelog

---
 CHANGELOG.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 969af78e..18f3dedb 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
+  on mode announcements, duplicate mode reply generated on announce commands, and the mode read
+  subservice not working properly.
 - DHB `setNormalDatapoolEntriesInvalid`: The default implementation did not set the validity
   to false correctly because the `read` and `write` calls were missing.
 - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead