eive-tmtc/pus_tc/system/proc.py

from tmtccmd.config import QueueCommands
from tmtccmd.tc.definitions import TcQueueT
from tmtccmd.tc.pus_3_fsfw_hk import *

"""
from config.object_ids import (
    BPX_HANDLER_ID,
    P60_DOCK_HANDLER,
    PDU_1_HANDLER_ID,
    PDU_2_HANDLER_ID,
    ACU_HANDLER_ID,
    CORE_CONTROLLER_ID,
)
"""
import config.object_ids as oids
from pus_tc.devs.bpx_batt import BpxSetIds
from pus_tc.system.core import SetIds as CoreSetIds
from gomspace.gomspace_common import SetIds as GsSetIds
from pus_tc.devs.rad_sensor import SetIds as RadSetIds
from pus_tc.devs.mgms import MgmLis3SetIds as MgmSetIds_0_2
from pus_tc.devs.mgms import MgmRm3100SetIds as MgmSetIds_1_3
from pus_tc.devs.gyros import AdisGyroSetIds as GyroSetIds_0_2
from pus_tc.devs.gyros import L3gGyroSetIds as GyroSetIds_1_3
from pus_tc.devs.gps import Gnss0SetIds as GnssSetIds_0
from pus_tc.devs.gps import Gnss1SetIds as GnssSetIds_1
from pus_tc.devs.imtq import ImtqSetIds
from pus_tc.devs.sus import Sus0SetIds
from pus_tc.devs.sus import Sus1SetIds
from pus_tc.devs.sus import Sus2SetIds
from pus_tc.devs.sus import Sus3SetIds
from pus_tc.devs.sus import Sus4SetIds
from pus_tc.devs.sus import Sus5SetIds
from pus_tc.devs.sus import Sus6SetIds
from pus_tc.devs.sus import Sus7SetIds
from pus_tc.devs.sus import Sus8SetIds
from pus_tc.devs.sus import Sus9SetIds
from pus_tc.devs.sus import Sus10SetIds
from pus_tc.devs.sus import Sus11SetIds
from pus_tc.devs.star_tracker import SetIds as StrSetIds
from pus_tc.devs.reaction_wheels import RwSetIds

from pus_tc.system.tcs import pack_tcs_sys_commands
from pus_tc.system.controllers import pack_controller_commands
from pus_tc.system.acs import pack_acs_command, pack_sus_cmds
from pus_tc.devs.imtq import pack_imtq_test_into, pack_dipole_command
from pus_tc.devs.star_tracker import pack_star_tracker_commands
from pus_tc.devs.reaction_wheels import pack_rw_ass_cmds, pack_set_speed_command



class OpCodes:
    HEATER = ["0", "heater"]
    BAT_FT = ["bat-ft"]
    CORE_FT = ["core-ft"]
    PCDU_FT = ["pcdu-ft"]
    RAD_SEN_FT = ["rad-sen-ft"]
    TCS_FT_ON = ["tcs-ft-on"]
    TCS_FT_OFF = ["tcs-ft-off"]
    ACS_FT = ["acs-ft"]
    MGT_FT = ["mgt-ft"]
    MGT_FT_DP = ["mgt-ft-dp"]
    SUS_FT = ["sus-ft"]
    STR_FT = ["str-ft"]
    RW_FT_ONE_RW = ["rw-ft-one-rw"]
    RW_FT_TWO_RWS = ["rw-ft-two-rws"]


class KeyAndInfo:
    HEATER = ["Heater", "heater procedure"]
    BAT_FT = ["BPX Battery", "battery functional test"]
    CORE_FT = ["OBC", "OBC functional test"]
    PCDU_FT = ["PCDU", "PCDU functional test"]
    RAD_SEN_FT = ["Radiation Sensor", "Radiation Sensor functional test"]
    TCS_FT_ON = ["TCS Act.", "TCS functional test activation"]
    TCS_FT_OFF = ["TCS Deact.", "TCS functional test deactivation"]
    ACS_FT = ["ACS", "ACS functional test"]
    MGT_FT = ["MGT", "MGT functional test"]
    MGT_FT_DP = ["MGT dipole", "MGT functional test with dipole"]
    SUS_FT = ["SUS", "SUS functional test"]
    STR_FT = ["STR", "STR functional test"]
    RW_FT_ONE_RW = ["RW ONE RW", "RW functional test with one RW"]
    RW_FT_TWO_RWS = ["RW TWO RWS", "RW functional test with two RWs"]


