add new submode commands

eive_tmtc/tmtc/payload/ploc_mpsoc.py

@@ -23,7 +23,7 @@ from tmtccmd.config.tmtc import (
 from spacepackets.ecss.tc import PusTelecommand
 from tmtccmd.tmtc import DefaultPusQueueHelper
 from eive_tmtc.utility.input_helper import InputHelper
-from tmtccmd.pus.s200_fsfw_mode import pack_mode_data, Mode
+from tmtccmd.pus.s200_fsfw_mode import pack_mode_data, Mode, create_mode_command
 from tmtccmd.pus.s20_fsfw_param import create_load_param_cmd, create_scalar_u8_parameter
 from tmtccmd.pus.s8_fsfw_action import create_action_cmd
 
@@ -94,6 +94,12 @@ class ActionId(enum.IntEnum):
     TC_FLASH_READ_FULL_FILE = 30
 
 
+class Submode(enum.IntEnum):
+    IDLE_OR_NONE = 0
+    REPLAY = 1
+    SNAPSHOT = 2
+
+
 class ParamId(enum.IntEnum):
     PLOC_SUPV_SKIP_CMD_TO_ON = 1
 
@@ -103,8 +109,8 @@ class OpCode:
     OFF = "off"
     NORMAL = "normal"
     VERIFY_BOOT = "verify_boot"
-    MODE_REPLAY = "mode_replay"
-    MODE_IDLE = "mode_idle"
+    MODE_REPLAY_LEGACY = "mode_replay_legacy"
+    MODE_IDLE_LEGACY = "mode_idle_legacy"
     REPLAY_WRITE_SEQ = "replay_write"
     DOWNLINK_PWR_ON = "downlink_pwr_on"
     MEM_WRITE = "memory_write"
@@ -116,12 +122,16 @@ class OpCode:
     FLASH_GET_DIR_CONTENT = "flash_get_dir_content"
     REPLAY_STOP = "replay_stop"
     REPLAY_START = "replay_start"
+    CAM_CMD_SEND = "cam_cmd_send"
     CAM_TAKE_PIC = "cam_take_pic"
     SIMPLEX_SEND_FILE = "simplex_send_file"
     DOWNLINK_DATA_MODULATE = "downlink_data_modulate"
-    MODE_SNAPSHOT = "mode_snapshot"
+    MODE_SNAPSHOT_LEGACY = "mode_snapshot_legacy"
     ENABLE_PLOC_SUPV_COMMANDING_TO_ON = "enable_ploc_supv_cmd_to_on"
     DISABLE_PLOC_SUPV_COMMANDING_TO_ON = "disable_ploc_supv_cmd_to_on"
+    MODE_IDLE = "mode_idle"
+    MODE_REPLAY = "mode_replay"
+    MODE_SNAPSHOT = "mode_snapshot"
 
 
 class Info:
@@ -139,6 +149,7 @@ class Info:
     MEM_READ = "Read from Memory"
     REPLAY_START = "Replay Start"
     CAM_TAKE_PIC = "Cam Take Picture"
+    CAM_CMD_SEND = "Send Camera Command"
     SIMPLEX_SEND_FILE = "Simplex Send File"
     FLASH_READ_FILE = "Copy file from MPSoC to OBC"
     FLASH_WRITE_FILE = "Copy file from OBC to MPSoC"
@@ -168,45 +179,6 @@ def create_ploc_mpsoc_node() -> CmdTreeNode:
     return node
 
 
-# Legacy command definitions.
-@tmtc_definitions_provider
-def add_ploc_mpsoc_cmds(defs: TmtcDefinitionWrapper):
-    oce = OpCodeEntry()
-    oce.add(OpCode.OFF, Info.OFF)
-    oce.add(OpCode.ON, Info.ON)
-    oce.add(OpCode.NORMAL, Info.NORMAL)
-    oce.add(OpCode.MEM_WRITE, "Ploc MPSoC: Memory write")
-    oce.add(OpCode.MEM_READ, "Ploc MPSoC: Memory read")
-    oce.add(OpCode.FLASH_WRITE_FILE, Info.FLASH_WRITE_FILE)
-    oce.add(OpCode.FLASH_READ_FILE, Info.FLASH_READ_FILE)
-    oce.add(OpCode.FLASH_DELETE_FILE, Info.FLASH_DELETE_FILE)
-    oce.add(OpCode.FLASH_GET_DIR_CONTENT, Info.FLASH_GET_DIR_CONTENT)
-    oce.add(OpCode.REPLAY_START, Info.REPLAY_START)
-    oce.add(OpCode.REPLAY_STOP, Info.REPLAY_STOP)
-    oce.add(OpCode.DOWNLINK_PWR_ON, Info.DOWNLINK_PWR_ON)
-    oce.add(OpCode.DOWNLINK_PWR_OFF, Info.DOWNLINK_PWR_OFF)
-    oce.add(OpCode.REPLAY_WRITE_SEQ, Info.REPLAY_WRITE_SEQ)
-    oce.add("12", "Ploc MPSoC: OBSW reset sequence count")
-    oce.add(OpCode.VERIFY_BOOT, Info.VERIFY_BOOT)
-    oce.add(OpCode.MODE_REPLAY, Info.MODE_REPLAY)
-    oce.add(OpCode.MODE_IDLE, Info.MODE_IDLE)
-    oce.add("16", "Ploc MPSoC: Tc cam command send")
-    oce.add("17", "Ploc MPSoC: Set UART TX tristate")
-    oce.add("18", "Ploc MPSoC: Relesase UART TX")
-    oce.add(OpCode.CAM_TAKE_PIC, Info.CAM_TAKE_PIC)
-    oce.add(OpCode.SIMPLEX_SEND_FILE, Info.SIMPLEX_SEND_FILE)
-    oce.add(OpCode.DOWNLINK_DATA_MODULATE, Info.DOWNLINK_DATA_MODULATE)
-    oce.add(OpCode.MODE_SNAPSHOT, Info.MODE_SNAPSHOT)
-    oce.add(
-        OpCode.ENABLE_PLOC_SUPV_COMMANDING_TO_ON, Info.ENABLE_PLOC_SUPV_COMMANDING_TO_ON
-    )
-    oce.add(
-        OpCode.DISABLE_PLOC_SUPV_COMMANDING_TO_ON,
-        Info.DISABLE_PLOC_SUPV_COMMANDING_TO_ON,
-    )
-    defs.add_service(CustomServiceList.PLOC_MPSOC.value, "Ploc MPSoC", oce)
-
-
 def pack_ploc_mpsoc_commands(
     q: DefaultPusQueueHelper, cmd_str: str
 ):  # noqa C901: Complexity okay here.
@@ -299,16 +271,16 @@ def pack_ploc_mpsoc_commands(
             + struct.pack("!H", num_words)
         )
         q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=data))
