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

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 TmtcDefinitionWrapper, OpCodeEntry
from tmtccmd.config.tmtc import tmtc_definitions_provider
from tmtccmd.tc.pus_200_fsfw_mode import create_mode_command, Mode
from tmtccmd.tc import DefaultPusQueueHelper
from tmtccmd.tc.pus_3_fsfw_hk import (
    make_sid,
    create_request_one_hk_command,
    create_enable_periodic_hk_command_with_interval,
    create_disable_periodic_hk_command,
)
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


@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(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
    if op_code in OpCode.RESET_GNSS:
        # TODO: This needs to be re-implemented
        _LOGGER.warning("Reset pin handling needs to be re-implemented")
    if op_code in 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(
            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 op_code in OpCode.DISABLE_CORE_HK:
        q.add_log_cmd(f"gps: {Info.DISABLE_CORE_HK}")
        q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=make_sid(object_id=object_id,
                                                                                 set_id=SetId.CORE_HK)))
    if op_code in 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 op_code in 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(
            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 op_code in OpCode.DISABLE_SKYVIEW_HK:
        q.add_log_cmd(f"gps: {Info.DISABLE_SKYVIEW_HK}")
        q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=make_sid(object_id=object_id,
                                                                                 set_id=SetId.SKYVIEW_HK)))
    if op_code in 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 op_code in OpCode.ON:
        q.add_log_cmd(f"GPS: {Info.ON}")
        q.add_pus_tc(create_mode_command(object_id, Mode.ON, 0))
    if op_code in 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])
    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 [dBW]', '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
    )