KAI = KeyAndInfo

PROC_INFO_DICT = {
    KAI.BAT_FT[0]: [OpCodes.BAT_FT, KAI.BAT_FT[1], 120.0, 10.0],
    KAI.CORE_FT[0]: [OpCodes.CORE_FT, KAI.CORE_FT[1], 120.0, 10.0],
    KAI.PCDU_FT[0]: [OpCodes.PCDU_FT, KAI.PCDU_FT[1], 120.0, 10.0],
    KAI.RAD_SEN_FT[0]: [OpCodes.RAD_SEN_FT, KAI.RAD_SEN_FT[1], 120.0, 10.0],
    KAI.TCS_FT_ON[0]: [OpCodes.TCS_FT_ON, KAI.TCS_FT_ON[1], 120.0, 10.0],
    KAI.TCS_FT_OFF[0]: [OpCodes.TCS_FT_OFF, KAI.TCS_FT_OFF[1], 120.0, 10.0],
    KAI.ACS_FT[0]: [OpCodes.ACS_FT, KAI.ACS_FT[1], 120.0, 10.0],
    KAI.MGT_FT[0]: [OpCodes.MGT_FT, KAI.MGT_FT[1], 120.0, 10.0],
# collection_time for KAI.MGT_FT_DP maybe be reduced as a full 120seconds is not needed after MGTs are tested
    KAI.MGT_FT_DP[0]: [OpCodes.MGT_FT_DP, KAI.MGT_FT_DP[1], 10.0, 10.0],
    KAI.SUS_FT[0]: [OpCodes.SUS_FT, KAI.SUS_FT[1], 120.0, 10.0],
    KAI.STR_FT[0]: [OpCodes.STR_FT, KAI.STR_FT[1], 120.0, 10.0],
# collection_time for KAI.RW_FT_ONE_RW maybe be reduced as a full 120seconds is not needed after RWs are tested
    KAI.RW_FT_ONE_RW[0]: [OpCodes.RW_FT_ONE_RW, KAI.RW_FT_ONE_RW[1], 10.0, 10.0],
# collection_time for KAI.RW_FT_ONE_RW maybe be reduced as a full 120seconds is not needed after RWs are tested
    KAI.RW_FT_ONE_RW[0]: [OpCodes.RW_FT_ONE_RW, KAI.RW_FT_ONE_RW[1], 10.0, 10.0],
}


def generic_print(tc_queue: TcQueueT, info: dict):
    tc_queue.appendleft(
        (QueueCommands.PRINT, f"Executing {info[1]} Procedure (OpCodes: {info[0]})")
    )


def pack_generic_hk_listening_cmds(
    tc_queue: TcQueueT,
    proc_key: str,
    sid_list: list[bytearray],
    diag: bool,
    mgt: bool,
):
    info = PROC_INFO_DICT[proc_key]
    collection_time = info[2]
    generic_print(tc_queue=tc_queue, info=info)

    for sid in sid_list:
        enable_listen_to_hk_for_x_seconds(
            diag=diag,
            tc_queue=tc_queue,
            device=proc_key,
            sid=sid,
            interval_seconds=info[3],
        )

    if mgt is True:
        activate_mgts_alternately(
            tc_queue=tc_queue,
        )

    elif one_rw is True:
        activate_one_rw_in_sequence(
            tc_queue=tc_queue,
        )

    elif two_rws is True:
        activate_two_rws_in_sequence(
            tc_queue=tc_queue,
        )

    else:
        pass

    tc_queue.appendleft((QueueCommands.WAIT, collection_time))

    for sid in sid_list:
        disable_listen_to_hk_for_x_seconds(
            diag=diag,
            tc_queue=tc_queue,
            device=proc_key,
            sid=sid,
        )

    sid_list.clear()


