Merge pull request 'new safe mode' (#221) from new-safe into main

Reviewed-on: #221
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

eive_tmtc/tmtc/acs/acs_ctrl.py

@@ -59,6 +59,7 @@ class SetId(enum.IntEnum):
     MEKF_DATA = 7
     CTRL_VAL_DATA = 8
     ACTUATOR_CMD_DATA = 9
+    FUSED_ROT_RATE_DATA = 10
 
 
 class ActionId(enum.IntEnum):
@@ -112,6 +113,9 @@ class OpCodes:
     REQUEST_ACT_CMD_HK = ["act_cmd_hk"]
     ENABLE_ACT_CMD_HK = ["act_cmd_enable_hk"]
     DISABLE_ACT_CMD_HK = ["act_cmd_disable_hk"]
+    REQUEST_FUSED_ROT_RATE_HK = ["f_rot_rate_hk"]
+    ENABLE_FUSED_ROT_RATE_HK = ["f_rot_rate_enable_hk"]
+    DISABLE_FUSED_ROT_RATE_HK = ["f_rot_rate_disable_hk"]
 
 
 class Info:
@@ -159,6 +163,9 @@ class Info:
     REQUEST_ACT_CMD_HK = "Request Actuator Commands HK"
     ENABLE_ACT_CMD_HK = "Enable Actuator Commands HK data generation"
     DISABLE_ACT_CMD_HK = "Disable Actuator Commands HK data generation"
+    REQUEST_FUSED_ROT_RATE_HK = "Request Fused Rotational Rates HK"
+    ENABLE_FUSED_ROT_RATE_HK = "Enable Fused Rotational Rates HK data generation"
+    DISABLE_FUSED_ROT_RATE_HK = "Disable Fused Rotational Rates HK data generation"
 
 
 PERFORM_MGM_CALIBRATION = False
@@ -223,6 +230,9 @@ def acs_cmd_defs(defs: TmtcDefinitionWrapper):
     oce.add(keys=OpCodes.REQUEST_ACT_CMD_HK, info=Info.REQUEST_ACT_CMD_HK)
     oce.add(keys=OpCodes.ENABLE_ACT_CMD_HK, info=Info.ENABLE_ACT_CMD_HK)
     oce.add(keys=OpCodes.DISABLE_ACT_CMD_HK, info=Info.DISABLE_ACT_CMD_HK)
+    oce.add(keys=OpCodes.REQUEST_FUSED_ROT_RATE_HK, info=Info.REQUEST_FUSED_ROT_RATE_HK)
+    oce.add(keys=OpCodes.ENABLE_FUSED_ROT_RATE_HK, info=Info.ENABLE_FUSED_ROT_RATE_HK)
+    oce.add(keys=OpCodes.DISABLE_FUSED_ROT_RATE_HK, info=Info.DISABLE_FUSED_ROT_RATE_HK)
     defs.add_service(
         name=CustomServiceList.ACS_CTRL.value, info="ACS Controller", op_code_entry=oce
     )
@@ -484,6 +494,26 @@ def pack_acs_ctrl_command(p: ServiceProviderParams):  # noqa C901
                 False, make_sid(ACS_CONTROLLER, SetId.ACTUATOR_CMD_DATA)
             )
         )
+    elif op_code in OpCodes.REQUEST_FUSED_ROT_RATE_HK:
+        q.add_log_cmd(Info.REQUEST_FUSED_ROT_RATE_HK)
+        q.add_pus_tc(
+            generate_one_hk_command(make_sid(ACS_CONTROLLER, SetId.FUSED_ROT_RATE_DATA))
+        )
+    elif op_code in OpCodes.ENABLE_FUSED_ROT_RATE_HK:
+        interval = float(input("Please specify interval in floating point seconds: "))
+        q.add_log_cmd(Info.ENABLE_FUSED_ROT_RATE_HK)
+        cmd_tuple = enable_periodic_hk_command_with_interval(
+            False, make_sid(ACS_CONTROLLER, SetId.FUSED_ROT_RATE_DATA), interval
+        )
+        q.add_pus_tc(cmd_tuple[0])
+        q.add_pus_tc(cmd_tuple[1])
+    elif op_code in OpCodes.DISABLE_FUSED_ROT_RATE_HK:
+        q.add_log_cmd(Info.DISABLE_FUSED_ROT_RATE_HK)
+        q.add_pus_tc(
+            disable_periodic_hk_command(
+                False, make_sid(ACS_CONTROLLER, SetId.FUSED_ROT_RATE_DATA)
+            )
+        )
     else:
         logging.getLogger(__name__).info(f"Unknown op code {op_code}")
 
