eive-tmtc/eive_tmtc/pus_tm/devs/gyros.py

76 lines
2.7 KiB
Python
Raw Normal View History

2022-05-19 13:58:43 +02:00
import struct
2022-11-29 16:53:29 +01:00
from eive_tmtc.pus_tm.defs import PrintWrapper
2022-07-08 16:25:46 +02:00
from tmtccmd.util import ObjectIdU32
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
2022-05-19 13:20:22 +02:00
2023-01-16 14:13:06 +01:00
from eive_tmtc.pus_tc.devs.gyros import L3gGyroSetId, AdisGyroSetId
2022-11-29 16:53:29 +01:00
import eive_tmtc.config.object_ids as obj_ids
2022-05-19 13:58:43 +02:00
2022-05-19 13:20:22 +02:00
def handle_gyros_hk_data(
2022-07-05 02:12:54 +02:00
object_id: ObjectIdU32, printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes
2022-05-19 13:20:22 +02:00
):
2022-05-19 13:58:43 +02:00
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(
2022-07-05 02:12:54 +02:00
object_id: ObjectIdU32, printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes
2022-05-19 13:58:43 +02:00
):
2023-01-16 14:13:06 +01:00
if set_id == AdisGyroSetId.CORE_HK:
2022-05-19 13:58:43 +02:00
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")
2023-01-16 14:13:06 +01:00
if set_id == AdisGyroSetId.CFG_HK:
2022-05-19 13:58:43 +02:00
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(
2022-07-05 02:12:54 +02:00
object_id: ObjectIdU32, printer: FsfwTmTcPrinter, set_id: int, hk_data: bytes
2022-05-19 13:58:43 +02:00
):
2023-01-16 14:13:06 +01:00
if set_id == L3gGyroSetId.CORE_HK:
2022-05-19 13:58:43 +02:00
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}"
)
2022-06-07 17:49:16 +02:00
pw.dlog(f"Temperature {temp} °C")