eive-tmtc/pus_tm/devs/gyros.py
2022-07-08 16:25:46 +02:00

76 lines
2.7 KiB
Python

import struct
from pus_tm.defs import PrintWrapper
from tmtccmd.util import ObjectIdU32
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
from pus_tc.devs.gyros import L3gGyroSetIds, AdisGyroSetIds
import config.object_ids as obj_ids
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 == AdisGyroSetIds.CORE_HK:
pw = PrintWrapper(printer)
fmt_str = "!ddddddf"
inc_len = struct.calcsize(fmt_str)
(angVelocX, angVelocY, angVelocZ, accelX, accelY, accelZ, 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 {angVelocX} | "
f"Y {angVelocY} | Z {angVelocZ}"
)
pw.dlog(f"Acceleration (m/s^2): X {accelX} | Y {accelY} | Z {accelZ}")
pw.dlog(f"Temperature {temp} C")
if set_id == AdisGyroSetIds.CFG_HK:
pw = PrintWrapper(printer)
fmt_str = "!HBHH"
inc_len = struct.calcsize(fmt_str)
(diag_stat_reg, filter_setting, 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"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 == L3gGyroSetIds.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")