def pack_proc_commands(tc_queue: TcQueueT, op_code: str):
    sid_list = []
    if op_code in OpCodes.BAT_FT:
        key = KAI.BAT_FT[0]
        sid_list.append(make_sid(oids.BPX_HANDLER_ID, BpxSetIds.GET_HK_SET))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False,
        )

    if op_code in OpCodes.CORE_FT:
        key = KAI.CORE_FT[0]
        sid_list.append(make_sid(oids.CORE_CONTROLLER_ID, CoreSetIds.HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

    if op_code in OpCodes.PCDU_FT:
        key = KAI.PCDU_FT[0]
        sid_list.append(make_sid(oids.P60_DOCK_HANDLER, GsSetIds.P60_CORE))
        sid_list.append(make_sid(oids.PDU_1_HANDLER_ID, GsSetIds.PDU_1_CORE))
        sid_list.append(make_sid(oids.PDU_2_HANDLER_ID, GsSetIds.PDU_2_CORE))
        sid_list.append(make_sid(oids.ACU_HANDLER_ID, GsSetIds.ACU_CORE))
        sid_list.append(make_sid(oids.P60_DOCK_HANDLER, GsSetIds.P60_AUX))
        sid_list.append(make_sid(oids.PDU_1_HANDLER_ID, GsSetIds.PDU_1_AUX))
        sid_list.append(make_sid(oids.PDU_2_HANDLER_ID, GsSetIds.PDU_2_AUX))
        sid_list.append(make_sid(oids.ACU_HANDLER_ID, GsSetIds.ACU_AUX))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

    if op_code in OpCodes.RAD_SEN_FT:
        key = KAI.RAD_SEN_FT[0]
        sid_list.append(make_sid(oids.RAD_SENSOR_ID, RadSetIds.RAD_SEN_CORE))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

    if op_code in OpCodes.TCS_FT_ON:
        # check whether tcs_assembly also has to be commanded to NORMAL Mode
        # pack_tcs_sys_commands(tc_queue=tc_queue, op_code="tcs-normal")
        pack_controller_commands(tc_queue=tc_queue, op_code="thermal_controller")

    if op_code in OpCodes.TCS_FT_OFF:
        # check whether tcs_assembly also has to be commanded to OFF Mode
        # pack_tcs_sys_commands(tc_queue=tc_queue, op_code="tcs-off")
        pack_controller_commands(tc_queue=tc_queue, op_code="thermal_controller")

    if op_code in OpCodes.ACS_FT:
        key = KAI.ACS_FT[0]

        pack_acs_command(tc_queue=tc_queue, op_code="acs-a")

        # MGMs
        sid_list.append(make_sid(oids.MGM_0_LIS3_HANDLER_ID, MgmSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.MGM_1_RM3100_HANDLER_ID, MgmSetIds_1_3.CORE_HK))
        # Gyros
        sid_list.append(make_sid(oids.GYRO_0_ADIS_HANDLER_ID, GyroSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.GYRO_0_ADIS_HANDLER_ID, GyroSetIds_0_2.CFG_HK))
        sid_list.append(make_sid(oids.GYRO_1_L3G_HANDLER_ID, GyroSetIds_1_3.CORE_HK))
        # GNSS0
        sid_list.append(make_sid(oids.GPS_HANDLER_0_ID, GnssSetIds_0.CORE_HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_acs_command(tc_queue=tc_queue, op_code="acs-off")
        tc_queue.appendleft((QueueCommands.WAIT, 5.0))
        pack_acs_command(tc_queue=tc_queue, op_code="acs-b")

        # MGMs
        sid_list.append(make_sid(oids.MGM_2_LIS3_HANDLER_ID, MgmSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.MGM_3_RM3100_HANDLER_ID, MgmSetIds_1_3.CORE_HK))
        # Gyros
        sid_list.append(make_sid(oids.GYRO_2_ADIS_HANDLER_ID, GyroSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.GYRO_2_ADIS_HANDLER_ID, GyroSetIds_0_2.CFG_HK))
        sid_list.append(make_sid(oids.GYRO_3_L3G_HANDLER_ID, GyroSetIds_1_3.CORE_HK))
        # GNSS1
        sid_list.append(make_sid(oids.GPS_HANDLER_1_ID, GnssSetIds_1.CORE_HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_acs_command(tc_queue=tc_queue, op_code="acs-off")
        tc_queue.appendleft((QueueCommands.WAIT, 5.0))
        pack_acs_command(tc_queue=tc_queue, op_code="acs-d")

        # MGMs
        sid_list.append(make_sid(oids.MGM_0_LIS3_HANDLER_ID, MgmSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.MGM_1_RM3100_HANDLER_ID, MgmSetIds_1_3.CORE_HK))
        sid_list.append(make_sid(oids.MGM_2_LIS3_HANDLER_ID, MgmSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.MGM_3_RM3100_HANDLER_ID, MgmSetIds_1_3.CORE_HK))
        # Gyros
        sid_list.append(make_sid(oids.GYRO_0_ADIS_HANDLER_ID, GyroSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.GYRO_0_ADIS_HANDLER_ID, GyroSetIds_0_2.CFG_HK))
        sid_list.append(make_sid(oids.GYRO_1_L3G_HANDLER_ID, GyroSetIds_1_3.CORE_HK))
        sid_list.append(make_sid(oids.GYRO_2_ADIS_HANDLER_ID, GyroSetIds_0_2.CORE_HK))
        sid_list.append(make_sid(oids.GYRO_2_ADIS_HANDLER_ID, GyroSetIds_0_2.CFG_HK))
        sid_list.append(make_sid(oids.GYRO_3_L3G_HANDLER_ID, GyroSetIds_1_3.CORE_HK))
        # GNSS0+1
        sid_list.append(make_sid(oids.GPS_HANDLER_0_ID, GnssSetIds_0.CORE_HK))
        sid_list.append(make_sid(oids.GPS_HANDLER_1_ID, GnssSetIds_1.CORE_HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_acs_command(tc_queue=tc_queue, op_code="acs-off")

    if op_code in OpCodes.MGT_FT:
        key = KAI.MGT_FT[0]

# mgt 1: imtq und hk
        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, tc_queue=tc_queue, op_code="2")

        sid_list.append(make_sid(oids.IMTQ_HANDLER_ID, ImtqSetIds.ENG_HK_SET))
        sid_list.append(make_sid(oids.IMTQ_HANDLER_ID, ImtqSetIds.CAL_MTM_SET))
        sid_list.append(make_sid(oids.IMTQ_HANDLER_ID, ImtqSetIds.RAW_MTM_SET))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, tc_queue=tc_queue, op_code="0")

    if op_code in OpCodes.MGT_FT_DP:
        key = KAI.MGT_FT_DP[0]

        pack_acs_command(tc_queue=tc_queue, op_code="acs-d")
# mgt 2.: imtq + dual side + dipole
        pack_imtq_test_into(object_id=oids.IMTQ_HANDLER_ID, tc_queue=tc_queue, op_code="2")

        sid_list.append(make_sid(oids.IMTQ_HANDLER_ID, ImtqSetIds.ENG_HK_SET))
        sid_list.append(make_sid(oids.IMTQ_HANDLER_ID, ImtqSetIds.CAL_MTM_SET))
        sid_list.append(make_sid(oids.IMTQ_HANDLER_ID, ImtqSetIds.RAW_MTM_SET))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=True
        )

        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, tc_queue=tc_queue, op_code="0")
        pack_acs_command(tc_queue=tc_queue, op_code="acs-off")

    if op_code in OpCodes.SUS_FT:
        key = KAI.SUS_FT[0]

        pack_sus_cmds(tc_queue=tc_queue, op_code="sus-nom")

        # SUSs
        sid_list.append(make_sid(oids.SUS_0_N_LOC_XFYFZM_PT_XF, Sus0SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_1_N_LOC_XBYFZM_PT_XB, Sus1SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_2_N_LOC_XFYBZB_PT_YB, Sus2SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_3_N_LOC_XFYBZF_PT_YF, Sus3SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_4_N_LOC_XMYFZF_PT_ZF, Sus4SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_5_N_LOC_XFYMZB_PT_ZB, Sus5SetIds.CORE_HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_acs_command(tc_queue=tc_queue, op_code="sus-off")
        tc_queue.appendleft((QueueCommands.WAIT, 5.0))
        pack_sus_cmds(tc_queue=tc_queue, op_code="sus-red")

        # SUSs
        sid_list.append(make_sid(oids.SUS_6_R_LOC_XFYBZM_PT_XF, Sus6SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_7_R_LOC_XBYBZM_PT_XB, Sus7SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_8_R_LOC_XBYBZB_PT_YB, Sus8SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_9_R_LOC_XBYBZB_PT_YF, Sus9SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_10_R_LOC_XMYBZF_PT_ZF, Sus10SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_11_R_LOC_XBYMZB_PT_ZB, Sus11SetIds.CORE_HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_acs_command(tc_queue=tc_queue, op_code="sus-off")
        tc_queue.appendleft((QueueCommands.WAIT, 5.0))
        pack_sus_cmds(tc_queue=tc_queue, op_code="sus-d")

        # SUSs
        sid_list.append(make_sid(oids.SUS_0_N_LOC_XFYFZM_PT_XF, Sus0SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_1_N_LOC_XBYFZM_PT_XB, Sus1SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_2_N_LOC_XFYBZB_PT_YB, Sus2SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_3_N_LOC_XFYBZF_PT_YF, Sus3SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_4_N_LOC_XMYFZF_PT_ZF, Sus4SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_5_N_LOC_XFYMZB_PT_ZB, Sus5SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_6_R_LOC_XFYBZM_PT_XF, Sus6SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_7_R_LOC_XBYBZM_PT_XB, Sus7SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_8_R_LOC_XBYBZB_PT_YB, Sus8SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_9_R_LOC_XBYBZB_PT_YF, Sus9SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_10_R_LOC_XMYBZF_PT_ZF, Sus10SetIds.CORE_HK))
        sid_list.append(make_sid(oids.SUS_11_R_LOC_XBYMZB_PT_ZB, Sus11SetIds.CORE_HK))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_acs_command(tc_queue=tc_queue, op_code="sus-off")

    if op_code in OpCodes.STR_FT:
        key = KAI.STR_FT[0]

        pack_star_tracker_commands(object_id=oids.STAR_TRACKER_ID, tc_queue=tc_queue, op_code="2")

        # STR
        sid_list.append(make_sid(oids.STAR_TRACKER_ID, StrSetIds.TEMPERATURE))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False
        )

        pack_star_tracker_commands(object_id=oids.STAR_TRACKER_ID, tc_queue=tc_queue, op_code="3")

    if op_code in OpCodes.RW_FT_ONE_RW:
        key = KAI.RW_FT_ONE_RW[0]
# RW NORMAL
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, tc_queue=tc_queue, op_code="nml")

        # RW
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.STATUS_SET_ID))
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.TEMPERATURE_SET_ID))
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.LAST_RESET))
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.TM_SET))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False, one_rw=True,
        )
