diff --git a/eive_tmtc/tmtc/acs/acs_ctrl.py b/eive_tmtc/tmtc/acs/acs_ctrl.py index be8ad18..934ccbf 100644 --- a/eive_tmtc/tmtc/acs/acs_ctrl.py +++ b/eive_tmtc/tmtc/acs/acs_ctrl.py @@ -297,7 +297,7 @@ def pack_acs_ctrl_command(p: ServiceProviderParams): elif op_code in OpCodes.REQUEST_RAW_GYR_HK: q.add_log_cmd(Info.REQUEST_RAW_GYR_HK) q.add_pus_tc( - generate_one_hk_command(make_sid(ACS_CONTROLLER, SetId.GYR_RAW_SET)) + create_request_one_diag_command(make_sid(ACS_CONTROLLER, SetId.GYR_RAW_SET)) ) elif op_code in OpCodes.ENABLE_RAW_GYR_HK: q.add_log_cmd(Info.ENABLE_RAW_GYR_HK) @@ -323,7 +323,7 @@ def pack_acs_ctrl_command(p: ServiceProviderParams): elif op_code in OpCodes.ENABLE_PROC_GYR_HK: q.add_log_cmd(Info.ENABLE_PROC_GYR_HK) cmd_tuple = enable_periodic_hk_command_with_interval( - False, make_sid(ACS_CONTROLLER, SetId.GYR_PROC_SET), 2.0 + True, make_sid(ACS_CONTROLLER, SetId.GYR_PROC_SET), 2.0 ) q.add_pus_tc(cmd_tuple[0]) q.add_pus_tc(cmd_tuple[1]) @@ -331,7 +331,7 @@ def pack_acs_ctrl_command(p: ServiceProviderParams): q.add_log_cmd(Info.DISABLE_PROC_GYR_HK) q.add_pus_tc( disable_periodic_hk_command( - False, make_sid(ACS_CONTROLLER, SetId.GYR_PROC_SET) + True, make_sid(ACS_CONTROLLER, SetId.GYR_PROC_SET) ) ) elif op_code in OpCodes.REQUEST_PROC_GPS_HK: @@ -355,7 +355,7 @@ def pack_acs_ctrl_command(p: ServiceProviderParams): ) elif op_code in OpCodes.REQUEST_MEKF_HK: q.add_log_cmd(Info.REQUEST_MEKF_HK) - q.add_pus_tc(generate_one_hk_command(make_sid(ACS_CONTROLLER, SetId.MEKF_DATA))) + q.add_pus_tc(create_request_one_diag_command(make_sid(ACS_CONTROLLER, SetId.MEKF_DATA))) elif op_code in OpCodes.ENABLE_MEKF_HK: q.add_log_cmd(Info.ENABLE_MEKF_HK) cmd_tuple = enable_periodic_hk_command_with_interval( @@ -708,14 +708,18 @@ def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes): def handle_mekf_data(pw: PrintWrapper, hk_data: bytes): + mekfStatus = {0 : "UNINITIALIZED", 1: "NO_GYR_DATA", 2: "NO_MODEL_VECTORS", 3: "NO_SUS_MGM_STR_DATA", + 4: "COVARIANCE_INVERSION_FAILED", 10: "INITIALIZED", 11: "RUNNING"} pw.dlog("Received MEKF Set") fmt_quat = "!dddd" fmt_str_4 = "[{:8.3f}, {:8.3f}, {:8.3f}, {:8.3f}]" fmt_str_3 = "[{:8.3f}, {:8.3f}, {:8.3f}]" fmt_vec = "!ddd" + fmt_sts = "!B" inc_len_quat = struct.calcsize(fmt_quat) inc_len_vec = struct.calcsize(fmt_vec) - if len(hk_data) < inc_len_quat + inc_len_vec: + inc_len_sts = struct.calcsize(fmt_sts) + if len(hk_data) < inc_len_quat + inc_len_vec + inc_len_sts: pw.dlog("Received HK set too small") return current_idx = 0 @@ -723,9 +727,12 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes): current_idx += inc_len_quat rate = struct.unpack(fmt_vec, hk_data[current_idx : current_idx + inc_len_vec]) current_idx += inc_len_vec + status = struct.unpack(fmt_sts, hk_data[current_idx : current_idx + inc_len_sts])[0] + current_idx += inc_len_sts + pw.dlog(f"{'MEKF Status'.ljust(25)}: {mekfStatus[status]}") pw.dlog(f"{'MEKF Quaternion'.ljust(25)}: {fmt_str_4.format(*quat)}") pw.dlog(f"{'MEKF Rotational Rate'.ljust(25)}: {fmt_str_3.format(*rate)}") - pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=2) + pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=3) def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes): @@ -736,7 +743,7 @@ def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes): inc_len_quat = struct.calcsize(fmt_quat) inc_len_scalar = struct.calcsize(fmt_scalar) inc_len_vec = struct.calcsize(fmt_vec) - if len(hk_data) < 2 * inc_len_quat + inc_len_scalar: + if len(hk_data) < 2 * inc_len_quat + inc_len_scalar + inc_len_vec: pw.dlog("Received HK set too small") return current_idx = 0