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

acs-ctrl-update #140

Merged
muellerr merged 13 commits from acs-ctrl-update into main 2023-02-28 11:49:14 +01:00
Conversation 0 Commits 13 Files Changed 2 +145 -2
1 changed files with 145 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6a30f6d0ac - Show all commits

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

@ -14,6 +14,7 @@ from tmtccmd.config.tmtc import (
OpCodeEntry,
)
from tmtccmd.tc import service_provider
from tmtccmd.tc.queue import DefaultPusQueueHelper
from tmtccmd.tc.pus_200_fsfw_mode import Mode, pack_mode_command
from tmtccmd.tc.decorator import ServiceProviderParams
from tmtccmd.tc.pus_3_fsfw_hk import (
@ -26,6 +27,20 @@ from tmtccmd.tc.pus_3_fsfw_hk import (
from tmtccmd.pus.s8_fsfw_funccmd import create_action_cmd
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
from tmtccmd.tc.pus_20_fsfw_param import (
create_load_param_cmd
)
from tmtccmd.pus.s20_fsfw_param_defs import (
create_scalar_u8_parameter,
create_scalar_u16_parameter,
create_scalar_u32_parameter,
create_scalar_float_parameter,
create_scalar_double_parameter,
create_vector_float_parameter,
create_vector_double_parameter,
)
class SetId(enum.IntEnum):
MGM_RAW_SET = 0
@ -65,6 +80,8 @@ class OpCodes:
INERTIAL = ["normal_inertial"]
SAFE_PTG = ["confirm_deployment"]
RESET_MEKF = ["reset_mekf"]
SET_PARAMETER_SCALAR = ["set_scalar_param"]
SET_PARAMETER_VECTOR = ["set_vector_param"]
REQUEST_RAW_MGM_HK = ["0", "mgm_raw_hk"]
ENABLE_RAW_MGM_HK = ["1", "mgm_raw_enable_hk"]
DISABLE_RAW_MGM_HK = ["2", "mgm_raw_disable_hk"]
@ -108,6 +125,9 @@ class Info:
INERTIAL = "Switch ACS CTRL normal - pointing inertial"
SAFE_PTG = "Confirm deployment of both solar arrays"
RESET_MEKF = "Reset the MEKF"
SET_PARAMETER_SCALAR = "Set Scalar Parameter"
SET_PARAMETER_VECTOR = "Set Vector Parameter"
SET_PARAMETER_MATRIX = "Set Matrix Parameter"
REQUEST_RAW_MGM_HK = "Request Raw MGM HK once"
ENABLE_RAW_MGM_HK = "Enable Raw MGM HK data generation"
DISABLE_RAW_MGM_HK = "Disable Raw MGM HK data generation"
@ -165,6 +185,8 @@ def acs_cmd_defs(defs: TmtcDefinitionWrapper):
oce.add(keys=OpCodes.INERTIAL, info=Info.INERTIAL)
oce.add(keys=OpCodes.SAFE_PTG, info=Info.SAFE_PTG)
oce.add(keys=OpCodes.RESET_MEKF, info=Info.RESET_MEKF)
oce.add(keys=OpCodes.SET_PARAMETER_SCALAR, info=Info.SET_PARAMETER_SCALAR)
oce.add(keys=OpCodes.SET_PARAMETER_VECTOR, info=Info.SET_PARAMETER_VECTOR)
oce.add(keys=OpCodes.REQUEST_RAW_MGM_HK, info=Info.REQUEST_RAW_MGM_HK)
oce.add(keys=OpCodes.ENABLE_RAW_MGM_HK, info=Info.ENABLE_RAW_MGM_HK)
oce.add(keys=OpCodes.DISABLE_RAW_MGM_HK, info=Info.DISABLE_RAW_MGM_HK)
@ -234,6 +256,12 @@ def pack_acs_ctrl_command(p: ServiceProviderParams):
elif op_code in OpCodes.RESET_MEKF:
q.add_log_cmd(f"{Info.RESET_MEKF}")
q.add_pus_tc(create_action_cmd(ACS_CONTROLLER, ActionId.RESET_MEKF))
elif op_code in OpCodes.SET_PARAMETER_SCALAR:
q.add_log_cmd(f"{Info.SET_PARAMETER_SCALAR}")
set_acs_ctrl_param_scalar(q)
elif op_code in OpCodes.SET_PARAMETER_VECTOR:
q.add_log_cmd(f"{Info.SET_PARAMETER_VECTOR}")
set_acs_ctrl_param_vector(q)
elif op_code in OpCodes.REQUEST_RAW_MGM_HK:
q.add_log_cmd(Info.REQUEST_RAW_MGM_HK)
q.add_pus_tc(
@ -426,6 +454,121 @@ def pack_acs_ctrl_command(p: ServiceProviderParams):
logging.getLogger(__name__).info(f"Unknown op code {op_code}")
def set_acs_ctrl_param_scalar(q: DefaultPusQueueHelper):
pt = int(input("Specify parameter type to set {0: \"uint8\", 1: \"uint16\", 2: \"uint32\", 3: \"float\", "
"4: \"double\"}: "))
sid = int(input("Specify parameter struct ID to set: "))
pid = int(input("Specify parameter ID to set: "))
match pt:
case 0:
param = int(input("Specify parameter value to set: "))
q.add_pus_tc(
create_load_param_cmd(
create_scalar_u8_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameter=param,
).pack()
)
)
case 1:
param = int(input("Specify parameter value to set: "))
q.add_pus_tc(
create_load_param_cmd(
create_scalar_u16_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameter=param,
).pack()
)
)
case 2:
param = int(input("Specify parameter value to set: "))
q.add_pus_tc(
create_load_param_cmd(
create_scalar_u32_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameter=param,
).pack()
)
)
case 3:
param = float(input("Specify parameter value to set: "))
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameter=param,
).pack()
)
)
case 4:
param = float(input("Specify parameter value to set: "))
q.add_pus_tc(
create_load_param_cmd(
create_scalar_double_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameter=param,
).pack()
)
)
def set_acs_ctrl_param_vector(q: DefaultPusQueueHelper):
pt = int(input("Specify parameter type to set {0: \"float\", 1: \"double\"}: "))
sid = int(input("Specify parameter struct ID to set: "))
pid = int(input("Specify parameter ID to set: "))
match pt:
case 0:
nr = int(input("Specify number of elements in vector to set: "))
param = []
for _ in range(nr):
param.append(float(input("Specify parameter vector entry value to set: ")))
print(param)
if bool(input("Confirm selected parameter values (Y/N): ")):
q.add_pus_tc(
create_load_param_cmd(
create_vector_float_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameters=param,
).pack()
)
)
else:
print("Aborting")
return
case 1:
nr = int(input("Specify number of elements in vector to set: "))
param = []
for _ in range(nr):
param.append(float(input("Specify parameter vector entry value to set: ")))
print(param)
if bool(input("Confirm selected parameter values (Y/N): ")):
q.add_pus_tc(
create_load_param_cmd(
create_vector_double_parameter(
object_id=ACS_CONTROLLER,
domain_id=sid,
unique_id=pid,
parameters=param,
).pack()
)
)
else:
print("Aborting")
return
def handle_acs_ctrl_hk_data(printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes):
pw = PrintWrapper(printer)
match set_id:
@ -724,7 +867,7 @@ def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
mekfStatus = {0 : "UNINITIALIZED", 1: "NO_GYR_DATA", 2: "NO_MODEL_VECTORS", 3: "NO_SUS_MGM_STR_DATA",
mekf_status = {0 : "UNINITIALIZED", 1: "NO_GYR_DATA", 2: "NO_MODEL_VECTORS", 3: "NO_SUS_MGM_STR_DATA",
4: "COVARIANCE_INVERSION_FAILED", 10: "INITIALIZED", 11: "RUNNING"}
pw.dlog("Received MEKF Set")
fmt_quat = "!dddd"
@ -745,7 +888,7 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
current_idx += inc_len_vec
status = struct.unpack(fmt_sts, hk_data[current_idx : current_idx + inc_len_sts])[0]
current_idx += inc_len_sts
pw.dlog(f"{'MEKF Status'.ljust(25)}: {mekfStatus[status]}")
pw.dlog(f"{'MEKF Status'.ljust(25)}: {mekf_status[status]}")
pw.dlog(f"{'MEKF Quaternion'.ljust(25)}: {fmt_str_4.format(*quat)}")
pw.dlog(f"{'MEKF Rotational Rate'.ljust(25)}: {fmt_str_3.format(*rate)}")
pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=3)