# RW off
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, tc_queue=tc_queue, op_code="off")

# ass command with 2 rws to speed
    if op_code in OpCodes.:
        key = KAI.[0]
# RW NORMAL
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, tc_queue=tc_queue, op_code="nml")

        # RW
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.STATUS_SET_ID))
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.TEMPERATURE_SET_ID))
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.LAST_RESET))
        sid_list.append(make_sid(oids.RW_ASSEMBLY, RwSetIds.TM_SET))
        pack_generic_hk_listening_cmds(
            tc_queue=tc_queue, proc_key=key, sid_list=sid_list, diag=False, mgt=False, one_rw=True,
        )
# RW off
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, tc_queue=tc_queue, op_code="off")




"""
def listen_to_hk_for_x_seconds(
    tc_queue: TcQueueT,
    diag: bool,
    device: str,
    sid: bytes,
    interval_seconds: float,
    collection_time: float,
):

    tc_queue.appendleft((QueueCommands.PRINT, f"Enabling periodic HK for {device}"))
    cmd_tuple = enable_periodic_hk_command_with_interval(
        diag=diag, sid=sid, interval_seconds=interval_seconds, ssc=0
    )
    for cmd in cmd_tuple:
        tc_queue.appendleft(cmd.pack_command_tuple())

    tc_queue.appendleft((QueueCommands.WAIT, collection_time))

    tc_queue.appendleft((QueueCommands.PRINT, f"Disabling periodic HK for {device}"))
    tc_queue.appendleft(
        disable_periodic_hk_command(diag=diag, sid=sid, ssc=0).pack_command_tuple()
    )
"""


