From f18a7f49cfbf80f8d3f6b18b8452df301c07d852 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 6 Mar 2023 13:09:28 +0100
Subject: [PATCH] add health set and announce health impl

---
 eive_tmtc/tmtc/health.py     | 30 +++++++++++++++++++++++++++---
 eive_tmtc/tmtc/obj_prompt.py |  3 ++-
 2 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/eive_tmtc/tmtc/health.py b/eive_tmtc/tmtc/health.py
index 8ba4daa..cb41000 100644
--- a/eive_tmtc/tmtc/health.py
+++ b/eive_tmtc/tmtc/health.py
@@ -1,4 +1,7 @@
+import struct
+
 from eive_tmtc.config.definitions import CustomServiceList
+from eive_tmtc.tmtc.obj_prompt import prompt_object
 from spacepackets.ecss import PusTelecommand
 from tmtccmd.config.tmtc import (
     tmtc_definitions_provider,
@@ -6,7 +9,7 @@ from tmtccmd.config.tmtc import (
     OpCodeEntry,
 )
 from tmtccmd.tc import service_provider
-from tmtccmd.pus.s201_fsfw_health import Subservice
+from tmtccmd.pus.s201_fsfw_health import Subservice, FsfwHealth
 from tmtccmd.tc.decorator import ServiceProviderParams
 
 
@@ -22,14 +25,35 @@ class Info:
     ANNOUNCE_HEALTH = "Read health state of one object"
 
 
+def prompt_health() -> FsfwHealth:
+    for item in FsfwHealth:
+        print(f"{item}: {item.name}")
+    health_idx = input("Please enter health by index: ")
+    return FsfwHealth(health_idx)
+
+
 @service_provider(CustomServiceList.HEALTH)
 def pack_test_command(p: ServiceProviderParams):
     o = p.op_code
     q = p.queue_helper
     if o == OpCode.SET_HEALTH:
-        raise NotImplementedError
+        app_data = bytearray(prompt_object())
+        health = prompt_health()
+        app_data.extend(struct.pack("!I", health))
+        q.add_log_cmd(Info.SET_HEALTH)
+        q.add_pus_tc(
+            PusTelecommand(
+                service=201, subservice=Subservice.TC_SET_HEALTH, app_data=app_data
+            )
+        )
     if o == OpCode.ANNOUNCE_HEALTH:
-        raise NotImplementedError
+        app_data = bytearray(prompt_object())
+        q.add_log_cmd(Info.ANNOUNCE_HEALTH)
+        q.add_pus_tc(
+            PusTelecommand(
+                service=201, subservice=Subservice.TC_ANNOUNCE_HEALTH, app_data=app_data
+            )
+        )
     elif o == OpCode.ANNOUNCE_HEALTH_ALL:
         q.add_log_cmd(Info.ANNOUNCE_HEALTH_ALL)
         q.add_pus_tc(
diff --git a/eive_tmtc/tmtc/obj_prompt.py b/eive_tmtc/tmtc/obj_prompt.py
index db7d6bd..280dcb2 100644
--- a/eive_tmtc/tmtc/obj_prompt.py
+++ b/eive_tmtc/tmtc/obj_prompt.py
@@ -4,7 +4,7 @@ from eive_tmtc.config.object_ids import (
     IMTQ_HANDLER_ID,
     ACS_BOARD_ASS_ID,
     RW_ASSEMBLY,
-    SUS_BOARD_ASS_ID
+    SUS_BOARD_ASS_ID,
 )
 
 SUBSYSTEM_DICT = {
@@ -21,6 +21,7 @@ ACS_OBJ_DICT = {
     4: ("iMTQ MGT", IMTQ_HANDLER_ID),
 }
 
+
 def prompt_object() -> bytes:
     for k, v in SUBSYSTEM_DICT:
         print(f"{k}: {v}")