eive/eive-tmtc
eive
/
eive-tmtc
13
0
Fork 0
Code Issues Pull Requests 1 Projects Releases 97 Wiki Activity

diag datasets and mekf update

This commit is contained in:
Marius Eggert 2023-02-24 14:53:09 +01:00
parent 09dbc6b14d
commit bc9f13f296
1 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
eive_tmtc/tmtc/acs/acs_ctrl.py
View File

@ -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