From 72230c613bf84d62325d990b36e9bdf5d9833162 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 2 Dec 2022 17:22:15 +0100
Subject: [PATCH] sus data processing

---
 eive_tmtc/tmtc/acs/acs_ctrl.py | 46 ++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/eive_tmtc/tmtc/acs/acs_ctrl.py b/eive_tmtc/tmtc/acs/acs_ctrl.py
index cc87b9f..f0f9179 100644
--- a/eive_tmtc/tmtc/acs/acs_ctrl.py
+++ b/eive_tmtc/tmtc/acs/acs_ctrl.py
@@ -85,6 +85,52 @@ def pack_acs_ctrl_command(p: ServiceProviderParams):
         LOGGER.info(f"Unknown op code {op_code}")
 
 
+def handle_acs_ctrl_sus_raw_data(printer: FsfwTmTcPrinter, hk_data: bytes):
+    pw = PrintWrapper(printer)
+    if len(hk_data) < 6 * 2 * 12:
+        pw.dlog(f"SUS Raw dataset with size {len(hk_data)} does not have expected size of {6 * 2 * 12} bytes")
+        return
+    current_idx = 0
+    for idx in range(12):
+        fmt_str = "!HHHHHH"
+        length = struct.calcsize(fmt_str)
+        sus_list = struct.unpack(fmt_str, hk_data[current_idx: current_idx + length])
+        sus_list_formatted = [f"{val:#04x}" for val in sus_list]
+        current_idx += length
+        pw.dlog(f"SUS {idx} RAW: {sus_list_formatted}")
+        printer.print_validity_buffer(hk_data[current_idx:], num_vars=12)
+
+
+def handle_acs_ctrl_sus_processed_data(printer: FsfwTmTcPrinter, hk_data: bytes):
+    pw = PrintWrapper(printer)
+    if len(hk_data) < 3 * 4 * 12:
+        pw.dlog(f"SUS Raw dataset with size {len(hk_data)} does not have expected size of {3 * 4 * 12} bytes")
+        return
+    current_idx = 0
+    for idx in range(12):
+        fmt_str = "!fff"
+        length = struct.calcsize(fmt_str)
+        sus_list = struct.unpack(fmt_str, hk_data[current_idx: current_idx + length])
+        sus_list_formatted = [f"{val:8.3f}" for val in sus_list]
+        current_idx += length
+        pw.dlog(f"SUS {idx} CALIB: {sus_list_formatted}")
+    fmt_str = "!ddd"
+    inc_len = struct.calcsize(fmt_str)
+    sus_vec_tot = list(struct.unpack(fmt_str, hk_data[current_idx: current_idx + inc_len]))
+    sus_vec_tot = [f"{val:8.3f}" for val in {sus_vec_tot}]
+    current_idx += inc_len
+    pw.dlog(f"SUS Vector Total: {sus_vec_tot}")
+    sus_vec_tot_deriv = struct.unpack(fmt_str, hk_data[current_idx: current_idx + inc_len])
+    sus_vec_tot_deriv = [f"{val:8.3f}" for val in {sus_vec_tot_deriv}]
+    current_idx += inc_len
+    pw.dlog(f"SUS Vector Derivative: {sus_vec_tot_deriv}")
+    sun_ijk_model = list(struct.unpack(fmt_str, hk_data[current_idx: current_idx + inc_len]))
+    sun_ijk_model = [f"{val:8.3f}" for val in {sun_ijk_model}]
+    current_idx += inc_len
+    pw.dlog(f"SUS ijk Model: {sun_ijk_model}")
+    printer.print_validity_buffer(hk_data[current_idx:], num_vars=12)
+
+
 def handle_acs_ctrl_mgm_data(printer: FsfwTmTcPrinter, hk_data: bytes):
     current_idx = 0
     pw = PrintWrapper(printer)