eive-tmtc/pus_tm/devs/gyros.py

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")