eive-tmtc/eive_tmtc/tmtc/acs/imtq.py

790 lines
30 KiB
Python
Raw Normal View History

2021-03-25 17:53:05 +01:00
# -*- coding: utf-8 -*-
"""
@file imtq.py
@brief Tests for the ISIS IMTQ (Magnettorquer) device handler
@author J. Meier
@date 25.03.2021
"""
2023-03-02 10:22:48 +01:00
import enum
2023-02-19 13:25:12 +01:00
import logging
2022-06-03 18:04:59 +02:00
import struct
2022-12-02 16:06:20 +01:00
from typing import List
2022-06-03 18:04:59 +02:00
2023-11-28 17:24:25 +01:00
from tmtccmd.config import CmdTreeNode
2022-11-29 16:53:29 +01:00
from eive_tmtc.config.definitions import CustomServiceList
from eive_tmtc.pus_tm.defs import PrintWrapper
2021-10-01 10:55:56 +02:00
from spacepackets.ecss.tc import PusTelecommand
2022-10-18 17:26:43 +02:00
from tmtccmd.config.tmtc import (
tmtc_definitions_provider,
OpCodeEntry,
TmtcDefinitionWrapper,
)
2023-11-10 19:23:06 +01:00
from tmtccmd.tmtc import DefaultPusQueueHelper
from tmtccmd.pus.tc.s3_fsfw_hk import (
2022-05-25 15:21:50 +02:00
make_sid,
generate_one_diag_command,
2023-03-02 14:41:23 +01:00
generate_one_hk_command,
create_request_one_diag_command,
2023-09-12 13:35:26 +02:00
create_enable_periodic_hk_command_with_interval_with_diag,
create_disable_periodic_hk_command_with_diag,
2022-05-25 15:21:50 +02:00
)
2023-11-10 19:23:06 +01:00
from tmtccmd.pus.s200_fsfw_mode import pack_mode_data, Mode
2022-07-08 16:25:46 +02:00
from tmtccmd.util import ObjectIdU32
2023-05-25 11:31:06 +02:00
from tmtccmd.fsfw.tmtc_printer import FsfwTmTcPrinter
2021-03-25 17:53:05 +01:00
2023-02-19 13:27:59 +01:00
_LOGGER = logging.getLogger(__name__)
2023-01-16 14:13:06 +01:00
class OpCode:
2023-02-19 12:39:25 +01:00
ON = "on"
NORMAL = "normal"
OFF = "off"
SET_DIPOLE = "set_dipole"
2023-03-04 17:05:11 +01:00
REQUEST_ENG_HK_NO_TORQUE = "hk_os_eng_hk_no_torque"
REQUEST_ENG_HK_WITH_TORQUE = "hk_os_eng_hk_with_torque"
REQUEST_MGM_RAW_NO_TORQUE = "hk_os_mgm_raw_no_torque"
2023-03-02 14:41:23 +01:00
ENABLE_MGM_RAW_NO_TORQUE = "enb_mgm_raw_no_torque"
DISABLE_MGM_RAW_NO_TORQUE = "dis_mgm_raw_no_torque"
REQUEST_MGM_RAW_WITH_TORQUE = "hk_os_mgm_raw_with_torque"
ENABLE_MGM_RAW_WITH_TORQUE = "enb_mgm_raw_with_torque"
DISABLE_MGM_RAW_WITH_TORQUE = "dis_mgm_raw_with_torque"
2023-03-02 14:41:23 +01:00
ENABLE_ENG_HK_NO_TORQUE = "enb_eng_hk_no_torque"
DISABLE_ENG_HK_NO_TORQUE = "dis_eng_hk_no_torque"
2023-03-04 17:05:11 +01:00
ENABLE_ENG_HK_WITH_TORQUE = "enb_eng_hk_with_torque"
DISABLE_ENG_HK_WITH_TORQUE = "dis_eng_hk_with_torque"
2023-02-19 12:39:25 +01:00
POS_X_SELF_TEST = "self_test_pos_x"
NEG_X_SELF_TEST = "self_test_neg_x"
POS_Y_SELF_TEST = "self_test_pos_y"
NEG_Y_SELF_TEST = "self_test_neg_y"
POS_Z_SELF_TEST = "self_test_pos_z"
NEG_Z_SELF_TEST = "self_test_neg_z"
2022-10-18 17:10:33 +02:00
2023-03-02 10:22:48 +01:00
class ImtqSetId(enum.IntEnum):
2023-02-19 03:48:30 +01:00
ENG_HK_NO_TORQUE = 1
RAW_MTM_NO_TORQUE = 2
ENG_HK_SET_WITH_TORQUE = 3
RAW_MTM_WITH_TORQUE = 4
STATUS_SET = 5
DIPOLES = 6
CAL_MTM_SET = 9
POSITIVE_X_TEST = 10
NEGATIVE_X_TEST = 11
POSITIVE_Y_TEST = 12
NEGATIVE_Y_TEST = 13
POSITIVE_Z_TEST = 14
NEGATIVE_Z_TEST = 15
2021-03-25 17:53:05 +01:00
2023-01-16 14:13:06 +01:00
class ImtqActionId:
2021-03-25 17:53:05 +01:00
start_actuation_dipole = bytearray([0x0, 0x0, 0x0, 0x02])
get_commanded_dipole = bytearray([0x0, 0x0, 0x0, 0x03])
2021-06-12 13:49:25 +02:00
perform_positive_x_test = bytearray([0x0, 0x0, 0x0, 0x07])
perform_negative_x_test = bytearray([0x0, 0x0, 0x0, 0x08])
perform_positive_y_test = bytearray([0x0, 0x0, 0x0, 0x09])
perform_negative_y_test = bytearray([0x0, 0x0, 0x0, 0x0A])
perform_positive_z_test = bytearray([0x0, 0x0, 0x0, 0x0B])
perform_negative_z_test = bytearray([0x0, 0x0, 0x0, 0x0C])
2022-07-04 15:22:53 +02:00
# Initiates the reading of the last performed self test. After sending this command the results
# can be downlinked via the housekeeping service by using the appropriate set ids listed above.
2021-06-12 13:49:25 +02:00
read_self_test_results = bytearray([0x0, 0x0, 0x0, 0x0D])
2021-03-25 17:53:05 +01:00
2023-11-28 17:24:25 +01:00
CTN = CmdTreeNode
def create_mgt_node() -> CmdTreeNode:
node = CmdTreeNode(
"mgt", "iMTQ MGT device node", hide_children_which_are_leaves=True
)
node.add_child(CmdTreeNode(OpCode.OFF, "Mode OFF"))
node.add_child(CmdTreeNode(OpCode.ON, "Mode ON"))
node.add_child(CmdTreeNode(OpCode.NORMAL, "Mode Normal"))
node.add_child(CmdTreeNode(OpCode.OFF, "Mode OFF"))
node.add_child(
CmdTreeNode(OpCode.REQUEST_ENG_HK_NO_TORQUE, "Request Engineering HK One Shot")
)
node.add_child(
CmdTreeNode(
OpCode.REQUEST_ENG_HK_WITH_TORQUE,
"Request Engineering HK One Shot during Torque",
)
)
node.add_child(
CmdTreeNode(OpCode.ENABLE_ENG_HK_NO_TORQUE, "Enable ENG HK not torque")
)
node.add_child(
CmdTreeNode(OpCode.ENABLE_ENG_HK_WITH_TORQUE, "Enable ENG HK with torque")
)
node.add_child(
CmdTreeNode(OpCode.DISABLE_ENG_HK_NO_TORQUE, "Disable ENG HK not torque")
)
node.add_child(
CmdTreeNode(OpCode.DISABLE_ENG_HK_WITH_TORQUE, "Disable ENG HK with torque")
)
node.add_child(
CmdTreeNode(
OpCode.REQUEST_MGM_RAW_NO_TORQUE,
"Request MGM Raw Without Torque HK One Shot",
)
)
node.add_child(
CmdTreeNode(OpCode.ENABLE_MGM_RAW_NO_TORQUE, "Enable MGM Raw Without Torque HK")
)
node.add_child(
CmdTreeNode(
OpCode.DISABLE_MGM_RAW_NO_TORQUE, "Disable MGM Raw Without Torque HK"
)
)
node.add_child(
CmdTreeNode(
OpCode.REQUEST_MGM_RAW_WITH_TORQUE,
"Request MGM Raw With Torque HK One Shot",
)
)
node.add_child(
CmdTreeNode(OpCode.ENABLE_MGM_RAW_WITH_TORQUE, "Enable MGM Raw With Torque HK")
)
node.add_child(
CTN(OpCode.DISABLE_MGM_RAW_WITH_TORQUE, "Disable MGM Raw With Torque HK")
)
node.add_child(CTN(OpCode.POS_X_SELF_TEST, "IMTQ perform pos X self test"))
node.add_child(CTN(OpCode.NEG_X_SELF_TEST, "IMTQ perform neg X self test"))
node.add_child(CTN(OpCode.POS_Y_SELF_TEST, "IMTQ perform pos Y self test"))
node.add_child(CTN(OpCode.NEG_Y_SELF_TEST, "IMTQ perform neg Y self test"))
node.add_child(CTN(OpCode.POS_Z_SELF_TEST, "IMTQ perform pos Z self test"))
node.add_child(CTN(OpCode.NEG_Z_SELF_TEST, "IMTQ perform neg Z self test"))
node.add_child(CTN(OpCode.SET_DIPOLE, "IMTQ command dipole"))
return node
2022-10-18 17:26:43 +02:00
@tmtc_definitions_provider
def add_imtq_cmds(defs: TmtcDefinitionWrapper):
oce = OpCodeEntry()
2023-01-16 14:13:06 +01:00
oce.add(OpCode.OFF, "Mode Off")
oce.add(OpCode.ON, "Mode On")
oce.add(OpCode.NORMAL, "Mode Normal")
2023-03-02 14:41:23 +01:00
oce.add(OpCode.REQUEST_ENG_HK_NO_TORQUE, "Request Engineering HK One Shot")
2023-03-04 17:05:11 +01:00
oce.add(
OpCode.REQUEST_ENG_HK_WITH_TORQUE,
"Request Engineering HK One Shot during Torque",
)
oce.add(OpCode.ENABLE_ENG_HK_NO_TORQUE, "Enable ENG HK not torque")
oce.add(OpCode.ENABLE_ENG_HK_WITH_TORQUE, "Enable ENG HK with torque")
oce.add(OpCode.DISABLE_ENG_HK_NO_TORQUE, "Disable ENG HK not torque")
oce.add(OpCode.DISABLE_ENG_HK_WITH_TORQUE, "Disable ENG HK with torque")
2023-03-03 16:38:17 +01:00
oce.add(
OpCode.REQUEST_MGM_RAW_NO_TORQUE, "Request MGM Raw Without Torque HK One Shot"
)
oce.add(OpCode.ENABLE_MGM_RAW_NO_TORQUE, "Enable MGM Raw Without Torque HK")
oce.add(OpCode.DISABLE_MGM_RAW_NO_TORQUE, "Disable MGM Raw Without Torque HK")
2023-03-03 16:38:17 +01:00
oce.add(
OpCode.REQUEST_MGM_RAW_WITH_TORQUE, "Request MGM Raw With Torque HK One Shot"
)
oce.add(OpCode.ENABLE_MGM_RAW_WITH_TORQUE, "Enable MGM Raw With Torque HK")
oce.add(OpCode.DISABLE_MGM_RAW_WITH_TORQUE, "Disable MGM Raw With Torque HK")
2023-02-19 12:39:25 +01:00
oce.add(OpCode.POS_X_SELF_TEST, "IMTQ perform pos X self test")
oce.add(OpCode.NEG_X_SELF_TEST, "IMTQ perform neg X self test")
oce.add(OpCode.POS_Y_SELF_TEST, "IMTQ perform pos Y self test")
oce.add(OpCode.NEG_Y_SELF_TEST, "IMTQ perform neg Y self test")
oce.add(OpCode.POS_Z_SELF_TEST, "IMTQ perform pos Z self test")
oce.add(OpCode.NEG_Z_SELF_TEST, "IMTQ perform neg Z self test")
2023-01-16 14:13:06 +01:00
oce.add(OpCode.SET_DIPOLE, "IMTQ command dipole")
2022-10-18 17:26:43 +02:00
oce.add("10", "IMTQ get commanded dipole")
oce.add("12", "IMTQ get calibrated MTM measurement one shot")
defs.add_service(CustomServiceList.IMTQ.value, "IMQT Device", oce)
2023-11-22 10:17:05 +01:00
def create_imtq_command( # noqa C901
object_id: ObjectIdU32, q: DefaultPusQueueHelper, cmd_str: str
2023-06-19 17:16:00 +02:00
):
2022-07-04 15:22:53 +02:00
q.add_log_cmd(
f"Testing ISIS IMTQ handler with object id: {object_id.as_hex_string}"
2021-03-25 17:53:05 +01:00
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.OFF:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Set mode off")
2023-01-16 15:05:33 +01:00
command = pack_mode_data(object_id.as_bytes, Mode.OFF, 0)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=command))
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.ON:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Set mode on")
2023-01-16 15:05:33 +01:00
command = pack_mode_data(object_id.as_bytes, Mode.ON, 0)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=command))
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.NORMAL:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Mode Normal")
2023-01-16 15:05:33 +01:00
command = pack_mode_data(object_id.as_bytes, Mode.NORMAL, 0)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=command))
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.POS_X_SELF_TEST:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Perform positive x self test")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.perform_positive_x_test
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-12 13:49:25 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Initiate reading of positive x self test results")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.read_self_test_results
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-12 13:49:25 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Request dataset with positive x self test results")
2023-01-16 14:13:06 +01:00
sid = make_sid(object_id.as_bytes, ImtqSetId.POSITIVE_X_TEST)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(generate_one_hk_command(sid))
2021-06-12 13:49:25 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.NEG_X_SELF_TEST:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Perform negative x self test")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.perform_negative_x_test
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
q.add_log_cmd("IMTQ: Initiate reading of negative x self test results")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.read_self_test_results
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
q.add_log_cmd("IMTQ: Request dataset with negative x self test results")
2023-01-16 14:13:06 +01:00
sid = make_sid(object_id.as_bytes, ImtqSetId.NEGATIVE_X_TEST)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(generate_one_hk_command(sid))
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.POS_Y_SELF_TEST:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Perform positive y self test")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.perform_positive_y_test
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
q.add_log_cmd("IMTQ: Initiate reading of positive y self test results")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.read_self_test_results
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-15 15:23:27 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Request dataset with positive y self test results")
2023-01-16 14:13:06 +01:00
sid = make_sid(object_id.as_bytes, ImtqSetId.POSITIVE_Y_TEST)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(generate_one_hk_command(sid))
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.NEG_Y_SELF_TEST:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Perform negative y self test")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.perform_negative_y_test
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-15 15:23:27 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Initiate reading of negative y self test results")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.read_self_test_results
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-15 15:23:27 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Request dataset with negative y self test results")
2023-01-16 14:13:06 +01:00
sid = make_sid(object_id.as_bytes, ImtqSetId.NEGATIVE_Y_TEST)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(generate_one_hk_command(sid))
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.POS_Z_SELF_TEST:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Perform positive z self test")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.perform_positive_z_test
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-15 15:23:27 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Initiate reading of positive z self test results")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.read_self_test_results
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-15 15:23:27 +02:00
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Request dataset with positive z self test results")
2023-01-16 14:13:06 +01:00
sid = make_sid(object_id.as_bytes, ImtqSetId.POSITIVE_Y_TEST)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(generate_one_hk_command(sid))
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.NEG_Z_SELF_TEST:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Perform negative z self test")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.perform_negative_z_test
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
q.add_log_cmd("IMTQ: Initiate reading of negative z self test results")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.read_self_test_results
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
q.add_log_cmd("IMTQ: Request dataset with negative z self test results")
2023-01-16 14:13:06 +01:00
sid = make_sid(object_id.as_bytes, ImtqSetId.NEGATIVE_Z_TEST)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(generate_one_hk_command(sid))
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str in OpCode.SET_DIPOLE:
2022-10-18 17:07:11 +02:00
x_dipole = int(input("Specify X dipole [range [0, 2000] * 10^-4 * Am^2]: "))
y_dipole = int(input("Specify Y dipole [range [0, 2000] * 10^-4 * Am^2]: "))
z_dipole = int(input("Specify Z dipole [range [0, 2000] * 10^-4 * Am^2]: "))
duration = int(
input(
f"Specify torque duration [range [0, {pow(2, 16) - 1}, "
2023-09-12 13:48:38 +02:00
"0 for continuous generation until update]: "
2022-10-18 17:07:11 +02:00
)
)
2022-10-18 17:26:43 +02:00
dur_str = "infinite" if duration == 0 else str(duration)
q.add_log_cmd(
2023-09-12 13:48:38 +02:00
f"IMTQ: Commanding dipole X={x_dipole}, Y={y_dipole}, Z={y_dipole},"
f" duration={dur_str}"
2022-10-18 17:26:43 +02:00
)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(
pack_dipole_command(
object_id.as_bytes, x_dipole, y_dipole, z_dipole, duration
)
)
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == "10": # doesnt seem to work anymore
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Get commanded dipole")
2023-01-16 14:13:06 +01:00
command = object_id.as_bytes + ImtqActionId.get_commanded_dipole
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
2021-06-15 15:23:27 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.ENABLE_ENG_HK_NO_TORQUE:
2023-03-02 14:41:23 +01:00
q.add_log_cmd("IMTQ: Enable ENG HK")
interval = float(input("Please enter collection interval in seconds: "))
2023-09-12 13:35:26 +02:00
cmds = create_enable_periodic_hk_command_with_interval_with_diag(
2023-03-02 14:41:23 +01:00
diag=True,
sid=make_sid(object_id.as_bytes, ImtqSetId.ENG_HK_NO_TORQUE),
interval_seconds=interval,
)
for cmd in cmds:
q.add_pus_tc(cmd)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.DISABLE_ENG_HK_NO_TORQUE:
2023-03-02 14:41:23 +01:00
q.add_log_cmd("IMTQ: Disable ENG HK (No Torque)")
q.add_pus_tc(
2023-09-12 13:35:26 +02:00
create_disable_periodic_hk_command_with_diag(
2023-03-02 14:41:23 +01:00
True, make_sid(object_id.as_bytes, ImtqSetId.ENG_HK_NO_TORQUE)
)
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.REQUEST_ENG_HK_WITH_TORQUE:
2023-03-04 17:05:11 +01:00
q.add_log_cmd("IMTQ: Get engineering hk set with torque")
q.add_pus_tc(
create_request_one_diag_command(
sid=make_sid(
object_id=object_id.as_bytes,
set_id=ImtqSetId.ENG_HK_SET_WITH_TORQUE,
)
)
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.ENABLE_ENG_HK_WITH_TORQUE:
2023-03-04 17:05:11 +01:00
q.add_log_cmd("IMTQ: Enable ENG HK with torque")
interval = float(input("Please enter collection interval in seconds: "))
2023-09-12 13:35:26 +02:00
cmds = create_enable_periodic_hk_command_with_interval_with_diag(
2023-03-04 17:05:11 +01:00
diag=True,
sid=make_sid(object_id.as_bytes, ImtqSetId.ENG_HK_SET_WITH_TORQUE),
interval_seconds=interval,
)
for cmd in cmds:
q.add_pus_tc(cmd)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.DISABLE_ENG_HK_WITH_TORQUE:
2023-03-04 17:05:11 +01:00
q.add_log_cmd("IMTQ: Disable ENG HK with Torque")
q.add_pus_tc(
2023-09-12 13:35:26 +02:00
create_disable_periodic_hk_command_with_diag(
2023-03-04 17:05:11 +01:00
True, make_sid(object_id.as_bytes, ImtqSetId.ENG_HK_SET_WITH_TORQUE)
)
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.REQUEST_ENG_HK_NO_TORQUE:
2023-03-04 17:05:11 +01:00
q.add_log_cmd("IMTQ: Get engineering hk set (no torque)")
2022-07-04 15:22:53 +02:00
q.add_pus_tc(
generate_one_diag_command(
2023-02-19 03:48:30 +01:00
sid=make_sid(
object_id=object_id.as_bytes, set_id=ImtqSetId.ENG_HK_NO_TORQUE
)
2022-07-04 15:22:53 +02:00
)
2022-05-24 18:57:52 +02:00
)
2023-11-22 10:17:05 +01:00
if cmd_str == "12":
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Get calibrated MTM hk set")
q.add_pus_tc(
2023-03-02 13:57:01 +01:00
create_request_one_diag_command(
2023-01-16 14:13:06 +01:00
sid=make_sid(object_id=object_id.as_bytes, set_id=ImtqSetId.CAL_MTM_SET)
2022-07-04 15:22:53 +02:00
)
2022-05-24 18:57:52 +02:00
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.REQUEST_MGM_RAW_NO_TORQUE:
2022-07-04 15:22:53 +02:00
q.add_log_cmd("IMTQ: Get raw MTM hk set")
q.add_pus_tc(
2023-03-02 13:57:01 +01:00
create_request_one_diag_command(
2023-02-19 03:48:30 +01:00
sid=make_sid(
object_id=object_id.as_bytes, set_id=ImtqSetId.RAW_MTM_NO_TORQUE
)
2022-07-04 15:22:53 +02:00
)
2022-05-24 18:57:52 +02:00
)
2021-06-12 13:49:25 +02:00
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.DISABLE_MGM_RAW_NO_TORQUE:
q.add_log_cmd("IMTQ: Disable MGM RAW HK (No Torque)")
q.add_pus_tc(
2023-09-12 13:35:26 +02:00
create_disable_periodic_hk_command_with_diag(
True, make_sid(object_id.as_bytes, ImtqSetId.RAW_MTM_NO_TORQUE)
)
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.ENABLE_MGM_RAW_NO_TORQUE:
q.add_log_cmd("IMTQ: Enable MGM RAW HK (No Torque)")
interval = float(input("Please enter collection interval in seconds: "))
2023-09-12 13:35:26 +02:00
cmds = create_enable_periodic_hk_command_with_interval_with_diag(
diag=True,
sid=make_sid(object_id.as_bytes, ImtqSetId.RAW_MTM_NO_TORQUE),
interval_seconds=interval,
)
for cmd in cmds:
q.add_pus_tc(cmd)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.REQUEST_MGM_RAW_WITH_TORQUE:
q.add_log_cmd("IMTQ: Get raw MTM hk set")
q.add_pus_tc(
create_request_one_diag_command(
sid=make_sid(
2023-03-03 09:57:26 +01:00
object_id=object_id.as_bytes, set_id=ImtqSetId.RAW_MTM_WITH_TORQUE
)
)
)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.ENABLE_MGM_RAW_WITH_TORQUE:
q.add_log_cmd("IMTQ: Enable MGM RAW HK (No Torque)")
interval = float(input("Please enter collection interval in seconds: "))
2023-09-12 13:35:26 +02:00
cmds = create_enable_periodic_hk_command_with_interval_with_diag(
diag=True,
sid=make_sid(object_id.as_bytes, ImtqSetId.RAW_MTM_WITH_TORQUE),
interval_seconds=interval,
)
for cmd in cmds:
q.add_pus_tc(cmd)
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.DISABLE_MGM_RAW_WITH_TORQUE:
q.add_log_cmd("IMTQ: Disable MGM RAW HK (No Torque)")
q.add_pus_tc(
2023-09-12 13:35:26 +02:00
create_disable_periodic_hk_command_with_diag(
True, make_sid(object_id.as_bytes, ImtqSetId.RAW_MTM_WITH_TORQUE)
)
)
2021-03-25 17:53:05 +01:00
2022-01-18 14:03:56 +01:00
def pack_dipole_command(
2022-07-04 15:22:53 +02:00
object_id: bytes, x_dipole: int, y_dipole: int, z_dipole: int, duration: int
) -> PusTelecommand:
2022-01-18 14:03:56 +01:00
"""This function packs the command causing the ISIS IMTQ to generate a dipole.
2022-10-18 17:07:11 +02:00
:param object_id: The object id of the IMTQ handler.
:param x_dipole: The dipole of the x coil in 10^-4*Am^2 (max. 2000)
:param y_dipole: The dipole of the y coil in 10^-4*Am^2 (max. 2000)
:param z_dipole: The dipole of the z coil in 10^-4*Am^2 (max. 2000)
:param duration: The duration in milliseconds the dipole will be generated by the coils.
2021-03-25 17:53:05 +01:00
When set to 0, the dipole will be generated until a new dipole actuation
command is sent.
"""
2023-01-16 14:13:06 +01:00
action_id = ImtqActionId.start_actuation_dipole
2021-03-25 17:53:05 +01:00
command = object_id + action_id
2022-10-18 17:07:11 +02:00
x_dipole = int(round(x_dipole))
y_dipole = int(round(y_dipole))
z_dipole = int(round(z_dipole))
if x_dipole < -2000 or x_dipole > 2000:
raise_dipole_error("X dipole", x_dipole)
if y_dipole < -2000 or y_dipole > 2000:
raise_dipole_error("Y dipole", y_dipole)
if z_dipole < -2000 or z_dipole > 2000:
raise_dipole_error("Z dipole", z_dipole)
duration = int(round(duration))
if duration < 0 or duration > pow(2, 16) - 1:
raise ValueError(
2023-09-12 13:48:38 +02:00
f"Duration in ms of {duration} smaller than 0 or larger than allowed"
f" {pow(2, 16) - 1}"
2022-10-18 17:07:11 +02:00
)
2022-07-04 15:22:53 +02:00
command += struct.pack("!h", x_dipole)
command += struct.pack("!h", y_dipole)
command += struct.pack("!h", z_dipole)
2022-10-18 17:07:11 +02:00
command += struct.pack("!H", duration)
2022-07-04 15:22:53 +02:00
command = PusTelecommand(service=8, subservice=128, app_data=command)
2021-03-26 13:56:02 +01:00
return command
2022-10-18 17:07:11 +02:00
def raise_dipole_error(dipole_str: str, value: int):
raise ValueError(
2023-09-12 13:48:38 +02:00
f"{dipole_str} {value} negative or larger than maximum allowed 2000 *"
" 10^-4*Am^2"
2022-10-18 17:07:11 +02:00
)
2022-10-20 14:22:19 +02:00
2023-02-19 03:48:30 +01:00
STATUS_HEADERS = [
2023-02-19 02:22:18 +01:00
"Status Byte Mode",
"Status Byte Error",
"Status Byte Config",
"Status Byte Uptime [s]",
2023-02-19 03:48:30 +01:00
]
ENG_HK_HEADERS = [
2022-12-02 16:06:20 +01:00
"Digital Voltage [mV]",
"Analog Voltage [mV]",
"Digital Current [mA]",
"Analog Current [mA]",
"Coil Current X [mA]",
"Coil Current Y [mA]",
"Coil Current Z [mA]",
"Coil X Temperature [°C]",
"Coil Y Temperature [°C]",
"Coil Z Temperature [°C]",
"MCU Temperature [°C]",
]
2023-05-23 09:54:51 +02:00
def handle_imtq_hk(pw: PrintWrapper, hk_data: bytes, set_id: int):
2023-03-03 09:56:16 +01:00
if (set_id >= ImtqSetId.POSITIVE_X_TEST) and (set_id <= ImtqSetId.NEGATIVE_Z_TEST):
2023-05-23 09:54:51 +02:00
return handle_self_test_data(pw, hk_data)
2023-02-19 13:25:12 +01:00
elif set_id == ImtqSetId.ENG_HK_NO_TORQUE:
2023-05-23 09:54:51 +02:00
return handle_eng_set(pw, hk_data, False)
2023-02-19 13:27:59 +01:00
elif set_id == ImtqSetId.ENG_HK_SET_WITH_TORQUE:
2023-06-09 18:53:11 +02:00
return handle_eng_set(pw, hk_data, True)
2023-02-19 13:25:12 +01:00
elif set_id == ImtqSetId.CAL_MTM_SET:
2023-06-09 18:53:11 +02:00
return handle_calibrated_mtm_measurement(pw, hk_data)
2023-02-19 13:25:12 +01:00
elif set_id == ImtqSetId.RAW_MTM_NO_TORQUE:
2023-06-09 18:53:11 +02:00
return handle_raw_mtm_measurement(pw, hk_data, False)
2023-02-19 13:27:59 +01:00
elif set_id == ImtqSetId.RAW_MTM_WITH_TORQUE:
2023-06-09 18:53:11 +02:00
return handle_raw_mtm_measurement(pw, hk_data, True)
2023-03-08 19:36:57 +01:00
elif set_id == ImtqSetId.DIPOLES:
2023-06-09 18:53:11 +02:00
return handle_dipole_set(pw, hk_data)
2023-02-19 13:25:12 +01:00
elif set_id == ImtqSetId.STATUS_SET:
2023-06-09 18:53:11 +02:00
return handle_status_set(pw, hk_data)
2023-02-19 13:25:12 +01:00
else:
2023-05-23 09:54:51 +02:00
pw.wlog(
_LOGGER,
f"IMTQ handler HK reply with unknown or unimplemented set id {set_id}",
2023-02-19 13:25:12 +01:00
)
2023-02-19 03:48:30 +01:00
def unpack_status_set(hk_data: bytes) -> List:
2023-02-19 02:22:18 +01:00
status_mode = hk_data[0]
status_error = hk_data[1]
status_conf = hk_data[2]
status_uptime = struct.unpack("!I", hk_data[3:7])[0]
2023-02-19 03:48:30 +01:00
return [status_mode, status_error, status_conf, status_uptime]
2023-06-09 18:53:11 +02:00
def handle_dipole_set(pw: PrintWrapper, hk_data: bytes):
2023-03-08 19:36:57 +01:00
pw.dlog("Received iMTQ dipole set")
fmt_str = "!hhhH"
fmt_len = struct.calcsize(fmt_str)
2023-03-08 19:57:28 +01:00
(dipole_x, dipole_y, dipole_z, current_torque_duration) = struct.unpack(
2023-03-09 17:50:40 +01:00
fmt_str, hk_data[0:8]
2023-03-08 19:57:28 +01:00
)
2023-03-08 19:36:57 +01:00
pw.dlog(f"Dipole X: {dipole_x}")
pw.dlog(f"Dipole Y: {dipole_y}")
pw.dlog(f"Dipole Z: {dipole_z}")
pw.dlog(f"Current torque duration: {current_torque_duration}")
2023-06-09 18:53:11 +02:00
FsfwTmTcPrinter.get_validity_buffer(hk_data[fmt_len:], 2)
2023-03-08 19:36:57 +01:00
2023-02-19 03:48:30 +01:00
def unpack_eng_hk(hk_data: bytes) -> List:
digital_voltage = struct.unpack("!H", hk_data[0:2])[0]
analog_voltage = struct.unpack("!H", hk_data[2:4])[0]
digital_current = struct.unpack("!f", hk_data[4:8])[0]
analog_current = struct.unpack("!f", hk_data[8:12])[0]
coil_x_current = struct.unpack("!f", hk_data[12:16])[0]
coil_y_current = struct.unpack("!f", hk_data[16:20])[0]
coil_z_current = struct.unpack("!f", hk_data[20:24])[0]
coil_x_temperature = struct.unpack("!h", hk_data[24:26])[0]
coil_y_temperature = struct.unpack("!h", hk_data[26:28])[0]
coil_z_temperature = struct.unpack("!h", hk_data[28:30])[0]
mcu_temperature = struct.unpack("!h", hk_data[30:32])[0]
2022-10-20 14:22:19 +02:00
content_list = [
digital_voltage,
analog_voltage,
digital_current,
analog_current,
coil_x_current,
coil_y_current,
coil_z_current,
coil_x_temperature,
coil_y_temperature,
coil_z_temperature,
mcu_temperature,
]
2022-12-02 16:06:20 +01:00
return content_list
2023-05-23 09:54:51 +02:00
def handle_eng_set(pw: PrintWrapper, hk_data: bytes, torque_on: bool):
2023-03-04 17:05:11 +01:00
pw.dlog(f"Found engineering HK. Torque Status: {torque_on}")
2022-12-02 16:06:20 +01:00
content_list = unpack_eng_hk(hk_data)
2023-02-19 03:48:30 +01:00
validity_buffer = hk_data[32:]
2022-12-02 16:06:20 +01:00
num_of_vars = len(ENG_HK_HEADERS)
2023-02-19 02:22:18 +01:00
for k, v in zip(ENG_HK_HEADERS, content_list):
pw.dlog(f"{k.ljust(30)}: {v}")
2023-05-23 09:54:51 +02:00
pw.dlog(
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=validity_buffer, num_vars=num_of_vars
)
)
2022-10-20 14:22:19 +02:00
2023-05-23 09:54:51 +02:00
def handle_status_set(pw: PrintWrapper, hk_data: bytes):
2023-02-19 03:48:30 +01:00
content_list = unpack_status_set(hk_data)
validity_buffer = hk_data[7:]
num_of_vars = 4
for k, v in zip(STATUS_HEADERS, content_list):
pw.dlog(f"{k.ljust(30)}: {v}")
2023-05-23 09:54:51 +02:00
pw.dlog(
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=validity_buffer, num_vars=num_of_vars
)
)
2023-02-19 03:48:30 +01:00
2023-05-23 09:54:51 +02:00
def handle_calibrated_mtm_measurement(pw: PrintWrapper, hk_data: bytes):
2022-10-20 14:22:19 +02:00
header_list = [
"Calibrated MTM X [nT]",
"Calibrated MTM Y [nT]",
"Calibrated MTM Z [nT]",
"Coil actuation status",
]
mtm_x = struct.unpack("!I", hk_data[0:4])[0]
mtm_y = struct.unpack("!I", hk_data[4:8])[0]
mtm_z = struct.unpack("!I", hk_data[8:12])[0]
coil_actuation_status = hk_data[12]
validity_buffer = hk_data[12:]
content_list = [mtm_x, mtm_y, mtm_z, coil_actuation_status]
num_of_vars = len(header_list)
pw.dlog(str(header_list))
pw.dlog(str(content_list))
2023-05-23 09:54:51 +02:00
pw.dlog(
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=validity_buffer, num_vars=num_of_vars
)
)
2022-10-20 14:22:19 +02:00
2023-05-23 09:54:51 +02:00
def handle_raw_mtm_measurement(pw: PrintWrapper, hk_data: bytes, torque_status: bool):
2023-03-04 17:05:11 +01:00
pw.dlog(f"Found raw MTM measurement. Torque Status: {torque_status}")
2022-10-20 14:22:19 +02:00
header_list = [
"Raw MTM X [nT]",
"Raw MTM Y [nT]",
"Raw MTM Z [nT]",
"Coil actuation status",
]
mtm_x = struct.unpack("!f", hk_data[0:4])[0]
mtm_y = struct.unpack("!f", hk_data[4:8])[0]
mtm_z = struct.unpack("!f", hk_data[8:12])[0]
coil_actuation_status = hk_data[12]
validity_buffer = hk_data[13:]
content_list = [mtm_x, mtm_y, mtm_z, coil_actuation_status]
num_of_vars = 2
pw.dlog(str(header_list))
pw.dlog(str(content_list))
2023-05-23 09:54:51 +02:00
pw.dlog(
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=validity_buffer, num_vars=num_of_vars
)
)
2022-10-20 14:22:19 +02:00
2023-05-23 09:54:51 +02:00
def handle_self_test_data(pw: PrintWrapper, hk_data: bytes):
2022-10-20 14:22:19 +02:00
header_list = [
"Init Err",
"Init Raw Mag X [nT]",
"Init Raw Mag Y [nT]",
"Init Raw Mag Z [nT]",
"Init Cal Mag X [nT]",
"Init Cal Mag Y [nT]",
"Init Cal Mag Z [nT]",
"Init Coil X Current [mA]",
"Init Coil Y Current [mA]",
"Init Coil Z Current [mA]",
"Init Coil X Temperature [°C]",
"Init Coil Y Temperature [°C]",
"Init Coil Z Temperature [°C]",
"Err",
"Raw Mag X [nT]",
"Raw Mag Y [nT]",
"Raw Mag Z [nT]",
"Cal Mag X [nT]",
"Cal Mag Y [nT]",
"Cal Mag Z [nT]",
"Coil X Current [mA]",
"Coil Y Current [mA]",
"Coil Z Current [mA]",
"Coil X Temperature [°C]",
"Coil Y Temperature [°C]",
"Coil Z Temperature [°C]",
"Fina Err",
"Fina Raw Mag X [nT]",
"Fina Raw Mag Y [nT]",
"Fina Raw Mag Z [nT]",
"Fina Cal Mag X [nT]",
"Fina Cal Mag Y [nT]",
"Fina Cal Mag Z [nT]",
"Fina Coil X Current [mA]",
"Fina Coil Y Current [mA]",
"Fina Coil Z Current [mA]",
"Fina Coil X Temperature [°C]",
"Fina Coil Y Temperature [°C]",
"Fina Coil Z Temperature [°C]",
]
# INIT step (no coil actuation)
init_err = hk_data[0]
init_raw_mag_x = struct.unpack("!f", hk_data[1:5])[0]
init_raw_mag_y = struct.unpack("!f", hk_data[5:9])[0]
init_raw_mag_z = struct.unpack("!f", hk_data[9:13])[0]
init_cal_mag_x = struct.unpack("!f", hk_data[13:17])[0]
init_cal_mag_y = struct.unpack("!f", hk_data[17:21])[0]
init_cal_mag_z = struct.unpack("!f", hk_data[21:25])[0]
init_coil_x_current = struct.unpack("!f", hk_data[25:29])[0]
init_coil_y_current = struct.unpack("!f", hk_data[29:33])[0]
init_coil_z_current = struct.unpack("!f", hk_data[33:37])[0]
init_coil_x_temperature = struct.unpack("!H", hk_data[37:39])[0]
init_coil_y_temperature = struct.unpack("!H", hk_data[39:41])[0]
init_coil_z_temperature = struct.unpack("!H", hk_data[41:43])[0]
# Actuation step
err = hk_data[43]
raw_mag_x = struct.unpack("!f", hk_data[44:48])[0]
raw_mag_y = struct.unpack("!f", hk_data[48:52])[0]
raw_mag_z = struct.unpack("!f", hk_data[52:56])[0]
cal_mag_x = struct.unpack("!f", hk_data[56:60])[0]
cal_mag_y = struct.unpack("!f", hk_data[60:64])[0]
cal_mag_z = struct.unpack("!f", hk_data[64:68])[0]
coil_x_current = struct.unpack("!f", hk_data[68:72])[0]
coil_y_current = struct.unpack("!f", hk_data[72:76])[0]
coil_z_current = struct.unpack("!f", hk_data[76:80])[0]
coil_x_temperature = struct.unpack("!H", hk_data[80:82])[0]
coil_y_temperature = struct.unpack("!H", hk_data[82:84])[0]
coil_z_temperature = struct.unpack("!H", hk_data[84:86])[0]
# FINA step (no coil actuation)
fina_err = hk_data[86]
fina_raw_mag_x = struct.unpack("!f", hk_data[87:91])[0]
fina_raw_mag_y = struct.unpack("!f", hk_data[91:95])[0]
fina_raw_mag_z = struct.unpack("!f", hk_data[95:99])[0]
fina_cal_mag_x = struct.unpack("!f", hk_data[99:103])[0]
fina_cal_mag_y = struct.unpack("!f", hk_data[103:107])[0]
fina_cal_mag_z = struct.unpack("!f", hk_data[107:111])[0]
fina_coil_x_current = struct.unpack("!f", hk_data[111:115])[0]
fina_coil_y_current = struct.unpack("!f", hk_data[115:119])[0]
fina_coil_z_current = struct.unpack("!f", hk_data[119:123])[0]
fina_coil_x_temperature = struct.unpack("!H", hk_data[123:125])[0]
fina_coil_y_temperature = struct.unpack("!H", hk_data[125:127])[0]
fina_coil_z_temperature = struct.unpack("!H", hk_data[127:129])[0]
validity_buffer = hk_data[129:]
content_list = [
init_err,
init_raw_mag_x,
init_raw_mag_y,
init_raw_mag_z,
init_cal_mag_x,
init_cal_mag_y,
init_cal_mag_z,
init_coil_x_current,
init_coil_y_current,
init_coil_z_current,
init_coil_x_temperature,
init_coil_y_temperature,
init_coil_z_temperature,
err,
raw_mag_x,
2023-06-19 17:16:00 +02:00
raw_mag_y,
2022-10-20 14:22:19 +02:00
raw_mag_z,
cal_mag_x,
cal_mag_y,
cal_mag_z,
coil_x_current,
coil_y_current,
coil_z_current,
coil_x_temperature,
coil_y_temperature,
coil_z_temperature,
fina_err,
fina_raw_mag_x,
fina_raw_mag_y,
fina_raw_mag_z,
fina_cal_mag_x,
fina_cal_mag_y,
fina_cal_mag_z,
fina_coil_x_current,
fina_coil_y_current,
fina_coil_z_current,
fina_coil_x_temperature,
fina_coil_y_temperature,
fina_coil_z_temperature,
]
num_of_vars = len(header_list)
pw.dlog(str(header_list))
pw.dlog(str(content_list))
2023-05-23 09:54:51 +02:00
pw.dlog(
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=validity_buffer, num_vars=num_of_vars
)
)