158 lines
5.1 KiB
Python
158 lines
5.1 KiB
Python
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 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):
|
|
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.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))
|
|
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_mgm_data_processed(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
|
|
)
|