add some gps cmds

This commit is contained in:
Robin Müller 2023-02-07 16:48:23 +01:00
parent 2bd6caa3c2
commit eed6b82353
No known key found for this signature in database
GPG Key ID: 71B58F8A3CDFA9AC

View File

@ -1,3 +1,4 @@
import datetime
import logging
import struct
@ -6,7 +7,12 @@ 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.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.util.tmtc_printer import FsfwTmTcPrinter
_LOGGER = logging.getLogger(__name__)
@ -14,11 +20,15 @@ _LOGGER = logging.getLogger(__name__)
class OpCode:
REQ_OS_HK = ["hk"]
ENABLE_HK = ["enable_hk"]
DISABLE_HK = ["disable_hk"]
RESET_GNSS = ["reset"]
class Info:
REQ_OS_HK = "Request One-Shot HK"
ENABLE_HK = "Enable HK"
DISABLE_HK = "Disable HK"
RESET_GNSS = "Reset GNSS using reset pin"
@ -31,6 +41,8 @@ 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)
oce.add(keys=OpCode.ENABLE_HK, info=Info.ENABLE_HK)
oce.add(keys=OpCode.DISABLE_HK, info=Info.DISABLE_HK)
defs.add_service(
name=CustomServiceList.GPS_CTRL.value,
info="GPS/GNSS Controller",
@ -39,14 +51,26 @@ def add_gps_cmds(defs: TmtcDefinitionWrapper):
def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
sid = make_sid(object_id=object_id, set_id=SetId.HK)
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))
if op_code in OpCode.ENABLE_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_HK}")
cmds = create_enable_periodic_hk_command_with_interval(
diag=False, sid=sid, interval_seconds=interval
)
for cmd in cmds:
q.add_pus_tc(cmd)
if op_code in OpCode.DISABLE_HK:
q.add_log_cmd(f"gps: {Info.DISABLE_HK}")
q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=sid))
if op_code in OpCode.REQ_OS_HK:
q.add_log_cmd(f"GPS: {Info.REQ_OS_HK}")
q.add_pus_tc(create_request_one_hk_command(sid=sid))
def handle_gps_data(printer: FsfwTmTcPrinter, hk_data: bytes):
@ -72,7 +96,12 @@ def handle_gps_data(printer: FsfwTmTcPrinter, hk_data: bytes):
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}"
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")