unpack and use packet DT

This commit is contained in:
2023-03-14 18:09:52 +01:00
parent f0c14e21e4
commit d45c6bce97
2 changed files with 26 additions and 8 deletions

View File

@ -1,3 +1,4 @@
import datetime
import enum
import logging
import socket
@ -44,6 +45,9 @@ from tmtccmd.pus.s20_fsfw_param_defs import (
)
_LOGGER = logging.getLogger(__name__)
class SetId(enum.IntEnum):
MGM_RAW_SET = 0
MGM_PROC_SET = 1
@ -655,8 +659,14 @@ def set_acs_ctrl_param_matrix(q: DefaultPusQueueHelper):
return
def handle_acs_ctrl_hk_data(printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes):
def handle_acs_ctrl_hk_data(
printer: FsfwTmTcPrinter,
set_id: int,
hk_data: bytes,
packet_time: datetime.datetime,
):
pw = PrintWrapper(printer)
pw.ilog(_LOGGER, f"Received ACS CTRL HK with packet time {packet_time}")
match set_id:
case SetId.MGM_RAW_SET:
handle_raw_mgm_data(pw, hk_data)
@ -985,17 +995,18 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
current_idx = 0
quat = struct.unpack(fmt_quat, hk_data[current_idx : current_idx + inc_len_quat])
current_idx += inc_len_quat
rate = (
struct.unpack(fmt_vec, hk_data[current_idx : current_idx + inc_len_vec])
* 180
/ math.pi
)
rates = [
rate * 180 / math.pi
for rate in struct.unpack(
fmt_vec, hk_data[current_idx : current_idx + inc_len_vec]
)
]
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)}: {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.dlog(f"{'MEKF Rotational Rate'.ljust(25)}: {fmt_str_3.format(*rates)}")
pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=3)