From dcbf1502a2d972ed1558be3d4c7bd52a3da1e464 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 15:08:56 +0100
Subject: [PATCH] boot sequence EIVE sys

---
 bsp_q7s/obsw.cpp               | 20 ++++++++++++++++++--
 bsp_q7s/obsw.h                 |  1 +
 mission/system/tree/system.cpp |  2 +-
 3 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/bsp_q7s/obsw.cpp b/bsp_q7s/obsw.cpp
index e264283f..d355a10f 100644
--- a/bsp_q7s/obsw.cpp
+++ b/bsp_q7s/obsw.cpp
@@ -15,6 +15,7 @@
 #include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/version.h"
 #include "mission/acsDefs.h"
+#include "mission/comDefs.h"
 #include "mission/system/tree/system.h"
 #include "q7sConfig.h"
 #include "watchdog/definitions.h"
@@ -116,7 +117,22 @@ void obsw::commandEiveSystemToSafe() {
   ReturnValue_t result =
       MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
   if (result != returnvalue::OK) {
-    sif::error << "Sending safe mode command to EIVE system failed" << std::endl;
+    sif::error << "obsw: Sending safe mode command to EIVE system failed" << std::endl;
+  }
+}
+
+void obsw::commandComSubsystemRxOnly() {
+  auto* comSs = ObjectManager::instance()->get<HasModesIF>(objects::COM_SUBSYSTEM);
+  if (comSs == nullptr) {
+    sif::error << "obsw: Could not retrieve COM subsystem object" << std::endl;
+    return;
+  }
+  CommandMessage msg;
+  ModeMessage::setCmdModeMessage(msg, com::RX_ONLY, 0);
+  ReturnValue_t result = MessageQueueSenderIF::sendMessage(comSs->getCommandQueue(), &msg,
+                                                           MessageQueueIF::NO_QUEUE, false);
+  if (result != returnvalue::OK) {
+    sif::error << "obsw: Sending RX_ONLY mode command to COM subsystem failed" << std::endl;
   }
 }
 
@@ -127,6 +143,6 @@ void obsw::announceAllModes() {
   ReturnValue_t result =
       MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
   if (result != returnvalue::OK) {
-    sif::error << "Sending safe mode command to EIVE system failed" << std::endl;
+    sif::error << "obsw: Sending safe mode command to EIVE system failed" << std::endl;
   }
 }
diff --git a/bsp_q7s/obsw.h b/bsp_q7s/obsw.h
index 1a6e4e05..8260a605 100644
--- a/bsp_q7s/obsw.h
+++ b/bsp_q7s/obsw.h
@@ -7,6 +7,7 @@ int obsw(int argc, char* argv[]);
 
 void bootDelayHandling();
 void commandEiveSystemToSafe();
+void commandComSubsystemRxOnly();
 void announceAllModes();
 
 };  // namespace obsw
diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp
index 30f76609..a242e8cd 100644
--- a/mission/system/tree/system.cpp
+++ b/mission/system/tree/system.cpp
@@ -98,7 +98,6 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
 
   // Build SAFE transition 0.
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
-  iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second);
   iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true);
   check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
@@ -192,4 +191,5 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
   check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_BOOT.first, &EIVE_SEQUENCE_BOOT.second,
                                      EIVE_SEQUENCE_SAFE.first)),
         ctxc);
+}
 }  // namespace