eive/eive-tmtc
eive/eive-tmtc
14
0
Fork 0
Code Issues Pull Requests 2 Projects Releases 97 Wiki Activity

new safe mode #221

Merged
muellerr merged 3 commits from new-safe into main 2023-07-26 09:51:27 +02:00
Conversation 0 Commits 3 Files Changed 1 +78 -7
1 changed files with 78 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,6 +59,7 @@ class SetId(enum.IntEnum):
MEKF_DATA = 7
CTRL_VAL_DATA = 8
ACTUATOR_CMD_DATA = 9
FUSED_ROT_RATE_DATA = 10
class ActionId(enum.IntEnum):
@ -112,6 +113,9 @@ class OpCodes:
REQUEST_ACT_CMD_HK = ["act_cmd_hk"]
ENABLE_ACT_CMD_HK = ["act_cmd_enable_hk"]
DISABLE_ACT_CMD_HK = ["act_cmd_disable_hk"]
REQUEST_FUSED_ROT_RATE_HK = ["f_rot_rate_hk"]
ENABLE_FUSED_ROT_RATE_HK = ["f_rot_rate_enable_hk"]
DISABLE_FUSED_ROT_RATE_HK = ["f_rot_rate_disable_hk"]
class Info:
@ -159,6 +163,9 @@ class Info:
REQUEST_ACT_CMD_HK = "Request Actuator Commands HK"
ENABLE_ACT_CMD_HK = "Enable Actuator Commands HK data generation"
DISABLE_ACT_CMD_HK = "Disable Actuator Commands HK data generation"
REQUEST_FUSED_ROT_RATE_HK = "Request Fused Rotational Rates HK"
ENABLE_FUSED_ROT_RATE_HK = "Enable Fused Rotational Rates HK data generation"
DISABLE_FUSED_ROT_RATE_HK = "Disable Fused Rotational Rates HK data generation"
PERFORM_MGM_CALIBRATION = False
@ -223,6 +230,9 @@ def acs_cmd_defs(defs: TmtcDefinitionWrapper):
oce.add(keys=OpCodes.REQUEST_ACT_CMD_HK, info=Info.REQUEST_ACT_CMD_HK)
oce.add(keys=OpCodes.ENABLE_ACT_CMD_HK, info=Info.ENABLE_ACT_CMD_HK)
oce.add(keys=OpCodes.DISABLE_ACT_CMD_HK, info=Info.DISABLE_ACT_CMD_HK)
oce.add(keys=OpCodes.REQUEST_FUSED_ROT_RATE_HK, info=Info.REQUEST_FUSED_ROT_RATE_HK)
oce.add(keys=OpCodes.ENABLE_FUSED_ROT_RATE_HK, info=Info.ENABLE_FUSED_ROT_RATE_HK)
oce.add(keys=OpCodes.DISABLE_FUSED_ROT_RATE_HK, info=Info.DISABLE_FUSED_ROT_RATE_HK)
defs.add_service(
name=CustomServiceList.ACS_CTRL.value, info="ACS Controller", op_code_entry=oce
)
@ -484,6 +494,26 @@ def pack_acs_ctrl_command(p: ServiceProviderParams): # noqa C901
False, make_sid(ACS_CONTROLLER, SetId.ACTUATOR_CMD_DATA)
)
)
elif op_code in OpCodes.REQUEST_FUSED_ROT_RATE_HK:
q.add_log_cmd(Info.REQUEST_FUSED_ROT_RATE_HK)
q.add_pus_tc(
generate_one_hk_command(make_sid(ACS_CONTROLLER, SetId.FUSED_ROT_RATE_DATA))
)
elif op_code in OpCodes.ENABLE_FUSED_ROT_RATE_HK:
interval = float(input("Please specify interval in floating point seconds: "))
q.add_log_cmd(Info.ENABLE_FUSED_ROT_RATE_HK)
cmd_tuple = enable_periodic_hk_command_with_interval(
False, make_sid(ACS_CONTROLLER, SetId.FUSED_ROT_RATE_DATA), interval
)
q.add_pus_tc(cmd_tuple[0])
q.add_pus_tc(cmd_tuple[1])
elif op_code in OpCodes.DISABLE_FUSED_ROT_RATE_HK:
q.add_log_cmd(Info.DISABLE_FUSED_ROT_RATE_HK)
q.add_pus_tc(
disable_periodic_hk_command(
False, make_sid(ACS_CONTROLLER, SetId.FUSED_ROT_RATE_DATA)
)
)
else:
logging.getLogger(__name__).info(f"Unknown op code {op_code}")
@ -699,6 +729,8 @@ def handle_acs_ctrl_hk_data(
handle_ctrl_val_data(pw, hk_data)
case SetId.ACTUATOR_CMD_DATA:
handle_act_cmd_data(pw, hk_data)
case SetId.FUSED_ROT_RATE_DATA:
handle_fused_rot_rate_data(pw, hk_data)
def handle_acs_ctrl_sus_raw_data(pw: PrintWrapper, hk_data: bytes):
@ -1026,16 +1058,20 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
safe_strat = {
ctrl_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",
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"
@ -1082,8 +1118,8 @@ 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]}")
if ctrl_strat.get(strat) is not None:
pw.dlog(f"{'Safe Ctrl Strategy'.ljust(25)}: {ctrl_strat[strat]}")
else:
pw.dlog(f"{'Safe Ctrl Strategy (key unknown)'.ljust(25)}: {strat}")
pw.dlog(f"Control Values Target Quaternion: {tgt_quat}")
@ -1132,6 +1168,41 @@ def handle_act_cmd_data(pw: PrintWrapper, hk_data: bytes):
FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=3)
def handle_fused_rot_rate_data(pw: PrintWrapper, hk_data: bytes):
pw.dlog("Received Fused Rotation Rates Data Set")
fmt_vec3_double = "!ddd"
inc_len_vec3_double = struct.calcsize(fmt_vec3_double)
if len(hk_data) < 3*inc_len_vec3_double:
pw.dlog("Received HK set too small")
return
current_idx = 0
rot_rate_orthogonal = [
f"{val*180/math.pi:8.3f}"
for val in struct.unpack(
fmt_vec3_double, hk_data[current_idx : current_idx + inc_len_vec3_double]
)
]
current_idx += inc_len_vec3_double
rot_rate_parallel = [
f"{val*180/math.pi:8.3f}"
for val in struct.unpack(
fmt_vec3_double, hk_data[current_idx : current_idx + inc_len_vec3_double]
)
]
current_idx += inc_len_vec3_double
rot_rate_total = [
f"{val*180/math.pi:8.3f}"
for val in struct.unpack(
fmt_vec3_double, hk_data[current_idx : current_idx + inc_len_vec3_double]
)
]
current_idx += inc_len_vec3_double
pw.dlog(f"Fused Rotational Rate Orthogonal: {rot_rate_orthogonal} [deg/s]")
pw.dlog(f"Fused Rotational Rate Parallel: {rot_rate_parallel} [deg/s]")
pw.dlog(f"Fused Rotational Rate Total: {rot_rate_total} [deg/s]")
FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=3)
def perform_mgm_calibration( # noqa C901: Complexity okay
pw: PrintWrapper, mgm_tuple: Tuple
): # noqa C901: Complexity okay