From d857e3866ad36809f9fb599590b3c6d419874908 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 10 Oct 2022 10:44:45 +0200
Subject: [PATCH] add command to report ACS modes

---
 tmtc/acs_ctrl.py      |  2 +-
 tmtc/acs_subsystem.py | 15 +++++++++++++++
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/tmtc/acs_ctrl.py b/tmtc/acs_ctrl.py
index 71cc1ba..bf8b7fd 100644
--- a/tmtc/acs_ctrl.py
+++ b/tmtc/acs_ctrl.py
@@ -2,7 +2,7 @@ import enum
 import socket
 import struct
 from socket import AF_INET
-from typing import Tuple, Optional
+from typing import Tuple
 
 from config.definitions import CustomServiceList
 from config.object_ids import ACS_CONTROLLER
diff --git a/tmtc/acs_subsystem.py b/tmtc/acs_subsystem.py
index 8f1e201..80bc85a 100644
--- a/tmtc/acs_subsystem.py
+++ b/tmtc/acs_subsystem.py
@@ -1,6 +1,7 @@
 import enum
 from typing import Tuple, Dict
 
+from spacepackets.ecss import PusTelecommand
 from .common import pack_mode_cmd_with_info
 from config.object_ids import ACS_SUBSYSTEM_ID
 from config.definitions import CustomServiceList
@@ -9,6 +10,7 @@ from tmtccmd.config.tmtc import (
     TmtcDefinitionWrapper,
     OpCodeEntry,
 )
+from tmtccmd.tc.pus_200_fsfw_modes import Subservices as ModeSubservices
 from tmtccmd.tc import service_provider
 from tmtccmd.tc.decorator import ServiceProviderParams
 
@@ -19,6 +21,7 @@ class OpCodes(str, enum.Enum):
     DETUMBLE = "detumble"
     IDLE = "idle"
     TARGET_PT = "target"
+    REPORT_ALL_MODES = "all_modes"
 
 
 class AcsModes(enum.IntEnum):
@@ -27,6 +30,7 @@ class AcsModes(enum.IntEnum):
     DETUMBLE = 2
     IDLE = 3
     TARGET_PT = 4
+    REPORT_ALL_MODES = 5
 
 
 class Info(str, enum.Enum):
@@ -35,6 +39,7 @@ class Info(str, enum.Enum):
     DETUMBLE = "Detumble Mode Command"
     IDLE = "Idle Mode Command"
     TARGET_PT = "Target Pointing Mode Command"
+    REPORT_ALL_MODES = "Report All Modes Recursively"
 
 
 HANDLER_LIST: Dict[str, Tuple[int, str]] = {
@@ -50,6 +55,15 @@ def build_acs_subsystem_cmd(p: ServiceProviderParams):
     op_code = p.op_code
     q = p.queue_helper
     info_prefix = "ACS Subsystem"
+    if op_code in OpCodes.REPORT_ALL_MODES:
+        q.add_log_cmd(f"{info_prefix}: {Info.REPORT_ALL_MODES}")
+        q.add_pus_tc(
+            PusTelecommand(
+                service=200,
+                subservice=ModeSubservices.TC_MODE_ANNOUNCE_RECURSIVE,
+                app_data=ACS_SUBSYSTEM_ID,
+            )
+        )
     mode_info_tup = HANDLER_LIST[op_code]
     if mode_info_tup is None:
         return
@@ -68,4 +82,5 @@ def add_acs_subsystem_cmds(defs: TmtcDefinitionWrapper):
     oce.add(OpCodes.OFF, Info.OFF)
     oce.add(OpCodes.SAFE, Info.SAFE)
     oce.add(OpCodes.IDLE, Info.IDLE)
+    oce.add(OpCodes.REPORT_ALL_MODES, Info.REPORT_ALL_MODES)
     defs.add_service(CustomServiceList.ACS_SS, "ACS Subsystem", oce)