import datetime import enum import logging import struct from eive_tmtc.config.definitions import CustomServiceList from eive_tmtc.pus_tm.defs import PrintWrapper from tmtccmd.config import CmdTreeNode, TmtcDefinitionWrapper, OpCodeEntry from tmtccmd.config.tmtc import tmtc_definitions_provider from tmtccmd.pus.s200_fsfw_mode import create_mode_command, Mode from tmtccmd.tmtc import DefaultPusQueueHelper from tmtccmd.pus.tc.s3_fsfw_hk import ( make_sid, create_request_one_hk_command, create_enable_periodic_hk_command_with_interval_with_diag, create_disable_periodic_hk_command_with_diag, ) from tmtccmd.fsfw.tmtc_printer import FsfwTmTcPrinter _LOGGER = logging.getLogger(__name__) class GpsInfo: MAX_SATELLITES = 30 class OpCode: OFF = "off" ON = "on" REQ_CORE_HK = "core_hk_request" ENABLE_CORE_HK = "core_hk_enable" DISABLE_CORE_HK = "core_hk_disable" REQ_SKYVIEW_HK = "skyview_hk_request" ENABLE_SKYVIEW_HK = "skyview_hk_enable" DISABLE_SKYVIEW_HK = "skyview_hk_disable" RESET_GNSS = "reset" class Info: OFF = "Off" ON = "On" REQ_CORE_HK = "Request Core HK" ENABLE_CORE_HK = "Enable Core HK" DISABLE_CORE_HK = "Disable Core HK" REQ_SKYVIEW_HK = "Request Skyview HK" ENABLE_SKYVIEW_HK = "Enable Skyview HK" DISABLE_SKYVIEW_HK = "Disable Skyview HK" RESET_GNSS = "Reset GNSS using reset pin" class SetId(enum.IntEnum): CORE_HK = 0 SKYVIEW_HK = 1 def create_gnss_node() -> CmdTreeNode: op_code_strs = [ getattr(OpCode, key) for key in dir(OpCode) if not key.startswith("__") ] info_strs = [getattr(Info, key) for key in dir(OpCode) if not key.startswith("__")] combined_dict = dict(zip(op_code_strs, info_strs)) node = CmdTreeNode("gnss", "GNSS device", hide_children_for_print=True) for op_code, info in combined_dict.items(): node.add_child(CmdTreeNode(op_code, info)) return node @tmtc_definitions_provider def add_gps_cmds(defs: TmtcDefinitionWrapper): oce = OpCodeEntry() oce.add(keys=OpCode.OFF, info=Info.OFF) oce.add(keys=OpCode.ON, info=Info.ON) oce.add(keys=OpCode.RESET_GNSS, info=Info.RESET_GNSS) oce.add(keys=OpCode.REQ_CORE_HK, info=Info.REQ_CORE_HK) oce.add(keys=OpCode.ENABLE_CORE_HK, info=Info.ENABLE_CORE_HK) oce.add(keys=OpCode.DISABLE_CORE_HK, info=Info.DISABLE_CORE_HK) oce.add(keys=OpCode.REQ_SKYVIEW_HK, info=Info.REQ_SKYVIEW_HK) oce.add(keys=OpCode.ENABLE_SKYVIEW_HK, info=Info.ENABLE_SKYVIEW_HK) oce.add(keys=OpCode.DISABLE_SKYVIEW_HK, info=Info.DISABLE_SKYVIEW_HK) defs.add_service( name=CustomServiceList.GPS_CTRL.value, info="GPS/GNSS Controller", op_code_entry=oce, ) def pack_gps_command( # noqa: C901 object_id: bytes, q: DefaultPusQueueHelper, cmd_str: str ): # noqa: C901: if cmd_str == OpCode.RESET_GNSS: # TODO: This needs to be re-implemented _LOGGER.warning("Reset pin handling needs to be re-implemented") if cmd_str == OpCode.ENABLE_CORE_HK: interval = float(input("Please specify interval in floating point seconds: ")) if interval <= 0: raise ValueError("invalid interval") q.add_log_cmd(f"GPS: {Info.ENABLE_CORE_HK}") cmds = create_enable_periodic_hk_command_with_interval_with_diag( diag=False, sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK), interval_seconds=interval, ) for cmd in cmds: q.add_pus_tc(cmd) if cmd_str == OpCode.DISABLE_CORE_HK: q.add_log_cmd(f"gps: {Info.DISABLE_CORE_HK}") q.add_pus_tc( create_disable_periodic_hk_command_with_diag( diag=False, sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK) ) ) if cmd_str == OpCode.REQ_CORE_HK: q.add_log_cmd(f"GPS: {Info.REQ_CORE_HK}") q.add_pus_tc( create_request_one_hk_command( sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK) ) ) if cmd_str == OpCode.ENABLE_SKYVIEW_HK: interval = float(input("Please specify interval in floating point seconds: ")) if interval <= 0: raise ValueError("invalid interval") q.add_log_cmd(f"GPS: {Info.ENABLE_SKYVIEW_HK}") cmds = create_enable_periodic_hk_command_with_interval_with_diag( diag=False, sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK), interval_seconds=interval, ) for cmd in cmds: q.add_pus_tc(cmd) if cmd_str == OpCode.DISABLE_SKYVIEW_HK: q.add_log_cmd(f"gps: {Info.DISABLE_SKYVIEW_HK}") q.add_pus_tc( create_disable_periodic_hk_command_with_diag( diag=False, sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK) ) ) if cmd_str == OpCode.REQ_SKYVIEW_HK: q.add_log_cmd(f"GPS: {Info.REQ_SKYVIEW_HK}") q.add_pus_tc( create_request_one_hk_command( sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK) ) ) if cmd_str == OpCode.ON: q.add_log_cmd(f"GPS: {Info.ON}") q.add_pus_tc(create_mode_command(object_id, Mode.ON, 0)) if cmd_str == OpCode.OFF: q.add_log_cmd(f"GPS: {Info.OFF}") q.add_pus_tc(create_mode_command(object_id, Mode.OFF, 0)) def handle_gps_data( pw: PrintWrapper, set_id: int, hk_data: bytes, packet_time: datetime.datetime, ): pw.ilog(_LOGGER, f"Received GPS CTRL HK with packet time {packet_time}") match set_id: case SetId.CORE_HK: handle_core_data(pw, hk_data) case SetId.SKYVIEW_HK: handle_skyview_data(pw, hk_data) def handle_core_data(pw: PrintWrapper, hk_data: bytes): if len(hk_data) < 4 * 8 + 4 + 2 + 8: pw.dlog( f"GPS Core dataset with size {len(hk_data)} does not have expected size" f" of {4*8+4+2+8} bytes" ) return current_idx = 0 fmt_str = "!ddddBBBHBBBBBI" inc_len = struct.calcsize(fmt_str) ( lat, long, alt, speed, fix, sats_in_use, sats_in_view, year, month, day, hours, minutes, seconds, unix_seconds, ) = struct.unpack(fmt_str, hk_data[current_idx : current_idx + inc_len]) current_idx += inc_len if year == 0: date_string = "No date string, year is 0" else: date_string = datetime.datetime( year=year, month=month, day=day, hour=hours, minute=minutes, second=seconds ) pw.dlog(f"Lat: {lat} deg") pw.dlog(f"Long: {long} deg") pw.dlog(f"Altitude: {alt} m | Speed: {speed} m/s") pw.dlog( f"Fix Type: {fix} | Sats in View {sats_in_view} | Sats in Use {sats_in_use}" ) pw.dlog(f"GNSS Date: {date_string}") pw.dlog(f"Unix seconds {unix_seconds}") FsfwTmTcPrinter.get_validity_buffer( validity_buffer=hk_data[current_idx:], num_vars=14 ) def handle_skyview_data(pw: PrintWrapper, hk_data: bytes): data_length = 8 + GpsInfo.MAX_SATELLITES * (8 + 3 * 2 + 1) if len(hk_data) < data_length: pw.dlog( f"GPS Skyview dataset with size {len(hk_data)} does not have expected size" f" of {data_length} bytes" ) return current_idx = 0 fmt_str_unix = "!d" fmt_str_int16 = "!" + "h" * GpsInfo.MAX_SATELLITES fmt_str_double = "!" + "d" * GpsInfo.MAX_SATELLITES fmt_str_uint8 = "!" + "B" * GpsInfo.MAX_SATELLITES inc_len_unix = struct.calcsize(fmt_str_unix) inc_len_int16 = struct.calcsize(fmt_str_int16) inc_len_double = struct.calcsize(fmt_str_double) inc_len_uint8 = struct.calcsize(fmt_str_uint8) unix = struct.unpack( fmt_str_unix, hk_data[current_idx : current_idx + inc_len_unix] )[0] current_idx += inc_len_unix prn_id = struct.unpack( fmt_str_int16, hk_data[current_idx : current_idx + inc_len_int16] ) current_idx += inc_len_int16 azimuth = struct.unpack( fmt_str_int16, hk_data[current_idx : current_idx + inc_len_int16] ) current_idx += inc_len_int16 elevation = struct.unpack( fmt_str_int16, hk_data[current_idx : current_idx + inc_len_int16] ) current_idx += inc_len_int16 signal_to_noise = struct.unpack( fmt_str_double, hk_data[current_idx : current_idx + inc_len_double] ) current_idx += inc_len_double used = struct.unpack( fmt_str_uint8, hk_data[current_idx : current_idx + inc_len_uint8] ) current_idx += inc_len_uint8 pw.dlog(f"Skyview Time: {unix} unix-sec") pw.dlog( "{:<8} {:<8} {:<8} {:<8} {:<8}".format( "PRN_ID", "AZ [°]", "EL [°]", "S2N [dBHz]", "USED" ) ) for idx in range(GpsInfo.MAX_SATELLITES): pw.dlog( "{:<8} {:<8} {:<8} {:<8} {:<8}".format( prn_id[idx], azimuth[idx], elevation[idx], signal_to_noise[idx], used[idx], ) ) FsfwTmTcPrinter.get_validity_buffer( validity_buffer=hk_data[current_idx:], num_vars=6 )