This commit is contained in:
Marius Eggert 2023-04-14 20:24:21 +02:00
parent 43b530cdb7
commit e9b8d2658d

View File

@ -1027,17 +1027,32 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes): def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
safe_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",
20: "DETUMBLE_FULL",
21: "DETUMBLE_DETERIORATED",
}
pw.dlog("Received CTRL Values Set") pw.dlog("Received CTRL Values Set")
fmt_strat = "!B"
fmt_quat = "!dddd" fmt_quat = "!dddd"
fmt_scalar = "!d" fmt_scalar = "!d"
fmt_vec = "!ddd" fmt_vec = "!ddd"
inc_len_strat = struct.calcsize(fmt_strat)
inc_len_quat = struct.calcsize(fmt_quat) inc_len_quat = struct.calcsize(fmt_quat)
inc_len_scalar = struct.calcsize(fmt_scalar) inc_len_scalar = struct.calcsize(fmt_scalar)
inc_len_vec = struct.calcsize(fmt_vec) inc_len_vec = struct.calcsize(fmt_vec)
if len(hk_data) < 2 * inc_len_quat + inc_len_scalar + inc_len_vec: if len(hk_data) < inc_len_strat + 2 * inc_len_quat + inc_len_scalar + inc_len_vec:
pw.dlog("Received HK set too small") pw.dlog("Received HK set too small")
return return
current_idx = 0 current_idx = 0
strat = struct.unpack(fmt_strat, hk_data[current_idx : current_idx + inc_len_strat])[0]
current_idx += inc_len_strat
tgt_quat = [ tgt_quat = [
f"{val:8.3f}" f"{val:8.3f}"
for val in struct.unpack( for val in struct.unpack(
@ -1066,11 +1081,15 @@ def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
) )
] ]
current_idx += inc_len_vec current_idx += inc_len_vec
if safe_strat.get(strat) is not None:
pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {safe_strat[strat]}")
else:
pw.dlog(f"{'Safe Ctrl Strategy (key unknown)'.ljust(25)}: {strat}")
pw.dlog(f"Control Values Target Quaternion: {tgt_quat}") pw.dlog(f"Control Values Target Quaternion: {tgt_quat}")
pw.dlog(f"Control Values Error Quaternion: {err_quat}") pw.dlog(f"Control Values Error Quaternion: {err_quat}")
pw.dlog(f"Control Values Error Angle: {err_ang} [deg]") pw.dlog(f"Control Values Error Angle: {err_ang} [deg]")
pw.dlog(f"Control Values Target Rotational Rate: {tgt_rot} [deg/s]") pw.dlog(f"Control Values Target Rotational Rate: {tgt_rot} [deg/s]")
pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=4) pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=5)
def handle_act_cmd_data(pw: PrintWrapper, hk_data: bytes): def handle_act_cmd_data(pw: PrintWrapper, hk_data: bytes):