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

85 lines
2.5 KiB
Python
Raw Normal View History

2023-02-01 11:17:04 +01:00
import logging
2023-02-01 16:25:17 +01:00
import struct
2021-09-08 13:20:22 +02:00
2022-11-29 16:53:29 +01:00
from eive_tmtc.config.definitions import CustomServiceList
2023-02-01 16:25:17 +01:00
from eive_tmtc.pus_tm.defs import PrintWrapper
from tmtccmd.config import TmtcDefinitionWrapper, OpCodeEntry
from tmtccmd.config.tmtc import tmtc_definitions_provider
2022-08-08 16:32:18 +02:00
from tmtccmd.tc import DefaultPusQueueHelper
2022-05-24 01:34:33 +02:00
from tmtccmd.tc.pus_3_fsfw_hk import make_sid, generate_one_hk_command
2023-02-01 16:25:17 +01:00
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
2021-09-08 13:20:22 +02:00
2023-02-01 11:17:04 +01:00
_LOGGER = logging.getLogger(__name__)
2021-09-08 13:20:22 +02:00
2022-05-24 01:34:33 +02:00
2023-01-16 14:13:06 +01:00
class OpCode:
2022-05-24 01:34:33 +02:00
REQ_OS_HK = ["0", "hk-os"]
RESET_GNSS = ["5", "reset"]
class Info:
REQ_OS_HK = "Request One-Shot HK"
RESET_GNSS = "Reset GNSS using reset pin"
2023-01-16 14:13:06 +01:00
class SetId:
2022-05-24 01:34:33 +02:00
HK = 0
@tmtc_definitions_provider
def add_gps_cmds(defs: TmtcDefinitionWrapper):
2022-07-05 02:12:54 +02:00
oce = OpCodeEntry()
2023-01-16 14:13:06 +01:00
oce.add(keys=OpCode.RESET_GNSS, info=Info.RESET_GNSS)
oce.add(keys=OpCode.REQ_OS_HK, info=Info.REQ_OS_HK)
2022-07-05 02:12:54 +02:00
defs.add_service(
2022-05-24 01:34:33 +02:00
name=CustomServiceList.GPS_CTRL.value,
2022-05-25 10:37:38 +02:00
info="GPS/GNSS Controller",
2022-07-05 02:12:54 +02:00
op_code_entry=oce,
2022-05-24 01:34:33 +02:00
)
2021-09-08 13:20:22 +02:00
2022-08-08 16:32:18 +02:00
def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
2023-01-16 14:13:06 +01:00
if op_code in OpCode.RESET_GNSS:
2022-05-24 01:49:57 +02:00
# TODO: This needs to be re-implemented
2023-02-01 11:17:04 +01:00
_LOGGER.warning("Reset pin handling needs to be re-implemented")
2023-01-16 14:13:06 +01:00
if op_code in OpCode.REQ_OS_HK:
2022-07-04 17:59:09 +02:00
q.add_log_cmd(f"GMSS: {Info.REQ_OS_HK}")
q.add_pus_tc(
2023-01-16 14:13:06 +01:00
generate_one_hk_command(sid=make_sid(object_id=object_id, set_id=SetId.HK))
2022-05-25 10:37:38 +02:00
)
2023-02-01 16:25:17 +01:00
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)