-    if cmd_str == OpCode.MODE_REPLAY:
+    if cmd_str == OpCode.MODE_REPLAY_LEGACY:
         q.add_log_cmd("PLOC MPSoC: Tc mode replay")
         data = object_id.as_bytes + struct.pack("!I", ActionId.TC_MODE_REPLAY)
         q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=data))
-    if cmd_str == OpCode.MODE_IDLE:
+    if cmd_str == OpCode.MODE_IDLE_LEGACY:
         q.add_log_cmd("PLOC MPSoC: Tc mode idle")
         data = object_id.as_bytes + struct.pack("!I", ActionId.TC_MODE_IDLE)
         q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=data))
-    if cmd_str == "16":
-        q.add_log_cmd("PLOC MPSoC: Tc cam command send")
+    if cmd_str == OpCode.CAM_CMD_SEND:
+        q.add_log_cmd(Info.CAM_CMD_SEND)
         cam_cmd = input("Specify cam command string: ")
         data = (
             object_id.as_bytes
@@ -336,10 +308,22 @@ def pack_ploc_mpsoc_commands(
         q.add_log_cmd("PLOC MPSoC: Downlink data modulate")
         data = prepare_downlink_data_modulate_cmd(object_id.as_bytes)
         q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=data))
-    if cmd_str == OpCode.MODE_SNAPSHOT:
+    if cmd_str == OpCode.MODE_SNAPSHOT_LEGACY:
         q.add_log_cmd("PLOC MPSoC: Mode snapshot")
         data = object_id.as_bytes + struct.pack("!I", ActionId.TC_MODE_SNAPSHOT)
         q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=data))
+    if cmd_str == OpCode.MODE_SNAPSHOT:
+        q.add_log_cmd(Info.MODE_SNAPSHOT)
+        mode = prompt_mode_for_submode_change()
+        q.add_pus_tc(create_mode_command(PLOC_MPSOC_ID, mode, Submode.SNAPSHOT))
+    if cmd_str == OpCode.MODE_IDLE:
+        q.add_log_cmd(Info.MODE_IDLE)
+        mode = prompt_mode_for_submode_change()
+        q.add_pus_tc(create_mode_command(PLOC_MPSOC_ID, mode, Submode.IDLE_OR_NONE))
+    if cmd_str == OpCode.MODE_REPLAY:
+        q.add_log_cmd(Info.MODE_REPLAY)
+        mode = prompt_mode_for_submode_change()
+        q.add_pus_tc(create_mode_command(PLOC_MPSOC_ID, mode, Submode.REPLAY))
     if cmd_str == OpCode.ENABLE_PLOC_SUPV_COMMANDING_TO_ON:
         q.add_log_cmd(Info.ENABLE_PLOC_SUPV_COMMANDING_TO_ON)
         q.add_pus_tc(
@@ -360,6 +344,20 @@ def pack_ploc_mpsoc_commands(
         )
 
 
+def prompt_mode_for_submode_change() -> int:
+    while True:
+        mode = input("Please specify primary mode: [0: ON, 1: NORMAL]")
+        if mode == 0:
+            mode = Mode.ON
+        elif mode == 1:
+            mode = Mode.NORMAL
+        else:
+            print("Invalid mode")
+            continue
+        break
+    return mode
+
+
 def generate_write_mem_command(
     object_id: bytes, memory_address: int, memory_data: int, mem_len: int
 ) -> bytearray: