Merge pull request 'ACS CTRL Set Handling' (#120) from mueller_acs_ctrl_sets into main

Reviewed-on: #120

CHANGELOG.md

@@ -8,6 +8,10 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/).
 The [milestone](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/milestones)
 list yields a list of all related PRs for each release.
 
+# [unreleased]
+
+- Handle ACS CTRL HK sets
+
 # [v2.0.2] 01.12.2022
 
 - Bumped dependencies, small fix to allow working script if PyQt is not installed

eive_tmtc/pus_tm/hk_handling.py

@@ -1,6 +1,6 @@
 """HK Handling for EIVE OBSW"""
 # from pus_tm.tcp_server_objects import TCP_SEVER_SENSOR_TEMPERATURES
-from eive_tmtc.tmtc.acs.acs_ctrl import handle_acs_ctrl_mgm_data
+from eive_tmtc.tmtc.acs.acs_ctrl import handle_raw_mgm_data, handle_acs_ctrl_hk_data
 from eive_tmtc.pus_tm.devs.plpcdu import handle_plpcdu_hk
 from eive_tmtc.pus_tm.devs.rad_sensor import handle_rad_sensor_data
 from eive_tmtc.pus_tm.devs.sus import handle_sus_hk
@@ -175,7 +175,7 @@ def handle_regular_hk_print(
     elif objb == obj_ids.PLOC_SUPV_ID:
         handle_supv_hk_data(set_id=set_id, hk_data=hk_data, printer=printer)
     elif objb == obj_ids.ACS_CONTROLLER:
-        handle_acs_ctrl_mgm_data(printer, hk_data)
+        handle_acs_ctrl_hk_data(printer, set_id, hk_data)
     else:
         LOGGER.info(
             f"Service 3 TM: Parsing for object {object_id} and set ID {set_id} "

eive_tmtc/tmtc/acs/acs_ctrl.py

@@ -23,23 +23,43 @@ from tmtccmd.tc.pus_3_fsfw_hk import (
 )
 from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
 
+
 LOGGER = get_console_logger()
 
 
 class SetIds(enum.IntEnum):
-    MGM_SET = 0
+    MGM_RAW_SET = 0
+    MGM_PROC_SET = 1
+    SUS_RAW_SET = 2
+    SUS_PROC_SET = 3
+    GYR_RAW_SET = 4
+    GYR_PROC_SET = 5
+    GPS_PROC_SET = 6
+    MEKF_DATA = 7
+    CTRL_VAL_DATA = 8
+    ACTUATOR_CMD_DATA = 9
 
 
 class OpCodes:
-    REQUEST_MGM_HK = ["0", "req-mgm-hk"]
+    REQUEST_MGM_HK = ["0", "mgm_raw_hk"]
-    ENABLE_MGM_HK = ["1", "enable-mgm-hk"]
+    ENABLE_MGM_HK = ["1", "enable_raw_hk"]
-    DISABLE_MGM_HK = ["1", "disable-mgm-hk"]
+    DISABLE_MGM_HK = ["1", "disable_raw_hk"]
+    REQUEST_PROC_MGM_HK = ["mgm_proc_hk"]
+    REQUEST_RAW_GYRO_HK = ["gyro_raw_hk"]
+    REQUEST_PROC_GYRO_HK = ["gyro_proc_hk"]
+    REQUEST_RAW_SUS_HK = ["sus_raw_hk"]
+    REQUEST_PROC_SUS_HK = ["sus_proc_hk"]
 
 
 class Info:
     REQUEST_MGM_HK = "Request MGM HK once"
     ENABLE_MGM_HK = "Enable MGM HK data generation"
     DISABLE_MGM_HK = "Disable MGM HK data generation"
+    REQUEST_PROC_MGM_HK = "Request Processed MGM HK"
+    REQUEST_RAW_GYRO_HK = "Request Raw Gyro HK"
+    REQUEST_PROC_GYRO_HK = "Request Processed Gyro HK"
+    REQUEST_RAW_SUS_HK = "Request Raw SUS HK"
+    REQUEST_PROC_SUS_HK = "Request Processed SUS HK"
 
 
 PERFORM_MGM_CALIBRATION = False
@@ -69,7 +89,7 @@ def acs_cmd_defs(defs: TmtcDefinitionWrapper):
 def pack_acs_ctrl_command(p: ServiceProviderParams):
     op_code = p.op_code
     q = p.queue_helper
-    sid = make_sid(ACS_CONTROLLER, SetIds.MGM_SET)
+    sid = make_sid(ACS_CONTROLLER, SetIds.MGM_RAW_SET)
     if op_code in OpCodes.REQUEST_MGM_HK:
         q.add_log_cmd(Info.REQUEST_MGM_HK)
         q.add_pus_tc(generate_one_hk_command(sid))
@@ -85,9 +105,80 @@ def pack_acs_ctrl_command(p: ServiceProviderParams):
         LOGGER.info(f"Unknown op code {op_code}")
 
 
-def handle_acs_ctrl_mgm_data(printer: FsfwTmTcPrinter, hk_data: bytes):
+def handle_acs_ctrl_hk_data(printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes):
-    current_idx = 0
     pw = PrintWrapper(printer)
+    if set_id == SetIds.MGM_RAW_SET:
+        handle_raw_mgm_data(pw, hk_data)
+    elif set_id == SetIds.MGM_PROC_SET:
+        handle_mgm_data_processed(pw, hk_data)
+    elif set_id == SetIds.GYR_RAW_SET:
+        handle_gyr_data_raw(pw, hk_data)
+    elif set_id == SetIds.GYR_PROC_SET:
+        handle_gyr_data_processed(pw, hk_data)
+    elif set_id == SetIds.SUS_RAW_SET:
+        handle_acs_ctrl_sus_raw_data(pw, hk_data)
+    elif set_id == SetIds.SUS_PROC_SET:
+        handle_acs_ctrl_sus_processed_data(pw, hk_data)
+
+
+def handle_acs_ctrl_sus_raw_data(pw: PrintWrapper, hk_data: bytes):
+    if len(hk_data) < 6 * 2 * 12:
+        pw.dlog(
+            f"SUS Raw dataset with size {len(hk_data)} does not have expected size"
+            f" 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}")
+        pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=12)
+
+
+def handle_acs_ctrl_sus_processed_data(pw: PrintWrapper, hk_data: bytes):
+    if len(hk_data) < 3 * 4 * 12:
+        pw.dlog(
+            f"SUS Raw dataset with size {len(hk_data)} does not have expected size"
+            f" 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}")
+    pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=12)
+
+
+def handle_raw_mgm_data(pw: PrintWrapper, hk_data: bytes):
+    current_idx = 0
 
     if len(hk_data) < 61:
         pw.dlog(
@@ -142,6 +233,99 @@ def handle_acs_ctrl_mgm_data(printer: FsfwTmTcPrinter, hk_data: bytes):
     assert current_idx == 61
 
 
+def handle_mgm_data_processed(pw: PrintWrapper, hk_data: bytes):
+    pw.dlog("Received Processed MGM Set")
+    fmt_str = "!fffffddd"
+    inc_len = struct.calcsize(fmt_str)
+    if len(hk_data) < inc_len:
+        pw.dlog("Recieved HK set too small")
+        return
+    current_idx = 0
+    for i in range(5):
+        fmt_str = "!fff"
+        inc_len = struct.calcsize(fmt_str)
+        mgm_vec = struct.unpack(fmt_str, hk_data[current_idx : current_idx + inc_len])
+        mgm_vec = [f"{val:8.3f}" for val in mgm_vec]
+        pw.dlog(f"MGM {i}: {mgm_vec}")
+    fmt_str = "!ddd"
+    inc_len = struct.calcsize(fmt_str)
+    mgm_vec_tot = struct.unpack(fmt_str, hk_data[current_idx : current_idx + inc_len])
+    mgm_vec_tot = [f"{val:8.3f}" for val in mgm_vec_tot]
+    current_idx += inc_len
+    pw.dlog(f"MGM Total Vec: {mgm_vec_tot}")
+    mgm_vec_tot_deriv = struct.unpack(
+        fmt_str, hk_data[current_idx : current_idx + inc_len]
+    )
+    mgm_vec_tot_deriv = [f"{val:8.3f}" for val in mgm_vec_tot_deriv]
+    pw.dlog(f"MGM Total Vec Deriv: {mgm_vec_tot_deriv}")
+    current_idx += inc_len
+    mag_igrf_model = struct.unpack(
+        fmt_str, hk_data[current_idx : current_idx + inc_len]
+    )
+    mag_igrf_model = [f"{val:8.3f}" for val in mag_igrf_model]
+    pw.dlog(f"MAG IGRF Model: {mag_igrf_model}")
+    current_idx += inc_len
+    pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=8)
+
+
+def handle_gyr_data_raw(pw: PrintWrapper, hk_data: bytes):
+    pw.dlog("Received Gyro Raw Set with rotation rates in deg per second")
+    float_fmt = "!fff"
+    double_fmt = "!ddd"
+    inc_len_flt = struct.calcsize(float_fmt)
+    inc_len_double = struct.calcsize(double_fmt)
+    if len(hk_data) < 2 * inc_len_double + 2 * inc_len_flt:
+        pw.dlog("HK data too small")
+        return
+    current_idx = 0
+    gyr_0_adis = [
+        f"{val:8.3f}" for val in struct.unpack(double_fmt, hk_data[:inc_len_double])
+    ]
+    current_idx += inc_len_double
+    gyr_1_l3 = [
+        f"{val:8.3f}" for val in struct.unpack(float_fmt, hk_data[:inc_len_flt])
+    ]
+    current_idx += inc_len_flt
+    gyr_2_adis = [
+        f"{val:8.3f}" for val in struct.unpack(double_fmt, hk_data[:inc_len_double])
+    ]
+    current_idx += inc_len_flt
+    gyr_3_l3 = [
+        f"{val:8.3f}" for val in struct.unpack(float_fmt, hk_data[:inc_len_flt])
+    ]
+    current_idx += inc_len_flt
+    pw.dlog(f"Gyro 0 ADIS: {gyr_0_adis}")
+    pw.dlog(f"Gyro 1 L3: {gyr_1_l3}")
+    pw.dlog(f"Gyro 2 ADIS: {gyr_2_adis}")
+    pw.dlog(f"Gyro 3 L3: {gyr_3_l3}")
+    pw.printer.print_validity_buffer(hk_data[current_idx:], 4)
+
+
+GYRO_NAMES = ["Gyro 0 ADIS", "Gyro 1 L3", "Gyro 2 ADIS", "Gyro 3 L3"]
+
+
+def handle_gyr_data_processed(pw: PrintWrapper, hk_data: bytes):
+    pw.dlog("Received Gyro Processed Set with rotation rates in deg per second")
+    fmt_str = "!ddd"
+    inc_len = struct.calcsize(fmt_str)
+    current_idx = 0
+    for i in range(4):
+        gyr_vec = [
+            f"{val:8.3f}"
+            for val in struct.unpack(
+                fmt_str, hk_data[current_idx : current_idx + inc_len]
+            )
+        ]
+        pw.dlog(f"{GYRO_NAMES[i]}: {gyr_vec}")
+        current_idx += inc_len
+    gyr_vec_tot = [
+        f"{val:8.3f}"
+        for val in struct.unpack(fmt_str, hk_data[current_idx : current_idx + inc_len])
+    ]
+    pw.dlog(f"Gyro Vec Total: {gyr_vec_tot}")
+    current_idx += inc_len
+
+
 def perform_mgm_calibration(pw: PrintWrapper, mgm_tuple: Tuple):
     global CALIBR_SOCKET, CALIBRATION_ADDR
     try:
@@ -150,7 +334,8 @@ def perform_mgm_calibration(pw: PrintWrapper, mgm_tuple: Tuple):
         reply = CALIBR_SOCKET.recv(1024)
         if len(reply) != 2:
             pw.dlog(
-                f"MGM calibration: Reply received command {declare_api_cmd} has invalid length {len(reply)}"
+                f"MGM calibration: Reply received command {declare_api_cmd} has"
+                f" invalid length {len(reply)}"
             )
             return
         else:
@@ -167,7 +352,8 @@ def perform_mgm_calibration(pw: PrintWrapper, mgm_tuple: Tuple):
         reply = CALIBR_SOCKET.recv(1024)
         if len(reply) != 2:
             pw.dlog(
-                f"MGM calibration: Reply received command magnetometer_field has invalid length {len(reply)}"
+                f"MGM calibration: Reply received command magnetometer_field has invalid "
+                f"length {len(reply)}"
             )
             return
         else: