76 lines
2.7 KiB
Python
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")
|