More GPS TM #226

Merged
muellerr merged 6 commits from more-gps-tm into main 2023-08-14 10:38:17 +02:00
2 changed files with 114 additions and 24 deletions

View File

@ -101,7 +101,9 @@ def handle_regular_hk_print( # noqa C901: Complexity okay here
elif objb == obj_ids.IMTQ_HANDLER_ID: elif objb == obj_ids.IMTQ_HANDLER_ID:
return handle_imtq_hk(pw=pw, hk_data=hk_data, set_id=set_id) return handle_imtq_hk(pw=pw, hk_data=hk_data, set_id=set_id)
elif objb == obj_ids.GPS_CONTROLLER: elif objb == obj_ids.GPS_CONTROLLER:
return handle_gps_data(pw=pw, hk_data=hk_data) return handle_gps_data(
pw=pw, set_id=set_id, hk_data=hk_data, packet_time=packet_dt
)
elif objb == obj_ids.PCDU_HANDLER_ID: elif objb == obj_ids.PCDU_HANDLER_ID:
return handle_pcdu_hk(pw=pw, set_id=set_id, hk_data=hk_data) return handle_pcdu_hk(pw=pw, set_id=set_id, hk_data=hk_data)
elif objb == obj_ids.BPX_HANDLER_ID: elif objb == obj_ids.BPX_HANDLER_ID:

View File

@ -19,38 +19,50 @@ from tmtccmd.fsfw.tmtc_printer import FsfwTmTcPrinter
_LOGGER = logging.getLogger(__name__) _LOGGER = logging.getLogger(__name__)
class GpsInfo:
MAX_SATELLITES = 30
class OpCode: class OpCode:
OFF = "off" OFF = "off"
ON = "on" ON = "on"
REQ_OS_HK = ["hk"] REQ_CORE_HK = ["core_hk_request"]
ENABLE_HK = ["enable_hk"] ENABLE_CORE_HK = ["core_hk_enable"]
DISABLE_HK = ["disable_hk"] 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"] RESET_GNSS = ["reset"]
class Info: class Info:
OFF = "Off" OFF = "Off"
ON = "On" ON = "On"
REQ_OS_HK = "Request One-Shot HK" REQ_CORE_HK = "Request Core HK"
ENABLE_HK = "Enable HK" ENABLE_CORE_HK = "Enable Core HK"
DISABLE_HK = "Disable 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" RESET_GNSS = "Reset GNSS using reset pin"
class SetId(enum.IntEnum): class SetId(enum.IntEnum):
HK = 0 CORE_HK = 0
SKYVIEW_HK = 1
@tmtc_definitions_provider @tmtc_definitions_provider
def add_gps_cmds(defs: TmtcDefinitionWrapper): def add_gps_cmds(defs: TmtcDefinitionWrapper):
oce = OpCodeEntry() 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)
oce.add(keys=OpCode.OFF, info=Info.OFF) oce.add(keys=OpCode.OFF, info=Info.OFF)
oce.add(keys=OpCode.ON, info=Info.ON) 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( defs.add_service(
name=CustomServiceList.GPS_CTRL.value, name=CustomServiceList.GPS_CTRL.value,
info="GPS/GNSS Controller", info="GPS/GNSS Controller",
@ -59,26 +71,43 @@ def add_gps_cmds(defs: TmtcDefinitionWrapper):
def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str): 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: if op_code in OpCode.RESET_GNSS:
# TODO: This needs to be re-implemented # TODO: This needs to be re-implemented
_LOGGER.warning("Reset pin handling needs to be re-implemented") _LOGGER.warning("Reset pin handling needs to be re-implemented")
if op_code in OpCode.ENABLE_HK: if op_code in OpCode.ENABLE_CORE_HK:
interval = float(input("Please specify interval in floating point seconds: ")) interval = float(input("Please specify interval in floating point seconds: "))
if interval <= 0: if interval <= 0:
raise ValueError("invalid interval") raise ValueError("invalid interval")
q.add_log_cmd(f"GPS: {Info.ENABLE_HK}") q.add_log_cmd(f"GPS: {Info.ENABLE_CORE_HK}")
cmds = create_enable_periodic_hk_command_with_interval( cmds = create_enable_periodic_hk_command_with_interval(
diag=False, sid=sid, interval_seconds=interval diag=False, sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK), interval_seconds=interval
) )
for cmd in cmds: for cmd in cmds:
q.add_pus_tc(cmd) q.add_pus_tc(cmd)
if op_code in OpCode.DISABLE_HK: if op_code in OpCode.DISABLE_CORE_HK:
q.add_log_cmd(f"gps: {Info.DISABLE_HK}") q.add_log_cmd(f"gps: {Info.DISABLE_CORE_HK}")
q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=sid)) q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=make_sid(object_id=object_id,
if op_code in OpCode.REQ_OS_HK: set_id=SetId.CORE_HK)))
q.add_log_cmd(f"GPS: {Info.REQ_OS_HK}") if op_code in OpCode.REQ_CORE_HK:
q.add_pus_tc(create_request_one_hk_command(sid=sid)) q.add_log_cmd(f"GPS: {Info.REQ_CORE_HK}")
q.add_pus_tc(create_request_one_hk_command(sid=make_sid(object_id=object_id, set_id=SetId.CORE_HK)))
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(
diag=False, sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK), interval_seconds=interval
)
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}")
q.add_pus_tc(create_disable_periodic_hk_command(diag=False, sid=make_sid(object_id=object_id,
set_id=SetId.SKYVIEW_HK)))
if op_code in OpCode.REQ_SKYVIEW_HK:
q.add_log_cmd(f"GPS: {Info.REQ_SKYVIEW_HK}")
q.add_pus_tc(create_request_one_hk_command(sid=make_sid(object_id=object_id, set_id=SetId.SKYVIEW_HK)))
if op_code in OpCode.ON: if op_code in OpCode.ON:
q.add_log_cmd(f"GPS: {Info.ON}") q.add_log_cmd(f"GPS: {Info.ON}")
q.add_pus_tc(create_mode_command(object_id, Mode.ON, 0)) q.add_pus_tc(create_mode_command(object_id, Mode.ON, 0))
@ -87,8 +116,27 @@ def pack_gps_command(object_id: bytes, q: DefaultPusQueueHelper, op_code: str):
q.add_pus_tc(create_mode_command(object_id, Mode.OFF, 0)) q.add_pus_tc(create_mode_command(object_id, Mode.OFF, 0))
def handle_gps_data(pw: PrintWrapper, hk_data: bytes): def handle_gps_data(
pw.dlog(f"Received GPS data, HK data length {len(hk_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_skyview_data(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 current_idx = 0
fmt_str = "!ddddBBBHBBBBBI" fmt_str = "!ddddBBBHBBBBBI"
inc_len = struct.calcsize(fmt_str) inc_len = struct.calcsize(fmt_str)
@ -126,3 +174,43 @@ def handle_gps_data(pw: PrintWrapper, hk_data: bytes):
FsfwTmTcPrinter.get_validity_buffer( FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=hk_data[current_idx:], num_vars=14 validity_buffer=hk_data[current_idx:], num_vars=14
) )
def handle_skyview_data(pw: PrintWrapper, hk_data: bytes):
data_length = 8+GpsInfo.MAX_SATELLITES*(8+3*2+1)
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)
unix = struct.unpack(fmt_str_unix, hk_data[current_idx: current_idx + inc_len_unix])
current_idx += inc_len_unix
prn_id = struct.unpack(fmt_str_int16, hk_data[current_idx: current_idx + inc_len_int16])
current_idx += inc_len_int16
azimuth = struct.unpack(fmt_str_int16, hk_data[current_idx: current_idx + inc_len_int16])
current_idx += inc_len_int16
elevation = struct.unpack(fmt_str_int16, hk_data[current_idx: current_idx + inc_len_int16])
current_idx += inc_len_int16
signal_to_noise = struct.unpack(fmt_str_double, hk_data[current_idx: current_idx + inc_len_double])
current_idx += inc_len_double
used = struct.unpack(fmt_str_uint8, hk_data[current_idx: current_idx + inc_len_uint8])
current_idx += inc_len_uint8
pw.dlog(f"Skyview Time: {unix} unix-sec")
pw.dlog("{:<8} {:<8} {:<8} {:<8} {:<8}".format('PRN_ID', 'AZ [°]', 'EL [°]', 'S2N [dBW]', 'USED'))
for idx in range(GpsInfo.MAX_SATELLITES):
pw.dlog("{:<8} {:<8} {:<8} {:<8} {:<8}".format(prn_id[idx], azimuth[idx], elevation[idx], signal_to_noise[idx],
used[idx]))
FsfwTmTcPrinter.get_validity_buffer(
validity_buffer=hk_data[current_idx:], num_vars=6
)