eive-tmtc/eive_tmtc/tmtc/acs/gyros.py

200 lines
6.6 KiB
Python
Raw Normal View History

2023-02-01 15:58:34 +01:00
import enum
2023-02-21 14:29:40 +01:00
import logging
2022-05-19 13:58:43 +02:00
import struct
2023-02-27 18:22:57 +01:00
from spacepackets.ecss import PusTelecommand
2023-11-29 17:27:24 +01:00
from tmtccmd.config import CmdTreeNode
2023-11-10 19:23:06 +01:00
from tmtccmd.tmtc import DefaultPusQueueHelper
from tmtccmd.pus.s201_fsfw_health import pack_set_health_cmd_data, FsfwHealth
2023-02-27 18:22:57 +01:00
from tmtccmd.pus.s201_fsfw_health import Subservice
2023-02-01 15:58:34 +01:00
2023-02-21 14:29:40 +01:00
import eive_tmtc.config.object_ids as obj_ids
2023-11-10 19:23:06 +01:00
from tmtccmd.pus.s200_fsfw_mode import create_mode_command, Mode
from tmtccmd.pus.tc.s3_fsfw_hk import create_request_one_hk_command, make_sid
2023-02-23 15:13:35 +01:00
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,
)
2023-02-21 14:29:40 +01:00
from eive_tmtc.config.definitions import CustomServiceList
2022-11-29 16:53:29 +01:00
from eive_tmtc.pus_tm.defs import PrintWrapper
2023-02-21 14:29:40 +01:00
2022-07-08 16:25:46 +02:00
from tmtccmd.util import ObjectIdU32
2022-05-19 13:20:22 +02:00
2023-02-01 15:58:34 +01:00
2023-02-21 14:29:40 +01:00
class OpCode:
NML = "normal"
OFF = "off"
2023-02-21 14:29:40 +01:00
CORE_HK = "core_hk"
CFG_HK = "cfg_hk"
2023-02-27 18:22:57 +01:00
SET_FAULTY = "set_faulty"
2023-02-21 14:29:40 +01:00
2023-02-01 15:58:34 +01:00
class AdisGyroSetId(enum.IntEnum):
CORE_HK = 0
CFG_HK = 1
class L3gGyroSetId(enum.IntEnum):
CORE_HK = 0
2022-05-19 13:58:43 +02:00
2022-05-19 13:20:22 +02:00
2023-02-21 14:29:40 +01:00
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),
}
2023-11-22 10:17:05 +01:00
def handle_gyr_cmd(q: DefaultPusQueueHelper, cmd_str: str):
2023-02-21 14:29:40 +01:00
print("Please select the Gyro Device")
2023-07-13 11:30:43 +02:00
for k, v in GYR_SEL_DICT.items():
2023-02-21 14:29:40 +01:00
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
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.NML:
q.add_log_cmd(f"Gyro {gyr_info[0]} NORMAL mode")
q.add_pus_tc(create_mode_command(gyr_obj_id, Mode.NORMAL, 0))
2023-11-22 10:17:05 +01:00
if cmd_str == OpCode.OFF:
q.add_log_cmd(f"Gyro {gyr_info[0]} OFF mode")
q.add_pus_tc(create_mode_command(gyr_obj_id, Mode.OFF, 0))
2023-11-22 10:17:05 +01:00
elif cmd_str == OpCode.CORE_HK:
2023-02-21 14:29:40 +01:00
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)))
2023-11-22 10:17:05 +01:00
elif cmd_str == OpCode.CFG_HK:
2023-02-21 14:29:40 +01:00
if not is_adis:
raise ValueError("No config HK for L3 device")
q.add_log_cmd(f"Gyro {gyr_info[0]} CFG HK")
2023-02-23 15:13:35 +01:00
q.add_pus_tc(
create_request_one_hk_command(make_sid(gyr_obj_id, AdisGyroSetId.CFG_HK))
)
2023-11-22 10:17:05 +01:00
elif cmd_str == OpCode.SET_FAULTY:
2023-02-27 18:22:57 +01:00
q.add_log_cmd(f"Gyro {gyr_info[0]} set faulty")
q.add_pus_tc(
PusTelecommand(
service=201,
subservice=Subservice.TC_SET_HEALTH,
2023-03-02 14:41:23 +01:00
app_data=pack_set_health_cmd_data(
object_id=gyr_obj_id, health=FsfwHealth.FAULTY
),
2023-02-27 18:22:57 +01:00
)
)
2023-02-21 14:29:40 +01:00
else:
2023-02-23 15:13:35 +01:00
logging.getLogger(__name__).warning(
2023-11-22 10:17:05 +01:00
f"invalid op code {cmd_str} for gyro command"
2023-02-23 15:13:35 +01:00
)
2023-02-21 14:29:40 +01:00
2022-05-19 13:20:22 +02:00
def handle_gyros_hk_data(
2023-05-24 13:44:45 +02:00
object_id: ObjectIdU32, pw: PrintWrapper, 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,
]:
2023-05-24 13:44:45 +02:00
handle_adis_gyro_hk(object_id=object_id, pw=pw, set_id=set_id, hk_data=hk_data)
2022-05-19 13:58:43 +02:00
elif object_id.as_bytes in [
obj_ids.GYRO_1_L3G_HANDLER_ID,
obj_ids.GYRO_3_L3G_HANDLER_ID,
]:
2023-05-24 13:44:45 +02:00
handle_l3g_gyro_hk(object_id=object_id, pw=pw, set_id=set_id, hk_data=hk_data)
2022-05-19 13:58:43 +02:00
def handle_adis_gyro_hk(
2023-05-24 13:44:45 +02:00
object_id: ObjectIdU32, pw: PrintWrapper, 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
fmt_str = "!ddddddf"
inc_len = struct.calcsize(fmt_str)
2023-02-23 15:13:35 +01:00
(
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])
2022-05-19 13:58:43 +02:00
pw.dlog(f"Received ADIS1650X Gyro HK data from object {object_id}")
pw.dlog(
2023-02-21 14:29:40 +01:00
f"Angular Velocities (degrees per second): X {ang_veloc_x} | "
f"Y {ang_veloc_y} | Z {ang_veloc_z}"
2022-05-19 13:58:43 +02:00
)
2023-02-21 14:29:40 +01:00
pw.dlog(f"Acceleration (m/s^2): X {accel_x} | Y {accel_y} | Z {accel_z}")
2022-05-19 13:58:43 +02:00
pw.dlog(f"Temperature {temp} C")
2023-01-16 14:13:06 +01:00
if set_id == AdisGyroSetId.CFG_HK:
2023-02-21 14:29:40 +01:00
fmt_str = "!HBHHH"
2022-05-19 13:58:43 +02:00
inc_len = struct.calcsize(fmt_str)
2023-02-21 14:29:40 +01:00
print(len(hk_data))
2023-02-23 15:13:35 +01:00
(
diag_stat_reg,
filter_setting,
range_mdl,
msc_ctrl_reg,
dec_rate_reg,
) = struct.unpack(fmt_str, hk_data[0 : 0 + inc_len])
2022-05-19 13:58:43 +02:00
pw.dlog(f"Diagnostic Status Register {diag_stat_reg:#018b}")
2023-02-21 14:29:40 +01:00
pw.dlog(f"Range MDL {range_mdl}")
2022-05-19 13:58:43 +02:00
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(
2023-05-24 13:44:45 +02:00
object_id: ObjectIdU32, pw: PrintWrapper, 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
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")
2023-02-21 14:29:40 +01:00
2023-11-29 17:27:24 +01:00
def create_gyros_node() -> CmdTreeNode:
node = CmdTreeNode("gyros", "Gyroscope devices")
node.add_child(CmdTreeNode(OpCode.CORE_HK, "Request Core HK"))
node.add_child(CmdTreeNode(OpCode.CFG_HK, "Request CFG HK"))
node.add_child(CmdTreeNode(OpCode.NML, "Normal Mode"))
node.add_child(CmdTreeNode(OpCode.OFF, "Off Mode"))
node.add_child(CmdTreeNode(OpCode.SET_FAULTY, "Set Faulty"))
return node
2023-02-21 14:29:40 +01:00
@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")
oce.add(keys=OpCode.NML, info="Normal Mode")
oce.add(keys=OpCode.OFF, info="Off Mode")
2023-02-27 18:22:57 +01:00
oce.add(keys=OpCode.SET_FAULTY, info="Set Faulty")
2023-02-21 14:29:40 +01:00
defs.add_service(CustomServiceList.GYRO, info="Gyro", op_code_entry=oce)