From 09b7a01ff623ec9e0a35b93edb6c6f4b9d597424 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 21 Jul 2023 11:20:20 +0200 Subject: [PATCH 1/2] init --- eive_tmtc/tmtc/acs/acs_ctrl.py | 67 ++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/eive_tmtc/tmtc/acs/acs_ctrl.py b/eive_tmtc/tmtc/acs/acs_ctrl.py index c763786..2088f05 100644 --- a/eive_tmtc/tmtc/acs/acs_ctrl.py +++ b/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): @@ -1132,6 +1164,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]") + pw.printer.print_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 From f5ec50b67450fab1351754f90331e85b7a4863c5 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 24 Jul 2023 09:47:52 +0200 Subject: [PATCH 2/2] guess i added PTG strats in june ... --- eive_tmtc/tmtc/acs/acs_ctrl.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/eive_tmtc/tmtc/acs/acs_ctrl.py b/eive_tmtc/tmtc/acs/acs_ctrl.py index 2088f05..02363fe 100644 --- a/eive_tmtc/tmtc/acs/acs_ctrl.py +++ b/eive_tmtc/tmtc/acs/acs_ctrl.py @@ -1058,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", - 11: "WITHOUT_MEKF", - 12: "ECLIPSE_DAMPING", - 13: "ECLIPSE_IDELING", + 10: "SAFE_MEKF", + 11: "SAFE_GYR", + 12: "SAFE_SUSMGM", + 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" @@ -1114,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: - pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {safe_strat[strat]}") + if ctrl_strat.get(strat) is not None: + 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}") @@ -1196,7 +1200,7 @@ def handle_fused_rot_rate_data(pw: PrintWrapper, hk_data: bytes): 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]") - pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=3) + FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=3) def perform_mgm_calibration( # noqa C901: Complexity okay