def enable_listen_to_hk_for_x_seconds(
    tc_queue: TcQueueT,
    diag: bool,
    device: str,
    sid: bytes,
    interval_seconds: float,
):
    tc_queue.appendleft((QueueCommands.PRINT, f"Enabling periodic HK for {device}"))
    cmd_tuple = enable_periodic_hk_command_with_interval(
        diag=diag, sid=sid, interval_seconds=interval_seconds, ssc=0
    )
    for cmd in cmd_tuple:
        tc_queue.appendleft(cmd.pack_command_tuple())

    tc_queue.appendleft((QueueCommands.WAIT, 2.0))


def disable_listen_to_hk_for_x_seconds(
    tc_queue: TcQueueT,
    diag: bool,
    device: str,
    sid: bytes,
):
    tc_queue.appendleft((QueueCommands.PRINT, f"Disabling periodic HK for {device}"))
    tc_queue.appendleft(
        disable_periodic_hk_command(diag=diag, sid=sid, ssc=0).pack_command_tuple()
    )


def activate_mgts_alternately(
    tc_queue: TcQueueT,
):

    command = pack_dipole_command(
        object_id=oids.IMTQ_HANDLER_ID,
        x_dipole=2000,
        y_dipole=0,
        z_dipole=0,
        duration=30000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 10.0))

    command = pack_dipole_command(
        object_id=oids.IMTQ_HANDLER_ID,
        x_dipole=-2000,
        y_dipole=0,
        z_dipole=0,
        duration=30000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 10.0))

    command = pack_dipole_command(
        object_id=oids.IMTQ_HANDLER_ID,
        x_dipole=0,
        y_dipole=2000,
        z_dipole=0,
        duration=30000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 10.0))

    command = pack_dipole_command(
        object_id=oids.IMTQ_HANDLER_ID,
        x_dipole=0,
        y_dipole=-2000,
        z_dipole=0,
        duration=30000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 10.0))

    command = pack_dipole_command(
        object_id=oids.IMTQ_HANDLER_ID,
        x_dipole=0,
        y_dipole=0,
        z_dipole=2000,
        duration=30000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 10.0))

    command = pack_dipole_command(
        object_id=oids.IMTQ_HANDLER_ID,
        x_dipole=0,
        y_dipole=0,
        z_dipole=-2000,
        duration=30000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 10.0))


