eive-tmtc/eive_tmtc/pus_tm/action_reply_handler.py

import base64
import logging
import struct

from spacepackets.ecss import PusTm
from tmtccmd.pus.s8_fsfw_action_defs import CustomSubservice
from eive_tmtc.config.definitions import PRINT_RAW_ACTION_DATA_REPLY_B64_STR
from eive_tmtc.config.object_ids import (
    ACU_HANDLER_ID,
    PDU_1_HANDLER_ID,
    PDU_2_HANDLER_ID,
    IMTQ_HANDLER_ID,
    PLOC_MPSOC_ID,
    PLOC_SUPV_ID,
    CORE_CONTROLLER_ID,
    STAR_TRACKER_ID,
    P60_DOCK_HANDLER,
    ACS_CONTROLLER,
)
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 handle_star_tracker_action_replies
from eive_tmtc.tmtc.acs.acs_ctrl import handle_acs_ctrl_action_replies
from eive_tmtc.tmtc.power.tm import handle_get_param_data_reply
from spacepackets.ccsds.time import CdsShortTimestamp
from tmtccmd.util import ObjectIdBase, ObjectIdDictT, ObjectIdU32

_LOGGER = logging.getLogger(__name__)


_LOGGER = logging.getLogger(__name__)


def handle_action_service_tm(
    raw_tm: bytes, pw: PrintWrapper, obj_id_dict: ObjectIdDictT
):
    """Core Action reply handler
    :return:
    """
    tm_packet = PusTm.unpack(raw_tm, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE)
    if len(tm_packet.source_data) < 8:
        _LOGGER.warning(
            "received action service reply with source data smaller than 8 bytes"
        )
        return
    object_id_raw = struct.unpack("!I", tm_packet.source_data[0:4])[0]
    action_id = struct.unpack("!I", tm_packet.source_data[4:8])[0]
    object_id = obj_id_dict.get(object_id_raw)
    custom_data = tm_packet.source_data[8:]
    if object_id is None:
        object_id = ObjectIdU32(object_id_raw, "Unknown ID")
    if tm_packet.subservice == CustomSubservice.TM_DATA_REPLY:
        if PRINT_RAW_ACTION_DATA_REPLY_B64_STR:
            print(f"PUS TM Base64: {base64.b64encode(raw_tm)}")
        if object_id.as_bytes == IMTQ_HANDLER_ID:
            return handle_imtq_replies(action_id, pw, custom_data)
        elif object_id.as_bytes == PLOC_MPSOC_ID:
            return handle_mpsoc_data_reply(action_id, pw, custom_data)
        elif object_id.as_bytes == PLOC_SUPV_ID:
            return handle_supervisor_replies(action_id, pw, custom_data)
        elif object_id.as_bytes == CORE_CONTROLLER_ID:
            return handle_core_ctrl_action_replies(action_id, pw, custom_data)
        elif object_id.as_bytes == STAR_TRACKER_ID:
            return handle_star_tracker_action_replies(action_id, pw, custom_data)
        elif object_id.as_bytes == ACS_CONTROLLER:
            return handle_acs_ctrl_action_replies(action_id, pw, 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:
            # TODO: Could add a handler here depending on action ID and object ID.
            handle_action_data_reply(tm_packet, object_id, action_id, pw)
    else:
        pw.dlog(
            f"service 8 packet from {object_id} with action ID {action_id} "
            f"and unknown subservice {tm_packet.subservice}"
        )


def handle_action_data_reply(
    tm_packet: PusTm, named_obj_id: ObjectIdBase, action_id: int, printer: PrintWrapper
):
    print_string = (
        f"service 8 data reply from {named_obj_id} with action ID {action_id} "
        f"and data size {len(tm_packet.tm_data[8:])}"
    )
    printer.dlog(print_string)


def handle_imtq_replies(action_id: int, pw: PrintWrapper, custom_data: bytes):
    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]
        pw.dlog(f"{header_list}")
        pw.dlog(f"{content_list}")


def handle_supervisor_replies(action_id: int, pw: PrintWrapper, custom_data: bytes):
    if action_id == SupvActionId.DUMP_MRAM:
        header_list = ["MRAM Dump"]
        content_list = [custom_data[: len(custom_data)]]
        pw.dlog(f"{header_list}")
        pw.dlog(f"{content_list}")
    elif action_id == SupvActionId.READ_GPIO:
        header_list = ["GPIO state"]
        content_list = [struct.unpack("!H", custom_data[:2])[0]]
        pw.dlog(f"{header_list}")
        pw.dlog(f"{content_list}")