@@ -699,6 +729,8 @@ def handle_acs_ctrl_hk_data(
             handle_ctrl_val_data(pw, hk_data)
         case SetId.ACTUATOR_CMD_DATA:
             handle_act_cmd_data(pw, hk_data)
+        case SetId.FUSED_ROT_RATE_DATA:
+            handle_fused_rot_rate_data(pw, hk_data)
 
 
 def handle_acs_ctrl_sus_raw_data(pw: PrintWrapper, hk_data: bytes):
@@ -1026,16 +1058,20 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
 
 
 def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
-    safe_strat = {
+    ctrl_strat = {
         0: "OFF",
         1: "NO_MAG_FIELD_FOR_CONTROL",
         2: "NO_SENSORS_FOR_CONTROL",
-        10: "ACTIVE_MEKF",
+        10: "SAFE_MEKF",
-        11: "WITHOUT_MEKF",
+        11: "SAFE_GYR",
-        12: "ECLIPSE_DAMPING",
+        12: "SAFE_SUSMGM",
-        13: "ECLIPSE_IDELING",
+        13: "SAFE_ECLIPSE_DAMPING_GYR",
+        14: "SAFE_ECLIPSE_DAMPING_SUSMGM",
+        15: "SAFE_ECLIPSE_IDELING",
         20: "DETUMBLE_FULL",
         21: "DETUMBLE_DETERIORATED",
+        30: "PTG_MEKF",
+        31: "PTG_RAW",
     }
     pw.dlog("Received CTRL Values Set")
     fmt_strat = "!B"
@@ -1082,8 +1118,8 @@ def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
         )
     ]
     current_idx += inc_len_vec
-    if safe_strat.get(strat) is not None:
+    if ctrl_strat.get(strat) is not None:
-        pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {safe_strat[strat]}")
+        pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {ctrl_strat[strat]}")
     else:
         pw.dlog(f"{'Safe Ctrl Strategy (key unknown)'.ljust(25)}: {strat}")
     pw.dlog(f"Control Values Target Quaternion: {tgt_quat}")
@@ -1132,6 +1168,41 @@ def handle_act_cmd_data(pw: PrintWrapper, hk_data: bytes):
     FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=3)
 
 
+def handle_fused_rot_rate_data(pw: PrintWrapper, hk_data: bytes):
+    pw.dlog("Received Fused Rotation Rates Data Set")
+    fmt_vec3_double = "!ddd"
+    inc_len_vec3_double = struct.calcsize(fmt_vec3_double)
+    if len(hk_data) < 3*inc_len_vec3_double:
+        pw.dlog("Received HK set too small")
+        return
+    current_idx = 0
+    rot_rate_orthogonal = [
+        f"{val*180/math.pi:8.3f}"
+        for val in struct.unpack(
+            fmt_vec3_double, hk_data[current_idx : current_idx + inc_len_vec3_double]
+        )
+    ]
+    current_idx += inc_len_vec3_double
+    rot_rate_parallel = [
+        f"{val*180/math.pi:8.3f}"
+        for val in struct.unpack(
+            fmt_vec3_double, hk_data[current_idx : current_idx + inc_len_vec3_double]
+        )
+    ]
+    current_idx += inc_len_vec3_double
+    rot_rate_total = [
+        f"{val*180/math.pi:8.3f}"
+        for val in struct.unpack(
+            fmt_vec3_double, hk_data[current_idx : current_idx + inc_len_vec3_double]
+        )
+    ]
+    current_idx += inc_len_vec3_double
+    pw.dlog(f"Fused Rotational Rate Orthogonal: {rot_rate_orthogonal} [deg/s]")
+    pw.dlog(f"Fused Rotational Rate Parallel: {rot_rate_parallel} [deg/s]")
+    pw.dlog(f"Fused Rotational Rate Total: {rot_rate_total} [deg/s]")
+    FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=3)
+
+
 def perform_mgm_calibration(  # noqa C901: Complexity okay
     pw: PrintWrapper, mgm_tuple: Tuple
 ):  # noqa C901: Complexity okay