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

init
All checks were successful
EIVE/-/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-07-21 11:20:20 +02:00
parent 83e8fe0587
commit 09b7a01ff6
1 changed files with 67 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
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):
@ -1132,6 +1164,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]")
pw.printer.print_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