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

259 lines
8.5 KiB
Python
Raw Normal View History

2023-02-07 16:48:23 +01:00
import datetime
2023-03-23 13:49:11 +01:00
import enum
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
2023-03-30 15:12:33 +02:00
from tmtccmd.tc.pus_200_fsfw_mode import create_mode_command, Mode
2022-08-08 16:32:18 +02:00
from tmtccmd.tc import DefaultPusQueueHelper
2023-02-07 16:48:23 +01:00
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,
)
2023-05-25 11:31:06 +02:00
from tmtccmd.fsfw.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
2023-08-15 11:47:07 +02:00
2023-08-04 16:24:16 +02:00
class GpsInfo:
MAX_SATELLITES = 30
2022-05-24 01:34:33 +02:00
2023-08-15 11:47:07 +02:00
2023-01-16 14:13:06 +01:00
class OpCode:
2023-03-30 15:12:33 +02:00
OFF = "off"
ON = "on"
2023-08-04 14:35:09 +02:00
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"]
2023-02-05 12:52:43 +01:00
RESET_GNSS = ["reset"]
2022-05-24 01:34:33 +02:00
class Info:
2023-03-30 15:12:33 +02:00
OFF = "Off"
ON = "On"
2023-08-04 14:35:09 +02:00
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"
2022-05-24 01:34:33 +02:00
RESET_GNSS = "Reset GNSS using reset pin"
2023-03-23 13:49:11 +01:00
class SetId(enum.IntEnum):
2023-08-04 14:35:09 +02:00
CORE_HK = 0
SKYVIEW_HK = 1
2022-05-24 01:34:33 +02:00
@tmtc_definitions_provider
def add_gps_cmds(defs: TmtcDefinitionWrapper):
2022-07-05 02:12:54 +02:00
oce = OpCodeEntry()
2023-03-30 15:12:33 +02:00
oce.add(keys=OpCode.OFF, info=Info.OFF)
oce.add(keys=OpCode.ON, info=Info.ON)
2023-08-04 14:35:09 +02:00
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)
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
2023-08-16 13:25:29 +02:00
def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
2023-08-04 15:38:32 +02: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-08-04 15:38:32 +02:00
if op_code in OpCode.ENABLE_CORE_HK:
2023-02-07 16:48:23 +01:00
interval = float(input("Please specify interval in floating point seconds: "))
if interval <= 0:
raise ValueError("invalid interval")
2023-08-04 15:01:34 +02:00
q.add_log_cmd(f"GPS: {Info.ENABLE_CORE_HK}")
2023-02-07 16:48:23 +01:00
cmds = create_enable_periodic_hk_command_with_interval(
2023-08-29 21:41:29 +02:00
diag=False,
sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK),
interval_seconds=interval,
2022-05-25 10:37:38 +02:00
)
2023-02-07 16:48:23 +01:00
for cmd in cmds:
q.add_pus_tc(cmd)
2023-08-04 15:38:32 +02:00
if op_code in OpCode.DISABLE_CORE_HK:
2023-08-04 15:01:34 +02:00
q.add_log_cmd(f"gps: {Info.DISABLE_CORE_HK}")
2023-08-29 21:41:29 +02:00
q.add_pus_tc(
create_disable_periodic_hk_command(
diag=False, sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK)
)
)
2023-08-04 15:38:32 +02:00
if op_code in OpCode.REQ_CORE_HK:
2023-08-04 15:01:34 +02:00
q.add_log_cmd(f"GPS: {Info.REQ_CORE_HK}")
2023-08-29 21:41:29 +02:00
q.add_pus_tc(
create_request_one_hk_command(
sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK)
)
)
2023-08-04 16:24:16 +02:00
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(
2023-08-29 21:41:29 +02:00
diag=False,
sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK),
interval_seconds=interval,
2023-08-04 16:24:16 +02:00
)
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}")
2023-08-29 21:41:29 +02:00
q.add_pus_tc(
create_disable_periodic_hk_command(
diag=False, sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK)
)
)
2023-08-04 16:24:16 +02:00
if op_code in OpCode.REQ_SKYVIEW_HK:
q.add_log_cmd(f"GPS: {Info.REQ_SKYVIEW_HK}")
2023-08-29 21:41:29 +02:00
q.add_pus_tc(
create_request_one_hk_command(
sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK)
)
)
2023-08-04 15:38:32 +02:00
if op_code in OpCode.ON:
2023-03-30 15:12:33 +02:00
q.add_log_cmd(f"GPS: {Info.ON}")
q.add_pus_tc(create_mode_command(object_id, Mode.ON, 0))
2023-08-04 15:38:32 +02:00
if op_code in OpCode.OFF:
2023-03-30 15:12:33 +02:00
q.add_log_cmd(f"GPS: {Info.OFF}")
q.add_pus_tc(create_mode_command(object_id, Mode.OFF, 0))
2023-02-01 16:25:17 +01:00
2023-08-04 14:51:55 +02:00
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)
2023-08-04 16:24:16 +02:00
case SetId.SKYVIEW_HK:
handle_skyview_data(pw, hk_data)
2023-08-04 14:51:55 +02:00
def handle_core_data(pw: PrintWrapper, hk_data: bytes):
2023-08-29 21:41:29 +02:00
if len(hk_data) < 4 * 8 + 4 + 2 + 8:
2023-08-04 14:51:55 +02:00
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
2023-02-01 16:25:17 +01:00
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
2023-02-07 16:48:23 +01:00
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
)
2023-02-01 16:25:17 +01:00
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}")
2023-05-24 13:44:45 +02:00
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=hk_data[current_idx:], num_vars=14
)
2023-08-04 16:24:16 +02:00
def handle_skyview_data(pw: PrintWrapper, hk_data: bytes):
2023-08-29 21:41:29 +02:00
data_length = 8 + GpsInfo.MAX_SATELLITES * (8 + 3 * 2 + 1)
2023-08-04 16:24:16 +02:00
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)
2023-08-29 21:41:29 +02:00
unix = struct.unpack(
fmt_str_unix, hk_data[current_idx : current_idx + inc_len_unix]
)[0]
2023-08-04 16:24:16 +02:00
current_idx += inc_len_unix
2023-08-29 21:41:29 +02:00
prn_id = struct.unpack(
fmt_str_int16, hk_data[current_idx : current_idx + inc_len_int16]
)
2023-08-04 16:24:16 +02:00
current_idx += inc_len_int16
2023-08-29 21:41:29 +02:00
azimuth = struct.unpack(
fmt_str_int16, hk_data[current_idx : current_idx + inc_len_int16]
)
2023-08-04 16:24:16 +02:00
current_idx += inc_len_int16
2023-08-29 21:41:29 +02:00
elevation = struct.unpack(
fmt_str_int16, hk_data[current_idx : current_idx + inc_len_int16]
)
2023-08-04 16:24:16 +02:00
current_idx += inc_len_int16
2023-08-29 21:41:29 +02:00
signal_to_noise = struct.unpack(
fmt_str_double, hk_data[current_idx : current_idx + inc_len_double]
)
2023-08-04 16:24:16 +02:00
current_idx += inc_len_double
2023-08-29 21:41:29 +02:00
used = struct.unpack(
fmt_str_uint8, hk_data[current_idx : current_idx + inc_len_uint8]
)
2023-08-04 16:24:16 +02:00
current_idx += inc_len_uint8
pw.dlog(f"Skyview Time: {unix} unix-sec")
2023-08-29 21:41:29 +02:00
pw.dlog(
"{:<8} {:<8} {:<8} {:<8} {:<8}".format(
"PRN_ID", "AZ [°]", "EL [°]", "S2N [dBW]", "USED"
)
)
2023-08-04 16:24:16 +02:00
for idx in range(GpsInfo.MAX_SATELLITES):
2023-08-29 21:41:29 +02:00
pw.dlog(
"{:<8} {:<8} {:<8} {:<8} {:<8}".format(
prn_id[idx],
azimuth[idx],
elevation[idx],
signal_to_noise[idx],
used[idx],
)
)
2023-08-04 16:24:16 +02:00
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=hk_data[current_idx:], num_vars=6
)