eive-tmtc/eive_tmtc/pus_tm/action_reply_handler.py

import struct
from eive_tmtc.config.object_ids import *
from eive_tmtc.tmtc.acs.imtq import ImtqActionId
from eive_tmtc.pus_tm.defs import PrintWrapper
from eive_tmtc.tmtc.core import handle_core_ctrl_action_replies
from eive_tmtc.tmtc.payload.ploc_mpsoc import handle_mpsoc_data_reply
from eive_tmtc.tmtc.payload.ploc_supervisor import SupvActionId
from eive_tmtc.tmtc.acs.star_tracker import StarTrackerActionId
from eive_tmtc.tmtc.power.tm import handle_get_param_data_reply
from tmtccmd.tm import Service8FsfwTm
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
from spacepackets.ccsds.time import CdsShortTimestamp

_LOGGER = logging.getLogger(__name__)


def handle_action_reply(
    raw_tm: bytes, printer: FsfwTmTcPrinter, obj_id_dict: ObjectIdDictT
):
    """Core Action reply handler
    :return:
    """
    tm_packet = Service8FsfwTm.unpack(
        raw_telemetry=raw_tm, time_reader=CdsShortTimestamp.empty()
    )
    object_id = obj_id_dict.get(tm_packet.source_object_id_as_bytes)
    pw = PrintWrapper(printer)
    custom_data = tm_packet.custom_data
    action_id = tm_packet.action_id
    generic_print_str = printer.generic_action_packet_tm_print(
        packet=tm_packet, obj_id=object_id
    )
    pw.dlog(generic_print_str)
    if object_id.as_bytes == IMTQ_HANDLER_ID:
        return handle_imtq_replies(action_id, printer, custom_data)
    elif object_id.as_bytes == PLOC_MPSOC_ID:
        return handle_mpsoc_data_reply(action_id, printer, custom_data)
    elif object_id.as_bytes == PLOC_SUPV_ID:
        return handle_supervisor_replies(action_id, printer, custom_data)
    elif object_id.as_bytes == CORE_CONTROLLER_ID:
        return handle_core_ctrl_action_replies(action_id, printer, custom_data)
    elif object_id.as_bytes == STAR_TRACKER_ID:
        return handle_startracker_replies(action_id, printer, custom_data)
    elif object_id.as_bytes in [
        ACU_HANDLER_ID,
        PDU_1_HANDLER_ID,
        PDU_2_HANDLER_ID,
        P60_DOCK_HANDLER,
    ]:
        return handle_get_param_data_reply(object_id, action_id, pw, custom_data)
    else:
        pw.dlog(f"No dedicated action reply handler found for reply from {object_id}")
        pw.dlog(f"Raw Data: {tm_packet.custom_data.hex(sep=',')}")


def handle_imtq_replies(
    action_id: int, printer: FsfwTmTcPrinter, custom_data: bytearray
):
    if action_id == struct.unpack("!I", ImtqActionId.get_commanded_dipole)[0]:
        header_list = [
            "Commanded X-Dipole",
            "Commanded Y-Dipole",
            "Commanded Z-Dipole",
        ]
        [x_dipole, y_dipole, z_dipole] = struct.unpack("!HHH", custom_data[0:6])
        content_list = [x_dipole, y_dipole, z_dipole]
        print(header_list)
        print(content_list)
        printer.file_logger.info(header_list)
        printer.file_logger.info(content_list)


def handle_supervisor_replies(
    action_id: int, printer: FsfwTmTcPrinter, custom_data: bytearray
):
    if action_id == SupvActionId.DUMP_MRAM:
        header_list = ["MRAM Dump"]
        content_list = [custom_data[: len(custom_data)]]
        print(header_list)
        print(content_list)
        printer.file_logger.info(header_list)
        printer.file_logger.info(content_list)
    elif action_id == SupvActionId.READ_GPIO:
        header_list = ["GPIO state"]
        content_list = [struct.unpack("!H", custom_data[:2])[0]]
        print(header_list)
        print(content_list)
        printer.file_logger.info(header_list)
        printer.file_logger.info(content_list)


def handle_startracker_replies(
    action_id: int, printer: FsfwTmTcPrinter, custom_data: bytearray
):
    if action_id == StarTrackerActionId.CHECKSUM:
        if len(custom_data) != 5:
            _LOGGER.warning(
                "Star tracker reply has invalid length {0}".format(len(custom_data))
            )
            return
        header_list = ["Checksum", "Checksum valid"]
        print(custom_data[4])
        checksum_valid_flag = custom_data[4] >> 8
        content_list = ["0x" + custom_data[:4].hex(), checksum_valid_flag]
        print(header_list)
        print(content_list)
        printer.file_logger.info(header_list)
        printer.file_logger.info(content_list)