from typing import Union from spacepackets.ecss.tc import PusTelecommand from tmtccmd.tmtc import DefaultPusQueueHelper from tmtccmd.pus.s200_fsfw_mode import pack_mode_data, Mode, Subservice from tmtccmd.util import ObjectIdU32 def pack_mode_cmd_with_info( object_id: Union[ObjectIdU32, bytes], mode: Union[int, Mode], submode: int, q: DefaultPusQueueHelper, info: str, ): if isinstance(object_id, ObjectIdU32): object_id_bytes = object_id.as_bytes elif isinstance(object_id, bytes): object_id_bytes = object_id else: raise ValueError("Invalid Object ID type") q.add_log_cmd(info) mode_data = pack_mode_data( object_id=object_id_bytes, mode=mode, submode=submode, ) q.add_pus_tc( PusTelecommand( service=200, subservice=Subservice.TC_MODE_COMMAND, app_data=mode_data ) )