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

116 lines
3.5 KiB
Python

import enum
import struct
from eive_tmtc.config.definitions import CustomServiceList
from tmtccmd.config import OpCodeEntry
import eive_tmtc.config.object_ids as obj_ids
from eive_tmtc.config.object_ids import (
MGM_0_LIS3_HANDLER_ID,
MGM_1_RM3100_HANDLER_ID,
MGM_2_LIS3_HANDLER_ID,
MGM_3_RM3100_HANDLER_ID,
)
from eive_tmtc.pus_tm.defs import PrintWrapper
from tmtccmd.config.tmtc import tmtc_definitions_provider, TmtcDefinitionWrapper
from tmtccmd.tc import DefaultPusQueueHelper
from tmtccmd.tc.pus_200_fsfw_mode import create_mode_command, Mode
from tmtccmd.util import ObjectIdU32
class OpCode:
NORMAL = "normal"
OFF = "off"
class MgmLis3SetId(enum.IntEnum):
CORE_HK = 0
class MgmRm3100SetId(enum.IntEnum):
CORE_HK = 0
class MgmSel(enum.IntEnum):
MGM_0_LIS3 = 0
MGM_1_RM3100 = 1
MGM_2_LIS3 = 2
MGM_3_RM3100 = 3
MGM_SEL_DICT = {
MgmSel.MGM_0_LIS3: ("MGM_0_LIS3", MGM_0_LIS3_HANDLER_ID),
MgmSel.MGM_1_RM3100: ("MGM_1_RM3100", MGM_1_RM3100_HANDLER_ID),
MgmSel.MGM_2_LIS3: ("MGM_2_LIS3", MGM_2_LIS3_HANDLER_ID),
MgmSel.MGM_3_RM3100: ("MGM_3_RM3100", MGM_3_RM3100_HANDLER_ID),
}
def handle_mgm_cmd(q: DefaultPusQueueHelper, op_code: str):
print("Please select the MGM Device")
for (k, v) in MGM_SEL_DICT.items():
print(f"{k}: {v[0]}")
sel_idx = int(input("Select MGM device by index: "))
mgm_info = MGM_SEL_DICT[MgmSel(sel_idx)]
mgm_obj_id = mgm_info[1]
if op_code == OpCode.NORMAL:
q.add_log_cmd(f"Gyro {mgm_info[0]} NORMAL mode")
q.add_pus_tc(create_mode_command(mgm_obj_id, Mode.NORMAL, 0))
if op_code == OpCode.OFF:
q.add_log_cmd(f"Gyro {mgm_info[0]} OFF mode")
q.add_pus_tc(create_mode_command(mgm_obj_id, Mode.OFF, 0))
def handle_mgm_hk_data(
object_id: ObjectIdU32, pw: PrintWrapper, set_id: int, hk_data: bytes
):
if object_id.as_bytes in [
obj_ids.MGM_0_LIS3_HANDLER_ID,
obj_ids.MGM_2_LIS3_HANDLER_ID,
]:
handle_mgm_lis3_hk_data(object_id, pw, set_id, hk_data)
elif object_id.as_bytes in [
obj_ids.MGM_1_RM3100_HANDLER_ID,
obj_ids.MGM_3_RM3100_HANDLER_ID,
]:
handle_mgm_rm3100_hk_data(object_id, pw, set_id, hk_data)
pass
def handle_mgm_lis3_hk_data(
object_id: ObjectIdU32, pw: PrintWrapper, set_id: int, hk_data: bytes
):
if set_id == MgmLis3SetId.CORE_HK:
fmt_str = "!ffff"
inc_len = struct.calcsize(fmt_str)
(field_x, field_y, field_z, temp) = struct.unpack(
fmt_str, hk_data[0 : 0 + inc_len]
)
pw.dlog(f"Received MGM LIS3 from object {object_id}")
pw.dlog(
f"Field strengths in micro Tesla X {field_x} | Y {field_y} | Z {field_z}"
)
pw.dlog(f"Temperature {temp} C")
def handle_mgm_rm3100_hk_data(
object_id: ObjectIdU32, pw: PrintWrapper, set_id: int, hk_data: bytes
):
if set_id == MgmRm3100SetId.CORE_HK:
fmt_str = "!fff"
inc_len = struct.calcsize(fmt_str)
(field_x, field_y, field_z) = struct.unpack(fmt_str, hk_data[0 : 0 + inc_len])
pw.dlog(f"Received MGM RM3100 from object {object_id}")
pw.dlog(
f"Field strengths in micro Tesla X {field_x} | Y {field_y} | Z {field_z}"
)
@tmtc_definitions_provider
def add_mgm_cmd_defs(defs: TmtcDefinitionWrapper):
oce = OpCodeEntry()
oce.add(keys=OpCode.NORMAL, info="Normal Mode")
oce.add(keys=OpCode.OFF, info="Off Mode")
defs.add_service(CustomServiceList.MGMS, info="MGMs", op_code_entry=oce)