From 1ddb93410ef6dd418a1b151c586294fc838c1dba Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 4 Aug 2023 16:24:16 +0200
Subject: [PATCH] =?UTF-8?q?w=C3=B6rks?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 eive_tmtc/tmtc/acs/gps.py | 65 ++++++++++++++++++++++++++++++++++++---
 1 file changed, 61 insertions(+), 4 deletions(-)

diff --git a/eive_tmtc/tmtc/acs/gps.py b/eive_tmtc/tmtc/acs/gps.py
index a70ca6a..9de4692 100644
--- a/eive_tmtc/tmtc/acs/gps.py
+++ b/eive_tmtc/tmtc/acs/gps.py
@@ -19,6 +19,8 @@ from tmtccmd.fsfw.tmtc_printer import FsfwTmTcPrinter
 
 _LOGGER = logging.getLogger(__name__)
 
+class GpsInfo:
+    MAX_SATELLITES = 30
 
 class OpCode:
     OFF = "off"
@@ -69,7 +71,6 @@ def add_gps_cmds(defs: TmtcDefinitionWrapper):
 
 
 def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
-    print("Yo")
     if op_code in OpCode.RESET_GNSS:
         # TODO: This needs to be re-implemented
         _LOGGER.warning("Reset pin handling needs to be re-implemented")
@@ -88,9 +89,25 @@ def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
         q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=make_sid(object_id=object_id,
                                                                                  set_id=SetId.CORE_HK)))
     if op_code in OpCode.REQ_CORE_HK:
-        print("Waddup")
         q.add_log_cmd(f"GPS: {Info.REQ_CORE_HK}")
         q.add_pus_tc(create_request_one_hk_command(sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK)))
+    if op_code in OpCode.ENABLE_SKYVIEW_HK:
+        interval = float(input("Please specify interval in floating point seconds: "))
+        if interval <= 0:
+            raise ValueError("invalid interval")
+        q.add_log_cmd(f"GPS: {Info.ENABLE_SKYVIEW_HK}")
+        cmds = create_enable_periodic_hk_command_with_interval(
+            diag=False, sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK), interval_seconds=interval
+        )
+        for cmd in cmds:
+            q.add_pus_tc(cmd)
+    if op_code in OpCode.DISABLE_SKYVIEW_HK:
+        q.add_log_cmd(f"gps: {Info.DISABLE_SKYVIEW_HK}")
+        q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=make_sid(object_id=object_id,
+                                                                                 set_id=SetId.SKYVIEW_HK)))
+    if op_code in OpCode.REQ_SKYVIEW_HK:
+        q.add_log_cmd(f"GPS: {Info.REQ_SKYVIEW_HK}")
+        q.add_pus_tc(create_request_one_hk_command(sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK)))
     if op_code in OpCode.ON:
         q.add_log_cmd(f"GPS: {Info.ON}")
         q.add_pus_tc(create_mode_command(object_id, Mode.ON, 0))
@@ -109,8 +126,8 @@ def handle_gps_data(
     match set_id:
         case SetId.CORE_HK:
             handle_core_data(pw, hk_data)
-        #case SetId.SKYVIEW_HK:
-        #    handle_mgm_data_processed(pw, hk_data)
+        case SetId.SKYVIEW_HK:
+            handle_skyview_data(pw, hk_data)
 
 
 def handle_core_data(pw: PrintWrapper, hk_data: bytes):
@@ -157,3 +174,43 @@ def handle_core_data(pw: PrintWrapper, hk_data: bytes):
     FsfwTmTcPrinter.get_validity_buffer(
         validity_buffer=hk_data[current_idx:], num_vars=14
     )
+
+
+def handle_skyview_data(pw: PrintWrapper, hk_data: bytes):
+    data_length = 8+GpsInfo.MAX_SATELLITES*(8+3*2+1)
+    if len(hk_data) < data_length:
+        pw.dlog(
+            f"GPS Skyview dataset with size {len(hk_data)} does not have expected size"
+            f" of {data_length} bytes"
+        )
+        return
+    current_idx = 0
+    fmt_str_unix = "!d"
+    fmt_str_int16 = "!" + "h" * GpsInfo.MAX_SATELLITES
+    fmt_str_double = "!" + "d" * GpsInfo.MAX_SATELLITES
+    fmt_str_uint8 = "!" + "B" * GpsInfo.MAX_SATELLITES
+    inc_len_unix = struct.calcsize(fmt_str_unix)
+    inc_len_int16 = struct.calcsize(fmt_str_int16)
+    inc_len_double = struct.calcsize(fmt_str_double)
+    inc_len_uint8 = struct.calcsize(fmt_str_uint8)
+    unix = struct.unpack(fmt_str_unix, hk_data[current_idx: current_idx + inc_len_unix])
+    current_idx += inc_len_unix
+    prn_id = struct.unpack(fmt_str_int16, hk_data[current_idx: current_idx + inc_len_int16])
+    current_idx += inc_len_int16
+    azimuth = struct.unpack(fmt_str_int16, hk_data[current_idx: current_idx + inc_len_int16])
+    current_idx += inc_len_int16
+    elevation = struct.unpack(fmt_str_int16, hk_data[current_idx: current_idx + inc_len_int16])
+    current_idx += inc_len_int16
+    signal_to_noise = struct.unpack(fmt_str_double, hk_data[current_idx: current_idx + inc_len_double])
+    current_idx += inc_len_double
+    used = struct.unpack(fmt_str_uint8, hk_data[current_idx: current_idx + inc_len_uint8])
+    current_idx += inc_len_uint8
+    pw.dlog(f"Skyview Time: {unix} unix-sec")
+    pw.dlog("{:<8} {:<8} {:<8} {:<8} {:<8}".format('PRN_ID', 'AZ [°]', 'EL [°]', 'S2N [dBW]', 'USED'))
+    for idx in range(GpsInfo.MAX_SATELLITES):
+        pw.dlog("{:<8} {:<8} {:<8} {:<8} {:<8}".format(prn_id[idx], azimuth[idx], elevation[idx], signal_to_noise[idx],
+                                                       used[idx]))
+    FsfwTmTcPrinter.get_validity_buffer(
+        validity_buffer=hk_data[current_idx:], num_vars=6
+    )
+