def activate_one_rw_in_sequence(
        tc_queue: TcQueueT,
):
# RW1 speed cmd
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    command = pack_set_speed_command(
        object_id=oids.RW1_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
# WRONG: should be RW INIT command from datasheet
    tc_queue.appendleft((QueueCommands.WAIT, 70.0))
    command = pack_set_speed_command(
        object_id=oids.RW1_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))

# RW2 speed cmd
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    command = pack_set_speed_command(
        object_id=oids.RW2_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
# WRONG: should be RW INIT command from datasheet
    tc_queue.appendleft((QueueCommands.WAIT, 70.0))
    command = pack_set_speed_command(
        object_id=oids.RW2_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))

# RW3 speed cmd
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    command = pack_set_speed_command(
        object_id=oids.RW3_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
# WRONG: should be RW INIT command from datasheet
    tc_queue.appendleft((QueueCommands.WAIT, 70.0))
    command = pack_set_speed_command(
        object_id=oids.RW3_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))

# RW4 speed cmd
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    command = pack_set_speed_command(
        object_id=oids.RW4_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
# WRONG: should be RW INIT command from datasheet
    tc_queue.appendleft((QueueCommands.WAIT, 70.0))
    command = pack_set_speed_command(
        object_id=oids.RW4_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))

def activate_two_rws_in_sequence(
        tc_queue: TcQueueT
):
    # RW1+3 speed cmd
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    command = pack_set_speed_command(
        object_id=oids.RW1_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    command = pack_set_speed_command(
        object_id=oids.RW3_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
# WRONG: should be RW INIT command from datasheet
    tc_queue.appendleft((QueueCommands.WAIT, 70.0))
    command = pack_set_speed_command(
        object_id=oids.RW1_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    command = pack_set_speed_command(
        object_id=oids.RW3_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    # RW2+4 speed cmd
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))
    command = pack_set_speed_command(
        object_id=oids.RW2_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    command = pack_set_speed_command(
        object_id=oids.RW4_ID,
        speed=20000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    # WRONG: should be RW INIT command from datasheet
    tc_queue.appendleft((QueueCommands.WAIT, 70.0))
    command = pack_set_speed_command(
        object_id=oids.RW2_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    command = pack_set_speed_command(
        object_id=oids.RW4_ID,
        speed=1000,
        ramp_time=10000,
    )
    tc_queue.appendleft(command.pack_command_tuple())
    tc_queue.appendleft((QueueCommands.WAIT, 5.0))