147 lines
5.0 KiB
Python
147 lines
5.0 KiB
Python
import enum
|
|
import logging
|
|
import struct
|
|
|
|
from tmtccmd.tc import DefaultPusQueueHelper
|
|
|
|
import eive_tmtc.config.object_ids as obj_ids
|
|
from tmtccmd.tc.pus_3_fsfw_hk import create_request_one_hk_command, make_sid
|
|
from tmtccmd.config.tmtc import tmtc_definitions_provider, OpCodeEntry, TmtcDefinitionWrapper
|
|
from eive_tmtc.config.object_ids import GYRO_0_ADIS_HANDLER_ID, GYRO_1_L3G_HANDLER_ID, GYRO_2_ADIS_HANDLER_ID, GYRO_3_L3G_HANDLER_ID
|
|
from eive_tmtc.config.definitions import CustomServiceList
|
|
from eive_tmtc.pus_tm.defs import PrintWrapper
|
|
|
|
from tmtccmd.util import ObjectIdU32
|
|
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
|
|
|
|
|
|
class OpCode:
|
|
CORE_HK = "core_hk"
|
|
CFG_HK = "cfg_hk"
|
|
|
|
|
|
class AdisGyroSetId(enum.IntEnum):
|
|
CORE_HK = 0
|
|
CFG_HK = 1
|
|
|
|
|
|
class L3gGyroSetId(enum.IntEnum):
|
|
CORE_HK = 0
|
|
|
|
|
|
class GyrSel(enum.IntEnum):
|
|
GYR_0_ADIS = 0
|
|
GYR_1_L3G = 1
|
|
GYR_2_ADIS = 2
|
|
GYR_3_L3G = 3
|
|
|
|
|
|
GYR_SEL_DICT = {
|
|
GyrSel.GYR_0_ADIS: ("GYRO_0_ADIS", GYRO_0_ADIS_HANDLER_ID),
|
|
GyrSel.GYR_1_L3G: ("GYRO_1_L3G", GYRO_1_L3G_HANDLER_ID),
|
|
GyrSel.GYR_2_ADIS: ("GYRO_2_ADIS", GYRO_2_ADIS_HANDLER_ID),
|
|
GyrSel.GYR_3_L3G: ("GYRO_3_L3G", GYRO_3_L3G_HANDLER_ID),
|
|
}
|
|
|
|
|
|
def handle_gyr_cmd(q: DefaultPusQueueHelper, op_code: str):
|
|
print("Please select the Gyro Device")
|
|
for (k, v) in GYR_SEL_DICT.items():
|
|
print(f"{k}: {v[0]}")
|
|
sel_idx = int(input("Select gyro device by index: "))
|
|
gyr_info = GYR_SEL_DICT[GyrSel(sel_idx)]
|
|
gyr_obj_id = gyr_info[1]
|
|
is_adis = False
|
|
if sel_idx == GyrSel.GYR_0_ADIS or sel_idx == GyrSel.GYR_2_ADIS:
|
|
is_adis = True
|
|
core_hk_id = AdisGyroSetId.CORE_HK
|
|
else:
|
|
core_hk_id = L3gGyroSetId.CORE_HK
|
|
if op_code == OpCode.CORE_HK:
|
|
q.add_log_cmd(f"Gyro {gyr_info[0]} Core HK")
|
|
q.add_pus_tc(create_request_one_hk_command(make_sid(gyr_obj_id, core_hk_id)))
|
|
elif op_code == OpCode.CFG_HK:
|
|
if not is_adis:
|
|
raise ValueError("No config HK for L3 device")
|
|
q.add_log_cmd(f"Gyro {gyr_info[0]} CFG HK")
|
|
q.add_pus_tc(create_request_one_hk_command(make_sid(gyr_obj_id, AdisGyroSetId.CFG_HK)))
|
|
else:
|
|
logging.getLogger(__name__).warning(f"invalid op code {op_code} for gyro command")
|
|
|
|
|
|
def handle_gyros_hk_data(
|
|
object_id: ObjectIdU32, printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes
|
|
):
|
|
if object_id.as_bytes in [
|
|
obj_ids.GYRO_0_ADIS_HANDLER_ID,
|
|
obj_ids.GYRO_2_ADIS_HANDLER_ID,
|
|
]:
|
|
handle_adis_gyro_hk(
|
|
object_id=object_id, printer=printer, set_id=set_id, hk_data=hk_data
|
|
)
|
|
elif object_id.as_bytes in [
|
|
obj_ids.GYRO_1_L3G_HANDLER_ID,
|
|
obj_ids.GYRO_3_L3G_HANDLER_ID,
|
|
]:
|
|
handle_l3g_gyro_hk(
|
|
object_id=object_id, printer=printer, set_id=set_id, hk_data=hk_data
|
|
)
|
|
|
|
|
|
def handle_adis_gyro_hk(
|
|
object_id: ObjectIdU32, printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes
|
|
):
|
|
if set_id == AdisGyroSetId.CORE_HK:
|
|
pw = PrintWrapper(printer)
|
|
fmt_str = "!ddddddf"
|
|
inc_len = struct.calcsize(fmt_str)
|
|
(ang_veloc_x, ang_veloc_y, ang_veloc_z, accel_x, accel_y, accel_z, temp) = struct.unpack(
|
|
fmt_str, hk_data[0 : 0 + inc_len]
|
|
)
|
|
pw.dlog(f"Received ADIS1650X Gyro HK data from object {object_id}")
|
|
pw.dlog(
|
|
f"Angular Velocities (degrees per second): X {ang_veloc_x} | "
|
|
f"Y {ang_veloc_y} | Z {ang_veloc_z}"
|
|
)
|
|
pw.dlog(f"Acceleration (m/s^2): X {accel_x} | Y {accel_y} | Z {accel_z}")
|
|
pw.dlog(f"Temperature {temp} C")
|
|
if set_id == AdisGyroSetId.CFG_HK:
|
|
pw = PrintWrapper(printer)
|
|
fmt_str = "!HBHHH"
|
|
inc_len = struct.calcsize(fmt_str)
|
|
print(len(hk_data))
|
|
(diag_stat_reg, filter_setting, range_mdl, msc_ctrl_reg, dec_rate_reg) = struct.unpack(
|
|
fmt_str, hk_data[0 : 0 + inc_len]
|
|
)
|
|
pw.dlog(f"Diagnostic Status Register {diag_stat_reg:#018b}")
|
|
pw.dlog(f"Range MDL {range_mdl}")
|
|
pw.dlog(f"Filter Settings {filter_setting:#010b}")
|
|
pw.dlog(f"Miscellaneous Control Register {msc_ctrl_reg:#018b}")
|
|
pw.dlog(f"Decimation Rate {dec_rate_reg:#06x}")
|
|
|
|
|
|
def handle_l3g_gyro_hk(
|
|
object_id: ObjectIdU32, printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes
|
|
):
|
|
if set_id == L3gGyroSetId.CORE_HK:
|
|
pw = PrintWrapper(printer)
|
|
fmt_str = "!ffff"
|
|
inc_len = struct.calcsize(fmt_str)
|
|
(angVelocX, angVelocY, angVelocZ, temp) = struct.unpack(
|
|
fmt_str, hk_data[0 : 0 + inc_len]
|
|
)
|
|
pw.dlog(f"Received L3GD20H Gyro HK data from object {object_id}")
|
|
pw.dlog(
|
|
f"Angular Velocities (degrees per second): X {angVelocX} | "
|
|
f"Y {angVelocY} | Z {angVelocZ}"
|
|
)
|
|
pw.dlog(f"Temperature {temp} °C")
|
|
|
|
|
|
@tmtc_definitions_provider
|
|
def add_gyr_cmd_defs(defs: TmtcDefinitionWrapper):
|
|
oce = OpCodeEntry()
|
|
oce.add(keys=OpCode.CORE_HK, info="Request Core HK")
|
|
oce.add(keys=OpCode.CFG_HK, info="Request CFG HK")
|
|
defs.add_service(CustomServiceList.GYRO, info="Gyro", op_code_entry=oce)
|