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

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 import DefaultPusQueueHelper
from tmtccmd.tc.pus_3_fsfw_hk import make_sid, generate_one_hk_command
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter

_LOGGER = logging.getLogger(__name__)


class OpCode:
    REQ_OS_HK = ["hk"]
    RESET_GNSS = ["reset"]


class Info:
    REQ_OS_HK = "Request One-Shot HK"
    RESET_GNSS = "Reset GNSS using reset pin"


class SetId:
    HK = 0


@tmtc_definitions_provider
def add_gps_cmds(defs: TmtcDefinitionWrapper):
    oce = OpCodeEntry()
    oce.add(keys=OpCode.RESET_GNSS, info=Info.RESET_GNSS)
    oce.add(keys=OpCode.REQ_OS_HK, info=Info.REQ_OS_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.REQ_OS_HK:
        q.add_log_cmd(f"GMSS: {Info.REQ_OS_HK}")
        q.add_pus_tc(
            generate_one_hk_command(sid=make_sid(object_id=object_id, set_id=SetId.HK))
        )


def handle_gps_data(printer: FsfwTmTcPrinter, hk_data: bytes):
    pw = PrintWrapper(printer)
    pw.dlog(f"Received GPS data, HK data length {len(hk_data)}")
    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
    date_string = f"{day}.{month}.{year} {hours}:{minutes}:{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}")
    printer.print_validity_buffer(validity_buffer=hk_data[current_idx:], num_vars=14)