update CTRL STATE enum and make it global

This commit is contained in:
Robin Müller 2023-07-26 13:12:12 +02:00
parent 39d6ec73c2
commit 1e4e524f95
Signed by: muellerr
GPG Key ID: 407F9B00F858F270

View File

@ -68,6 +68,27 @@ class ActionId(enum.IntEnum):
RESTORE_MEKF_NONFINITE_RECOVERY = 2
CTRL_STRAT_DICT = {
0: "OFF",
1: "NO_MAG_FIELD_FOR_CONTROL",
2: "NO_SENSORS_FOR_CONTROL",
10: "LEGACY_SAFE_MEKF",
11: "LEGACY_WITHOUT_MEKF",
12: "LEGACY_ECLIPSE_DAMPING",
13: "LEGACY_ECLIPSE_IDELING",
14: "SAFE_MEKF",
15: "SAFE_GYR",
16: "SAFE_SUSMGM",
17: "SAFE_ECLIPSE_DAMPING_GYR",
18: "SAFE_ECLIPSE_DAMPING_SUSMGM",
19: "SAFE_ECLIPSE_IDELING",
20: "DETUMBLE_FULL",
21: "DETUMBLE_DETERIORATED",
30: "PTG_MEKF",
31: "PTG_RAW",
}
class OpCodes:
OFF = ["off"]
SAFE = ["safe"]
@ -1058,21 +1079,6 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
ctrl_strat = {
0: "OFF",
1: "NO_MAG_FIELD_FOR_CONTROL",
2: "NO_SENSORS_FOR_CONTROL",
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"
fmt_quat = "!dddd"
@ -1118,8 +1124,8 @@ def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
)
]
current_idx += inc_len_vec
if ctrl_strat.get(strat) is not None:
pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {ctrl_strat[strat]}")
if CTRL_STRAT_DICT.get(strat) is not None:
pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {CTRL_STRAT_DICT[strat]}")
else:
pw.dlog(f"{'Safe Ctrl Strategy (key unknown)'.ljust(25)}: {strat}")
pw.dlog(f"Control Values Target Quaternion: {tgt_quat}")