strat #184
@ -1027,17 +1027,32 @@ def handle_mekf_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")
|
||||
fmt_strat = "!B"
|
||||
fmt_quat = "!dddd"
|
||||
fmt_scalar = "!d"
|
||||
fmt_vec = "!ddd"
|
||||
inc_len_strat = struct.calcsize(fmt_strat)
|
||||
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 + 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")
|
||||
return
|
||||
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 = [
|
||||
f"{val:8.3f}"
|
||||
for val in struct.unpack(
|
||||
@ -1066,11 +1081,15 @@ 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]}")
|
||||
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 Error Quaternion: {err_quat}")
|
||||
pw.dlog(f"Control Values Error Angle: {err_ang} [deg]")
|
||||
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):
|
||||
|
Loading…
Reference in New Issue
Block a user