diff --git a/images/minisim-arch/minisim-arch.graphml b/images/minisim-arch/minisim-arch.graphml new file mode 100644 index 0000000..6eff044 --- /dev/null +++ b/images/minisim-arch/minisim-arch.graphml @@ -0,0 +1,260 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Simulation + + + + + + + + + + + PCDU + + + + + + + + + + + Magnetometer + + + + + + + + + + + Magnetorquer + + + + + + + + + + + SimController + + + + + + + + + + + UDP TC Receiver + + + + + + + + + + + UDP TM Sender + + + + + + + + + + + Client + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + schedule_event + + + + + + + + + + + + + + SimRequest + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + SimReply + + + + + + + + + + + + + + SimRequest in UDP + + + + + + + + + + + + + + SimReply in UDP + + + + + + + + + + + + + step + + + + + + + + + diff --git a/images/minisim-arch/minisim-arch.png b/images/minisim-arch/minisim-arch.png new file mode 100644 index 0000000..8e384ad Binary files /dev/null and b/images/minisim-arch/minisim-arch.png differ diff --git a/satrs-example/Cargo.toml b/satrs-example/Cargo.toml index b22904b..3749cff 100644 --- a/satrs-example/Cargo.toml +++ b/satrs-example/Cargo.toml @@ -27,6 +27,9 @@ serde_json = "1" path = "../satrs" features = ["test_util"] +[dependencies.satrs-minisim] +path = "../satrs-minisim" + [dependencies.satrs-mib] version = "0.1.1" path = "../satrs-mib" diff --git a/satrs-example/README.md b/satrs-example/README.md index 3447a0d..b661423 100644 --- a/satrs-example/README.md +++ b/satrs-example/README.md @@ -48,16 +48,17 @@ It is recommended to use a virtual environment to do this. To set up one in the you can use `python3 -m venv venv` on Unix systems or `py -m venv venv` on Windows systems. After doing this, you can check the [venv tutorial](https://docs.python.org/3/tutorial/venv.html) on how to activate the environment and then use the following command to install the required -dependency: +dependency interactively: ```sh -pip install -r requirements.txt +pip install -e . ``` Alternatively, if you would like to use the GUI functionality provided by `tmtccmd`, you can also install it manually with ```sh +pip install -e . pip install tmtccmd[gui] ``` @@ -72,3 +73,22 @@ the `simpleclient`: ``` You can also simply call the script without any arguments to view the command tree. + +## Adding the mini simulator application + +This example application features a few device handlers. The +[`satrs-minisim`](https://egit.irs.uni-stuttgart.de/rust/sat-rs/src/branch/main/satrs-minisim) +can be used to simulate the physical devices managed by these device handlers. + +The example application will attempt communication with the mini simulator on UDP port 7303. +If this works, the device handlers will use communication interfaces dedicated to the communication +with the mini simulator. Otherwise, they will be replaced by dummy interfaces which either +return constant values or behave like ideal devices. + +In summary, you can use the following command command to run the mini-simulator first: + +```sh +cargo run -p satrs-minisim +``` + +and then start the example using `cargo run -p satrs-example`. diff --git a/satrs-example/pytmtc/.gitignore b/satrs-example/pytmtc/.gitignore index d994678..008bdd0 100644 --- a/satrs-example/pytmtc/.gitignore +++ b/satrs-example/pytmtc/.gitignore @@ -1,3 +1,4 @@ +/tmtc_conf.json __pycache__ /venv @@ -7,3 +8,136 @@ __pycache__ /seqcnt.txt /.tmtc-history.txt + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +.python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# PyCharm +.idea diff --git a/satrs-example/pytmtc/main.py b/satrs-example/pytmtc/main.py index a90a011..d8a55b8 100755 --- a/satrs-example/pytmtc/main.py +++ b/satrs-example/pytmtc/main.py @@ -3,27 +3,17 @@ import logging import sys import time -from typing import Any, Optional -from prompt_toolkit.history import History -from prompt_toolkit.history import FileHistory -from spacepackets.ccsds import PacketId, PacketType import tmtccmd -from spacepackets.ecss import PusTelemetry, PusVerificator -from spacepackets.ecss.pus_17_test import Service17Tm -from spacepackets.ecss.pus_1_verification import UnpackParams, Service1Tm -from spacepackets.ccsds.time import CdsShortTimestamp +from spacepackets.ecss import PusVerificator -from tmtccmd import TcHandlerBase, ProcedureParamsWrapper +from tmtccmd import ProcedureParamsWrapper from tmtccmd.core.base import BackendRequest from tmtccmd.pus import VerificationWrapper -from tmtccmd.tmtc import CcsdsTmHandler, GenericApidHandlerBase -from tmtccmd.com import ComInterface +from tmtccmd.tmtc import CcsdsTmHandler from tmtccmd.config import ( - CmdTreeNode, default_json_path, SetupParams, - HookBase, params_to_procedure_conversion, ) from tmtccmd.config import PreArgsParsingWrapper, SetupWrapper @@ -33,193 +23,20 @@ from tmtccmd.logging.pus import ( RawTmtcTimedLogWrapper, TimedLogWhen, ) -from tmtccmd.tmtc import ( - TcQueueEntryType, - ProcedureWrapper, - TcProcedureType, - FeedWrapper, - SendCbParams, - DefaultPusQueueHelper, - QueueWrapper, -) -from spacepackets.seqcount import FileSeqCountProvider, PusFileSeqCountProvider -from tmtccmd.util.obj_id import ObjectIdDictT +from spacepackets.seqcount import PusFileSeqCountProvider -import pus_tc -from common import Apid, EventU32 +from pytmtc.config import SatrsConfigHook +from pytmtc.pus_tc import TcHandler +from pytmtc.pus_tm import PusHandler _LOGGER = logging.getLogger() -class SatRsConfigHook(HookBase): - def __init__(self, json_cfg_path: str): - super().__init__(json_cfg_path=json_cfg_path) - - def get_communication_interface(self, com_if_key: str) -> Optional[ComInterface]: - from tmtccmd.config.com import ( - create_com_interface_default, - create_com_interface_cfg_default, - ) - - assert self.cfg_path is not None - packet_id_list = [] - for apid in Apid: - packet_id_list.append(PacketId(PacketType.TM, True, apid)) - cfg = create_com_interface_cfg_default( - com_if_key=com_if_key, - json_cfg_path=self.cfg_path, - space_packet_ids=packet_id_list, - ) - assert cfg is not None - return create_com_interface_default(cfg) - - def get_command_definitions(self) -> CmdTreeNode: - """This function should return the root node of the command definition tree.""" - return pus_tc.create_cmd_definition_tree() - - def get_cmd_history(self) -> Optional[History]: - """Optionlly return a history class for the past command paths which will be used - when prompting a command path from the user in CLI mode.""" - return FileHistory(".tmtc-history.txt") - - def get_object_ids(self) -> ObjectIdDictT: - from tmtccmd.config.objects import get_core_object_ids - - return get_core_object_ids() - - -class PusHandler(GenericApidHandlerBase): - def __init__( - self, - file_logger: logging.Logger, - verif_wrapper: VerificationWrapper, - raw_logger: RawTmtcTimedLogWrapper, - ): - super().__init__(None) - self.file_logger = file_logger - self.raw_logger = raw_logger - self.verif_wrapper = verif_wrapper - - def handle_tm(self, apid: int, packet: bytes, _user_args: Any): - try: - pus_tm = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - except ValueError as e: - _LOGGER.warning("Could not generate PUS TM object from raw data") - _LOGGER.warning(f"Raw Packet: [{packet.hex(sep=',')}], REPR: {packet!r}") - raise e - service = pus_tm.service - if service == 1: - tm_packet = Service1Tm.unpack( - data=packet, params=UnpackParams(CdsShortTimestamp.TIMESTAMP_SIZE, 1, 2) - ) - res = self.verif_wrapper.add_tm(tm_packet) - if res is None: - _LOGGER.info( - f"Received Verification TM[{tm_packet.service}, {tm_packet.subservice}] " - f"with Request ID {tm_packet.tc_req_id.as_u32():#08x}" - ) - _LOGGER.warning( - f"No matching telecommand found for {tm_packet.tc_req_id}" - ) - else: - self.verif_wrapper.log_to_console(tm_packet, res) - self.verif_wrapper.log_to_file(tm_packet, res) - elif service == 3: - _LOGGER.info("No handling for HK packets implemented") - _LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]") - pus_tm = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - if pus_tm.subservice == 25: - if len(pus_tm.source_data) < 8: - raise ValueError("No addressable ID in HK packet") - json_str = pus_tm.source_data[8:] - _LOGGER.info(json_str) - elif service == 5: - tm_packet = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - src_data = tm_packet.source_data - event_u32 = EventU32.unpack(src_data) - _LOGGER.info( - f"Received event packet. Source APID: {Apid(tm_packet.apid)!r}, Event: {event_u32}" - ) - if event_u32.group_id == 0 and event_u32.unique_id == 0: - _LOGGER.info("Received test event") - elif service == 17: - tm_packet = Service17Tm.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - if tm_packet.subservice == 2: - self.file_logger.info("Received Ping Reply TM[17,2]") - _LOGGER.info("Received Ping Reply TM[17,2]") - else: - self.file_logger.info( - f"Received Test Packet with unknown subservice {tm_packet.subservice}" - ) - _LOGGER.info( - f"Received Test Packet with unknown subservice {tm_packet.subservice}" - ) - else: - _LOGGER.info( - f"The service {service} is not implemented in Telemetry Factory" - ) - tm_packet = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - self.raw_logger.log_tm(pus_tm) - - -class TcHandler(TcHandlerBase): - def __init__( - self, - seq_count_provider: FileSeqCountProvider, - verif_wrapper: VerificationWrapper, - ): - super(TcHandler, self).__init__() - self.seq_count_provider = seq_count_provider - self.verif_wrapper = verif_wrapper - self.queue_helper = DefaultPusQueueHelper( - queue_wrapper=QueueWrapper.empty(), - tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE, - seq_cnt_provider=seq_count_provider, - pus_verificator=self.verif_wrapper.pus_verificator, - default_pus_apid=None, - ) - - def send_cb(self, send_params: SendCbParams): - entry_helper = send_params.entry - if entry_helper.is_tc: - if entry_helper.entry_type == TcQueueEntryType.PUS_TC: - pus_tc_wrapper = entry_helper.to_pus_tc_entry() - raw_tc = pus_tc_wrapper.pus_tc.pack() - _LOGGER.info(f"Sending {pus_tc_wrapper.pus_tc}") - send_params.com_if.send(raw_tc) - elif entry_helper.entry_type == TcQueueEntryType.LOG: - log_entry = entry_helper.to_log_entry() - _LOGGER.info(log_entry.log_str) - - def queue_finished_cb(self, info: ProcedureWrapper): - if info.proc_type == TcProcedureType.TREE_COMMANDING: - def_proc = info.to_tree_commanding_procedure() - _LOGGER.info(f"Queue handling finished for command {def_proc.cmd_path}") - - def feed_cb(self, info: ProcedureWrapper, wrapper: FeedWrapper): - q = self.queue_helper - q.queue_wrapper = wrapper.queue_wrapper - if info.proc_type == TcProcedureType.TREE_COMMANDING: - def_proc = info.to_tree_commanding_procedure() - assert def_proc.cmd_path is not None - pus_tc.pack_pus_telecommands(q, def_proc.cmd_path) - - def main(): add_colorlog_console_logger(_LOGGER) tmtccmd.init_printout(False) - hook_obj = SatRsConfigHook(json_cfg_path=default_json_path()) + hook_obj = SatrsConfigHook(json_cfg_path=default_json_path()) parser_wrapper = PreArgsParsingWrapper() parser_wrapper.create_default_parent_parser() parser_wrapper.create_default_parser() diff --git a/satrs-example/pytmtc/pyproject.toml b/satrs-example/pytmtc/pyproject.toml new file mode 100644 index 0000000..fcb4a32 --- /dev/null +++ b/satrs-example/pytmtc/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "pytmtc" +description = "Python TMTC client for OPS-SAT" +readme = "README.md" +version = "0.1.0" +requires-python = ">=3.8" +authors = [ + {name = "Robin Mueller", email = "robin.mueller.m@gmail.com"}, +] +dependencies = [ + "tmtccmd~=8.0", + "pydantic~=2.7" +] + +[tool.setuptools.packages] +find = {} + +[tool.ruff] +extend-exclude = ["archive"] +[tool.ruff.lint] +ignore = ["E501"] +[tool.ruff.lint.extend-per-file-ignores] +"__init__.py" = ["F401"] diff --git a/satrs-example/pytmtc/pus_tm.py b/satrs-example/pytmtc/pytmtc/__init__.py similarity index 100% rename from satrs-example/pytmtc/pus_tm.py rename to satrs-example/pytmtc/pytmtc/__init__.py diff --git a/satrs-example/pytmtc/common.py b/satrs-example/pytmtc/pytmtc/common.py similarity index 100% rename from satrs-example/pytmtc/common.py rename to satrs-example/pytmtc/pytmtc/common.py diff --git a/satrs-example/pytmtc/pytmtc/config.py b/satrs-example/pytmtc/pytmtc/config.py new file mode 100644 index 0000000..6647769 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/config.py @@ -0,0 +1,47 @@ +from typing import Optional +from prompt_toolkit.history import FileHistory, History +from spacepackets.ccsds import PacketId, PacketType +from tmtccmd import HookBase +from tmtccmd.com import ComInterface +from tmtccmd.config import CmdTreeNode +from tmtccmd.util.obj_id import ObjectIdDictT + +from pytmtc.common import Apid +from pytmtc.pus_tc import create_cmd_definition_tree + + +class SatrsConfigHook(HookBase): + def __init__(self, json_cfg_path: str): + super().__init__(json_cfg_path=json_cfg_path) + + def get_communication_interface(self, com_if_key: str) -> Optional[ComInterface]: + from tmtccmd.config.com import ( + create_com_interface_default, + create_com_interface_cfg_default, + ) + + assert self.cfg_path is not None + packet_id_list = [] + for apid in Apid: + packet_id_list.append(PacketId(PacketType.TM, True, apid)) + cfg = create_com_interface_cfg_default( + com_if_key=com_if_key, + json_cfg_path=self.cfg_path, + space_packet_ids=packet_id_list, + ) + assert cfg is not None + return create_com_interface_default(cfg) + + def get_command_definitions(self) -> CmdTreeNode: + """This function should return the root node of the command definition tree.""" + return create_cmd_definition_tree() + + def get_cmd_history(self) -> Optional[History]: + """Optionlly return a history class for the past command paths which will be used + when prompting a command path from the user in CLI mode.""" + return FileHistory(".tmtc-history.txt") + + def get_object_ids(self) -> ObjectIdDictT: + from tmtccmd.config.objects import get_core_object_ids + + return get_core_object_ids() diff --git a/satrs-example/pytmtc/pytmtc/hk.py b/satrs-example/pytmtc/pytmtc/hk.py new file mode 100644 index 0000000..6e8aa71 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/hk.py @@ -0,0 +1,42 @@ +import logging +import struct +from spacepackets.ecss.pus_3_hk import Subservice +from spacepackets.ecss import PusTm + +from pytmtc.common import AcsId, Apid +from pytmtc.mgms import handle_mgm_hk_report + + +_LOGGER = logging.getLogger(__name__) + + +def handle_hk_packet(pus_tm: PusTm): + if len(pus_tm.source_data) < 4: + raise ValueError("no unique ID in HK packet") + unique_id = struct.unpack("!I", pus_tm.source_data[:4])[0] + if ( + pus_tm.subservice == Subservice.TM_HK_REPORT + or pus_tm.subservice == Subservice.TM_DIAGNOSTICS_REPORT + ): + if len(pus_tm.source_data) < 8: + raise ValueError("no set ID in HK packet") + set_id = struct.unpack("!I", pus_tm.source_data[4:8])[0] + handle_hk_report(pus_tm, unique_id, set_id) + _LOGGER.warning( + f"handling for HK packet with subservice {pus_tm.subservice} not implemented yet" + ) + + +def handle_hk_report(pus_tm: PusTm, unique_id: int, set_id: int): + hk_data = pus_tm.source_data[8:] + if pus_tm.apid == Apid.ACS: + if unique_id == AcsId.MGM_0: + handle_mgm_hk_report(pus_tm, set_id, hk_data) + else: + _LOGGER.warning( + f"handling for HK report with unique ID {unique_id} not implemented yet" + ) + else: + _LOGGER.warning( + f"handling for HK report with apid {pus_tm.apid} not implemented yet" + ) diff --git a/satrs-example/pytmtc/pytmtc/hk_common.py b/satrs-example/pytmtc/pytmtc/hk_common.py new file mode 100644 index 0000000..bb9890a --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/hk_common.py @@ -0,0 +1,16 @@ +import struct + +from spacepackets.ecss import PusService, PusTc +from spacepackets.ecss.pus_3_hk import Subservice + + +def create_request_one_shot_hk_cmd(apid: int, unique_id: int, set_id: int) -> PusTc: + app_data = bytearray() + app_data.extend(struct.pack("!I", unique_id)) + app_data.extend(struct.pack("!I", set_id)) + return PusTc( + service=PusService.S3_HOUSEKEEPING, + subservice=Subservice.TC_GENERATE_ONE_PARAMETER_REPORT, + apid=apid, + app_data=app_data, + ) diff --git a/satrs-example/pytmtc/pytmtc/mgms.py b/satrs-example/pytmtc/pytmtc/mgms.py new file mode 100644 index 0000000..d420b3e --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/mgms.py @@ -0,0 +1,45 @@ +import logging +import struct +import enum +from typing import List +from spacepackets.ecss import PusTm +from tmtccmd.tmtc import DefaultPusQueueHelper + +from pytmtc.common import AcsId, Apid +from pytmtc.hk_common import create_request_one_shot_hk_cmd +from pytmtc.mode import handle_set_mode_cmd + + +_LOGGER = logging.getLogger(__name__) + + +class SetId(enum.IntEnum): + SENSOR_SET = 0 + + +def create_mgm_cmds(q: DefaultPusQueueHelper, cmd_path: List[str]): + assert len(cmd_path) >= 3 + if cmd_path[2] == "hk": + if cmd_path[3] == "one_shot_hk": + q.add_log_cmd("Sending HK one shot request") + q.add_pus_tc( + create_request_one_shot_hk_cmd(Apid.ACS, AcsId.MGM_0, SetId.SENSOR_SET) + ) + + if cmd_path[2] == "mode": + if cmd_path[3] == "set_mode": + handle_set_mode_cmd(q, "MGM 0", cmd_path[4], Apid.ACS, AcsId.MGM_0) + + +def handle_mgm_hk_report(pus_tm: PusTm, set_id: int, hk_data: bytes): + if set_id == SetId.SENSOR_SET: + if len(hk_data) != 13: + raise ValueError(f"invalid HK data length, expected 13, got {len(hk_data)}") + data_valid = hk_data[0] + mgm_x = struct.unpack("!f", hk_data[1:5])[0] + mgm_y = struct.unpack("!f", hk_data[5:9])[0] + mgm_z = struct.unpack("!f", hk_data[9:13])[0] + _LOGGER.info( + f"received MGM HK set in uT: Valid {data_valid} X {mgm_x} Y {mgm_y} Z {mgm_z}" + ) + pass diff --git a/satrs-example/pytmtc/pytmtc/mode.py b/satrs-example/pytmtc/pytmtc/mode.py new file mode 100644 index 0000000..918fdb1 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/mode.py @@ -0,0 +1,31 @@ +import struct +from spacepackets.ecss import PusTc +from tmtccmd.pus.s200_fsfw_mode import Mode, Subservice +from tmtccmd.tmtc import DefaultPusQueueHelper + + +def create_set_mode_cmd(apid: int, unique_id: int, mode: int, submode: int) -> PusTc: + app_data = bytearray() + app_data.extend(struct.pack("!I", unique_id)) + app_data.extend(struct.pack("!I", mode)) + app_data.extend(struct.pack("!H", submode)) + return PusTc( + service=200, + subservice=Subservice.TC_MODE_COMMAND, + apid=apid, + app_data=app_data, + ) + + +def handle_set_mode_cmd( + q: DefaultPusQueueHelper, target_str: str, mode_str: str, apid: int, unique_id: int +): + if mode_str == "off": + q.add_log_cmd(f"Sending Mode OFF to {target_str}") + q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.OFF, 0)) + elif mode_str == "on": + q.add_log_cmd(f"Sending Mode ON to {target_str}") + q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.ON, 0)) + elif mode_str == "normal": + q.add_log_cmd(f"Sending Mode NORMAL to {target_str}") + q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.NORMAL, 0)) diff --git a/satrs-example/pytmtc/pus_tc.py b/satrs-example/pytmtc/pytmtc/pus_tc.py similarity index 60% rename from satrs-example/pytmtc/pus_tc.py rename to satrs-example/pytmtc/pytmtc/pus_tc.py index b0febdc..6e1570c 100644 --- a/satrs-example/pytmtc/pus_tc.py +++ b/satrs-example/pytmtc/pytmtc/pus_tc.py @@ -1,33 +1,69 @@ import datetime -import struct import logging from spacepackets.ccsds import CdsShortTimestamp from spacepackets.ecss import PusTelecommand +from spacepackets.seqcount import FileSeqCountProvider +from tmtccmd import ProcedureWrapper, TcHandlerBase from tmtccmd.config import CmdTreeNode -from tmtccmd.pus.tc.s200_fsfw_mode import Mode -from tmtccmd.tmtc import DefaultPusQueueHelper +from tmtccmd.pus import VerificationWrapper +from tmtccmd.tmtc import ( + DefaultPusQueueHelper, + FeedWrapper, + QueueWrapper, + SendCbParams, + TcProcedureType, + TcQueueEntryType, +) from tmtccmd.pus.s11_tc_sched import create_time_tagged_cmd -from tmtccmd.pus.s200_fsfw_mode import Subservice as ModeSubservice -from common import AcsId, Apid +from pytmtc.common import Apid +from pytmtc.mgms import create_mgm_cmds _LOGGER = logging.getLogger(__name__) -def create_set_mode_cmd( - apid: int, unique_id: int, mode: int, submode: int -) -> PusTelecommand: - app_data = bytearray() - app_data.extend(struct.pack("!I", unique_id)) - app_data.extend(struct.pack("!I", mode)) - app_data.extend(struct.pack("!H", submode)) - return PusTelecommand( - service=200, - subservice=ModeSubservice.TC_MODE_COMMAND, - apid=apid, - app_data=app_data, - ) +class TcHandler(TcHandlerBase): + def __init__( + self, + seq_count_provider: FileSeqCountProvider, + verif_wrapper: VerificationWrapper, + ): + super(TcHandler, self).__init__() + self.seq_count_provider = seq_count_provider + self.verif_wrapper = verif_wrapper + self.queue_helper = DefaultPusQueueHelper( + queue_wrapper=QueueWrapper.empty(), + tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE, + seq_cnt_provider=seq_count_provider, + pus_verificator=self.verif_wrapper.pus_verificator, + default_pus_apid=None, + ) + + def send_cb(self, send_params: SendCbParams): + entry_helper = send_params.entry + if entry_helper.is_tc: + if entry_helper.entry_type == TcQueueEntryType.PUS_TC: + pus_tc_wrapper = entry_helper.to_pus_tc_entry() + raw_tc = pus_tc_wrapper.pus_tc.pack() + _LOGGER.info(f"Sending {pus_tc_wrapper.pus_tc}") + send_params.com_if.send(raw_tc) + elif entry_helper.entry_type == TcQueueEntryType.LOG: + log_entry = entry_helper.to_log_entry() + _LOGGER.info(log_entry.log_str) + + def queue_finished_cb(self, info: ProcedureWrapper): + if info.proc_type == TcProcedureType.TREE_COMMANDING: + def_proc = info.to_tree_commanding_procedure() + _LOGGER.info(f"Queue handling finished for command {def_proc.cmd_path}") + + def feed_cb(self, info: ProcedureWrapper, wrapper: FeedWrapper): + q = self.queue_helper + q.queue_wrapper = wrapper.queue_wrapper + if info.proc_type == TcProcedureType.TREE_COMMANDING: + def_proc = info.to_tree_commanding_procedure() + assert def_proc.cmd_path is not None + pack_pus_telecommands(q, def_proc.cmd_path) def create_cmd_definition_tree() -> CmdTreeNode: @@ -112,32 +148,4 @@ def pack_pus_telecommands(q: DefaultPusQueueHelper, cmd_path: str): if cmd_path_list[0] == "acs": assert len(cmd_path_list) >= 2 if cmd_path_list[1] == "mgms": - assert len(cmd_path_list) >= 3 - if cmd_path_list[2] == "hk": - if cmd_path_list[3] == "one_shot_hk": - q.add_log_cmd("Sending HK one shot request") - # TODO: Fix - # q.add_pus_tc( - # create_request_one_hk_command( - # make_addressable_id(Apid.ACS, AcsId.MGM_SET) - # ) - # ) - if cmd_path_list[2] == "mode": - if cmd_path_list[3] == "set_mode": - handle_set_mode_cmd( - q, "MGM 0", cmd_path_list[4], Apid.ACS, AcsId.MGM_0 - ) - - -def handle_set_mode_cmd( - q: DefaultPusQueueHelper, target_str: str, mode_str: str, apid: int, unique_id: int -): - if mode_str == "off": - q.add_log_cmd(f"Sending Mode OFF to {target_str}") - q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.OFF, 0)) - elif mode_str == "on": - q.add_log_cmd(f"Sending Mode ON to {target_str}") - q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.ON, 0)) - elif mode_str == "normal": - q.add_log_cmd(f"Sending Mode NORMAL to {target_str}") - q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.NORMAL, 0)) + create_mgm_cmds(q, cmd_path_list) diff --git a/satrs-example/pytmtc/pytmtc/pus_tm.py b/satrs-example/pytmtc/pytmtc/pus_tm.py new file mode 100644 index 0000000..ed55212 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/pus_tm.py @@ -0,0 +1,93 @@ +import logging +from typing import Any +from spacepackets.ccsds.time import CdsShortTimestamp +from spacepackets.ecss import PusTm +from spacepackets.ecss.pus_17_test import Service17Tm +from spacepackets.ecss.pus_1_verification import Service1Tm, UnpackParams +from tmtccmd.logging.pus import RawTmtcTimedLogWrapper +from tmtccmd.pus import VerificationWrapper +from tmtccmd.tmtc import GenericApidHandlerBase + +from pytmtc.common import Apid, EventU32 +from pytmtc.hk import handle_hk_packet + + +_LOGGER = logging.getLogger(__name__) + + +class PusHandler(GenericApidHandlerBase): + def __init__( + self, + file_logger: logging.Logger, + verif_wrapper: VerificationWrapper, + raw_logger: RawTmtcTimedLogWrapper, + ): + super().__init__(None) + self.file_logger = file_logger + self.raw_logger = raw_logger + self.verif_wrapper = verif_wrapper + + def handle_tm(self, apid: int, packet: bytes, _user_args: Any): + try: + pus_tm = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + except ValueError as e: + _LOGGER.warning("Could not generate PUS TM object from raw data") + _LOGGER.warning(f"Raw Packet: [{packet.hex(sep=',')}], REPR: {packet!r}") + raise e + service = pus_tm.service + if service == 1: + tm_packet = Service1Tm.unpack( + data=packet, params=UnpackParams(CdsShortTimestamp.TIMESTAMP_SIZE, 1, 2) + ) + res = self.verif_wrapper.add_tm(tm_packet) + if res is None: + _LOGGER.info( + f"Received Verification TM[{tm_packet.service}, {tm_packet.subservice}] " + f"with Request ID {tm_packet.tc_req_id.as_u32():#08x}" + ) + _LOGGER.warning( + f"No matching telecommand found for {tm_packet.tc_req_id}" + ) + else: + self.verif_wrapper.log_to_console(tm_packet, res) + self.verif_wrapper.log_to_file(tm_packet, res) + elif service == 3: + pus_tm = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + handle_hk_packet(pus_tm) + elif service == 5: + tm_packet = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + src_data = tm_packet.source_data + event_u32 = EventU32.unpack(src_data) + _LOGGER.info( + f"Received event packet. Source APID: {Apid(tm_packet.apid)!r}, Event: {event_u32}" + ) + if event_u32.group_id == 0 and event_u32.unique_id == 0: + _LOGGER.info("Received test event") + elif service == 17: + tm_packet = Service17Tm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + if tm_packet.subservice == 2: + self.file_logger.info("Received Ping Reply TM[17,2]") + _LOGGER.info("Received Ping Reply TM[17,2]") + else: + self.file_logger.info( + f"Received Test Packet with unknown subservice {tm_packet.subservice}" + ) + _LOGGER.info( + f"Received Test Packet with unknown subservice {tm_packet.subservice}" + ) + else: + _LOGGER.info( + f"The service {service} is not implemented in Telemetry Factory" + ) + tm_packet = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + self.raw_logger.log_tm(pus_tm) diff --git a/satrs-example/pytmtc/requirements.txt b/satrs-example/pytmtc/requirements.txt index 325615c..9c558e3 100644 --- a/satrs-example/pytmtc/requirements.txt +++ b/satrs-example/pytmtc/requirements.txt @@ -1,2 +1 @@ -tmtccmd == 8.0.0rc2 -# -e git+https://github.com/robamu-org/tmtccmd@97e5e51101a08b21472b3ddecc2063359f7e307a#egg=tmtccmd +. diff --git a/satrs-example/pytmtc/tc_definitions.py b/satrs-example/pytmtc/tc_definitions.py deleted file mode 100644 index 74fbff8..0000000 --- a/satrs-example/pytmtc/tc_definitions.py +++ /dev/null @@ -1,38 +0,0 @@ -from tmtccmd.config import OpCodeEntry, TmtcDefinitionWrapper, CoreServiceList -from tmtccmd.config.globals import get_default_tmtc_defs - -from common import HkOpCodes - - -def tc_definitions() -> TmtcDefinitionWrapper: - defs = get_default_tmtc_defs() - srv_5 = OpCodeEntry() - srv_5.add("0", "Event Test") - defs.add_service( - name=CoreServiceList.SERVICE_5.value, - info="PUS Service 5 Event", - op_code_entry=srv_5, - ) - srv_17 = OpCodeEntry() - srv_17.add("ping", "Ping Test") - srv_17.add("trigger_event", "Trigger Event") - defs.add_service( - name=CoreServiceList.SERVICE_17_ALT, - info="PUS Service 17 Test", - op_code_entry=srv_17, - ) - srv_3 = OpCodeEntry() - srv_3.add(HkOpCodes.GENERATE_ONE_SHOT, "Generate AOCS one shot HK") - defs.add_service( - name=CoreServiceList.SERVICE_3, - info="PUS Service 3 Housekeeping", - op_code_entry=srv_3, - ) - srv_11 = OpCodeEntry() - srv_11.add("0", "Scheduled TC Test") - defs.add_service( - name=CoreServiceList.SERVICE_11, - info="PUS Service 11 TC Scheduling", - op_code_entry=srv_11, - ) - return defs diff --git a/satrs-example/pytmtc/tests/__init__.py b/satrs-example/pytmtc/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/satrs-example/pytmtc/tests/test_tc_mods.py b/satrs-example/pytmtc/tests/test_tc_mods.py new file mode 100644 index 0000000..0b56bde --- /dev/null +++ b/satrs-example/pytmtc/tests/test_tc_mods.py @@ -0,0 +1,48 @@ +from unittest import TestCase + +from spacepackets.ccsds import CdsShortTimestamp +from tmtccmd.tmtc import DefaultPusQueueHelper, QueueEntryHelper +from tmtccmd.tmtc.queue import QueueWrapper + +from pytmtc.config import SatrsConfigHook +from pytmtc.pus_tc import pack_pus_telecommands + + +class TestTcModules(TestCase): + def setUp(self): + self.hook = SatrsConfigHook(json_cfg_path="tmtc_conf.json") + self.queue_helper = DefaultPusQueueHelper( + queue_wrapper=QueueWrapper.empty(), + tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE, + seq_cnt_provider=None, + pus_verificator=None, + default_pus_apid=None, + ) + + def test_cmd_tree_creation_works_without_errors(self): + cmd_defs = self.hook.get_command_definitions() + self.assertIsNotNone(cmd_defs) + + def test_ping_cmd_generation(self): + pack_pus_telecommands(self.queue_helper, "/test/ping") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper = QueueEntryHelper(queue_entry) + log_queue = entry_helper.to_log_entry() + self.assertEqual(log_queue.log_str, "Sending PUS ping telecommand") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper.entry = queue_entry + pus_tc_entry = entry_helper.to_pus_tc_entry() + self.assertEqual(pus_tc_entry.pus_tc.service, 17) + self.assertEqual(pus_tc_entry.pus_tc.subservice, 1) + + def test_event_trigger_generation(self): + pack_pus_telecommands(self.queue_helper, "/test/trigger_event") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper = QueueEntryHelper(queue_entry) + log_queue = entry_helper.to_log_entry() + self.assertEqual(log_queue.log_str, "Triggering test event") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper.entry = queue_entry + pus_tc_entry = entry_helper.to_pus_tc_entry() + self.assertEqual(pus_tc_entry.pus_tc.service, 17) + self.assertEqual(pus_tc_entry.pus_tc.subservice, 128) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index d50bc6d..b75e126 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -1,52 +1,126 @@ use derive_new::new; use satrs::hk::{HkRequest, HkRequestVariant}; +use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender}; use satrs::queue::{GenericSendError, GenericTargetedMessagingError}; -use satrs::spacepackets::ecss::hk; -use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; -use satrs::spacepackets::SpHeader; -use satrs_example::{DeviceMode, TimeStampHelper}; +use satrs_example::{DeviceMode, TimestampHelper}; +use satrs_minisim::acs::lis3mdl::{ + MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, +}; +use satrs_minisim::acs::MgmRequestLis3Mdl; +use satrs_minisim::eps::PcduSwitch; +use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; +use std::fmt::Debug; use std::sync::mpsc::{self}; use std::sync::{Arc, Mutex}; +use std::time::Duration; use satrs::mode::{ ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler, }; use satrs::pus::{EcssTmSender, PusTmVariant}; use satrs::request::{GenericMessage, MessageMetadata, UniqueApidTargetId}; -use satrs_example::config::components::PUS_MODE_SERVICE; +use satrs_example::config::components::{NO_SENDER, PUS_MODE_SERVICE}; +use crate::hk::PusHkHelper; use crate::pus::hk::{HkReply, HkReplyVariant}; use crate::requests::CompositeRequest; use serde::{Deserialize, Serialize}; -const GAUSS_TO_MICROTESLA_FACTOR: f32 = 100.0; -// This is the selected resoltion for the STM LIS3MDL device for the 4 Gauss sensitivity setting. -const FIELD_LSB_PER_GAUSS_4_SENS: f32 = 1.0 / 6842.0; +pub const NR_OF_DATA_AND_CFG_REGISTERS: usize = 14; + +// Register adresses to access various bytes from the raw reply. +pub const X_LOWBYTE_IDX: usize = 9; +pub const Y_LOWBYTE_IDX: usize = 11; +pub const Z_LOWBYTE_IDX: usize = 13; + +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +#[repr(u32)] +pub enum SetId { + SensorData = 0, +} + +#[derive(Default, Debug, PartialEq, Eq)] +pub enum TransitionState { + #[default] + Idle, + PowerSwitching, + Done, +} pub trait SpiInterface { - type Error; + type Error: Debug; fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>; } #[derive(Default)] pub struct SpiDummyInterface { - pub dummy_val_0: i16, - pub dummy_val_1: i16, - pub dummy_val_2: i16, + pub dummy_values: MgmLis3RawValues, } impl SpiInterface for SpiDummyInterface { type Error = (); fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { - rx[0..2].copy_from_slice(&self.dummy_val_0.to_be_bytes()); - rx[2..4].copy_from_slice(&self.dummy_val_1.to_be_bytes()); - rx[4..6].copy_from_slice(&self.dummy_val_2.to_be_bytes()); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.y.to_be_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.z.to_be_bytes()); Ok(()) } } +pub struct SpiSimInterface { + pub sim_request_tx: mpsc::Sender, + pub sim_reply_rx: mpsc::Receiver, +} + +impl SpiInterface for SpiSimInterface { + type Error = (); + + // Right now, we only support requesting sensor data and not configuration of the sensor. + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; + if let Err(e) = self + .sim_request_tx + .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) + { + log::error!("failed to send MGM LIS3 request: {}", e); + } + match self.sim_reply_rx.recv_timeout(Duration::from_millis(50)) { + Ok(sim_reply) => { + let sim_reply_lis3 = MgmLis3MdlReply::from_sim_message(&sim_reply) + .expect("failed to parse LIS3 reply"); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes()); + } + Err(e) => { + log::warn!("MGM LIS3 SIM reply timeout: {}", e); + } + } + Ok(()) + } +} + +pub enum SpiSimInterfaceWrapper { + Dummy(SpiDummyInterface), + Sim(SpiSimInterface), +} + +impl SpiInterface for SpiSimInterfaceWrapper { + type Error = (); + + fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + match self { + SpiSimInterfaceWrapper::Dummy(dummy) => dummy.transfer(tx, rx), + SpiSimInterfaceWrapper::Sim(sim_if) => sim_if.transfer(tx, rx), + } + } +} + #[derive(Default, Debug, Copy, Clone, Serialize, Deserialize)] pub struct MgmData { pub valid: bool, @@ -57,61 +131,85 @@ pub struct MgmData { pub struct MpscModeLeafInterface { pub request_rx: mpsc::Receiver>, - pub reply_tx_to_pus: mpsc::Sender>, - pub reply_tx_to_parent: mpsc::Sender>, + pub reply_to_pus_tx: mpsc::Sender>, + pub reply_to_parent_tx: mpsc::SyncSender>, +} + +#[derive(Default)] +pub struct BufWrapper { + tx_buf: [u8; 32], + rx_buf: [u8; 32], + tm_buf: [u8; 32], +} + +pub struct ModeHelpers { + current: ModeAndSubmode, + target: Option, + requestor_info: Option, + transition_state: TransitionState, +} + +impl Default for ModeHelpers { + fn default() -> Self { + Self { + current: ModeAndSubmode::new(DeviceMode::Off as u32, 0), + target: Default::default(), + requestor_info: Default::default(), + transition_state: Default::default(), + } + } } /// Example MGM device handler strongly based on the LIS3MDL MEMS device. #[derive(new)] #[allow(clippy::too_many_arguments)] -pub struct MgmHandlerLis3Mdl { +pub struct MgmHandlerLis3Mdl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, +> { id: UniqueApidTargetId, dev_str: &'static str, mode_interface: MpscModeLeafInterface, - composite_request_receiver: mpsc::Receiver>, - hk_reply_sender: mpsc::Sender>, + composite_request_rx: mpsc::Receiver>, + hk_reply_tx: mpsc::Sender>, + switch_helper: SwitchHelper, tm_sender: TmSender, - com_interface: ComInterface, + pub com_interface: ComInterface, shared_mgm_set: Arc>, - #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] - mode_and_submode: ModeAndSubmode, + #[new(value = "PusHkHelper::new(id)")] + hk_helper: PusHkHelper, #[new(default)] - tx_buf: [u8; 12], + mode_helpers: ModeHelpers, #[new(default)] - rx_buf: [u8; 12], + bufs: BufWrapper, #[new(default)] - tm_buf: [u8; 16], - #[new(default)] - stamp_helper: TimeStampHelper, + stamp_helper: TimestampHelper, } -impl MgmHandlerLis3Mdl { +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > MgmHandlerLis3Mdl +{ pub fn periodic_operation(&mut self) { self.stamp_helper.update_from_now(); // Handle requests. self.handle_composite_requests(); self.handle_mode_requests(); + if let Some(target_mode_submode) = self.mode_helpers.target { + self.handle_mode_transition(target_mode_submode); + } if self.mode() == DeviceMode::Normal as u32 { log::trace!("polling LIS3MDL sensor {}", self.dev_str); - // Communicate with the device. - let result = self.com_interface.transfer(&self.tx_buf, &mut self.rx_buf); - assert!(result.is_ok()); - // Actual data begins on the second byte, similarly to how a lot of SPI devices behave. - let x_raw = i16::from_be_bytes(self.rx_buf[1..3].try_into().unwrap()); - let y_raw = i16::from_be_bytes(self.rx_buf[3..5].try_into().unwrap()); - let z_raw = i16::from_be_bytes(self.rx_buf[5..7].try_into().unwrap()); - // Simple scaling to retrieve the float value, assuming a sensor resolution of - let mut mgm_guard = self.shared_mgm_set.lock().unwrap(); - mgm_guard.x = x_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR * FIELD_LSB_PER_GAUSS_4_SENS; - mgm_guard.y = y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR * FIELD_LSB_PER_GAUSS_4_SENS; - mgm_guard.z = z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR * FIELD_LSB_PER_GAUSS_4_SENS; - drop(mgm_guard); + self.poll_sensor(); } } pub fn handle_composite_requests(&mut self) { loop { - match self.composite_request_receiver.try_recv() { + match self.composite_request_rx.try_recv() { Ok(ref msg) => match &msg.message { CompositeRequest::Hk(hk_request) => { self.handle_hk_request(&msg.requestor_info, hk_request) @@ -139,34 +237,33 @@ impl MgmHandlerLis3Mdl { - self.hk_reply_sender - .send(GenericMessage::new( - *requestor_info, - HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), - )) - .expect("failed to send HK reply"); - let sec_header = PusTmSecondaryHeader::new( - 3, - hk::Subservice::TmHkPacket as u8, - 0, - 0, - self.stamp_helper.stamp(), - ); let mgm_snapshot = *self.shared_mgm_set.lock().unwrap(); - // Use binary serialization here. We want the data to be tightly packed. - self.tm_buf[0] = mgm_snapshot.valid as u8; - self.tm_buf[1..5].copy_from_slice(&mgm_snapshot.x.to_be_bytes()); - self.tm_buf[5..9].copy_from_slice(&mgm_snapshot.y.to_be_bytes()); - self.tm_buf[9..13].copy_from_slice(&mgm_snapshot.z.to_be_bytes()); - let hk_tm = PusTmCreator::new( - SpHeader::new_from_apid(self.id.apid), - sec_header, - &self.tm_buf[0..12], - true, - ); - self.tm_sender - .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) - .expect("failed to send HK TM"); + if let Ok(hk_tm) = self.hk_helper.generate_hk_report_packet( + self.stamp_helper.stamp(), + SetId::SensorData as u32, + &mut |hk_buf| { + hk_buf[0] = mgm_snapshot.valid as u8; + hk_buf[1..5].copy_from_slice(&mgm_snapshot.x.to_be_bytes()); + hk_buf[5..9].copy_from_slice(&mgm_snapshot.y.to_be_bytes()); + hk_buf[9..13].copy_from_slice(&mgm_snapshot.z.to_be_bytes()); + Ok(13) + }, + &mut self.bufs.tm_buf, + ) { + // TODO: If sending the TM fails, we should also send a failure reply. + self.tm_sender + .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) + .expect("failed to send HK TM"); + self.hk_reply_tx + .send(GenericMessage::new( + *requestor_info, + HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), + )) + .expect("failed to send HK reply"); + } else { + // TODO: Send back failure reply. Need result code for this. + log::error!("TM buffer too small to generate HK data"); + } } HkRequestVariant::EnablePeriodic => todo!(), HkRequestVariant::DisablePeriodic => todo!(), @@ -199,20 +296,91 @@ impl MgmHandlerLis3Mdl ModeProvider - for MgmHandlerLis3Mdl -{ - fn mode_and_submode(&self) -> ModeAndSubmode { - self.mode_and_submode + pub fn poll_sensor(&mut self) { + // Communicate with the device. This is actually how to read the data from the LIS3 device + // SPI interface. + self.com_interface + .transfer( + &self.bufs.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1], + &mut self.bufs.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1], + ) + .expect("failed to transfer data"); + let x_raw = i16::from_le_bytes( + self.bufs.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] + .try_into() + .unwrap(), + ); + let y_raw = i16::from_le_bytes( + self.bufs.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2] + .try_into() + .unwrap(), + ); + let z_raw = i16::from_le_bytes( + self.bufs.rx_buf[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2] + .try_into() + .unwrap(), + ); + // Simple scaling to retrieve the float value, assuming the best sensor resolution. + let mut mgm_guard = self.shared_mgm_set.lock().unwrap(); + mgm_guard.x = x_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + mgm_guard.y = y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + mgm_guard.z = z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + mgm_guard.valid = true; + drop(mgm_guard); + } + + pub fn handle_mode_transition(&mut self, target_mode_submode: ModeAndSubmode) { + if target_mode_submode.mode() == DeviceMode::On as u32 + || target_mode_submode.mode() == DeviceMode::Normal as u32 + { + if self.mode_helpers.transition_state == TransitionState::Idle { + let result = self + .switch_helper + .send_switch_on_cmd(MessageMetadata::new(0, self.id.id()), PcduSwitch::Mgm); + if result.is_err() { + // Could not send switch command.. still continue with transition. + log::error!("failed to send switch on command"); + } + self.mode_helpers.transition_state = TransitionState::PowerSwitching; + } + if self.mode_helpers.transition_state == TransitionState::PowerSwitching + && self + .switch_helper + .is_switch_on(PcduSwitch::Mgm) + .expect("switch info error") + { + self.mode_helpers.transition_state = TransitionState::Done; + } + if self.mode_helpers.transition_state == TransitionState::Done { + self.mode_helpers.current = self.mode_helpers.target.unwrap(); + self.handle_mode_reached(self.mode_helpers.requestor_info) + .expect("failed to handle mode reached"); + self.mode_helpers.transition_state = TransitionState::Idle; + } + } } } -impl ModeRequestHandler - for MgmHandlerLis3Mdl +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > ModeProvider for MgmHandlerLis3Mdl +{ + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_helpers.current + } +} + +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > ModeRequestHandler for MgmHandlerLis3Mdl { type Error = ModeError; + fn start_transition( &mut self, requestor: MessageMetadata, @@ -223,8 +391,18 @@ impl ModeRequestHandler self.dev_str, mode_and_submode ); - self.mode_and_submode = mode_and_submode; - self.handle_mode_reached(Some(requestor))?; + self.mode_helpers.current = mode_and_submode; + if mode_and_submode.mode() == DeviceMode::Off as u32 { + self.shared_mgm_set.lock().unwrap().valid = false; + self.handle_mode_reached(Some(requestor))?; + } else if mode_and_submode.mode() == DeviceMode::Normal as u32 + || mode_and_submode.mode() == DeviceMode::On as u32 + { + // TODO: Write helper method for the struct? Might help for other handlers as well.. + self.mode_helpers.transition_state = TransitionState::Idle; + self.mode_helpers.requestor_info = Some(requestor); + self.mode_helpers.target = Some(mode_and_submode); + } Ok(()) } @@ -232,7 +410,7 @@ impl ModeRequestHandler log::info!( "{} announcing mode: {:?}", self.dev_str, - self.mode_and_submode + self.mode_and_submode() ); } @@ -240,11 +418,15 @@ impl ModeRequestHandler &mut self, requestor: Option, ) -> Result<(), Self::Error> { + self.mode_helpers.target = None; self.announce_mode(requestor, false); if let Some(requestor) = requestor { + if requestor.sender_id() == NO_SENDER { + return Ok(()); + } if requestor.sender_id() != PUS_MODE_SERVICE.id() { log::warn!( - "can not send back mode reply to sender {}", + "can not send back mode reply to sender {:x}", requestor.sender_id() ); } else { @@ -266,7 +448,7 @@ impl ModeRequestHandler ); } self.mode_interface - .reply_tx_to_pus + .reply_to_pus_tx .send(GenericMessage::new(requestor, reply)) .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; Ok(()) @@ -280,3 +462,193 @@ impl ModeRequestHandler Ok(()) } } + +#[cfg(test)] +mod tests { + use std::sync::{mpsc, Arc}; + + use satrs::{ + mode::{ModeReply, ModeRequest}, + power::SwitchStateBinary, + request::{GenericMessage, UniqueApidTargetId}, + tmtc::PacketAsVec, + }; + use satrs_example::config::components::Apid; + use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; + + use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest}; + + use super::*; + + #[derive(Default)] + pub struct TestSpiInterface { + pub call_count: u32, + pub next_mgm_data: MgmLis3RawValues, + } + + impl SpiInterface for TestSpiInterface { + type Error = (); + + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] + .copy_from_slice(&self.next_mgm_data.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2] + .copy_from_slice(&self.next_mgm_data.y.to_le_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2] + .copy_from_slice(&self.next_mgm_data.z.to_le_bytes()); + self.call_count += 1; + Ok(()) + } + } + + pub struct MgmTestbench { + pub mode_request_tx: mpsc::Sender>, + pub mode_reply_rx_to_pus: mpsc::Receiver>, + pub mode_reply_rx_to_parent: mpsc::Receiver>, + pub composite_request_tx: mpsc::Sender>, + pub hk_reply_rx: mpsc::Receiver>, + pub tm_rx: mpsc::Receiver, + pub handler: + MgmHandlerLis3Mdl, TestSwitchHelper>, + } + + impl MgmTestbench { + pub fn new() -> Self { + let (request_tx, request_rx) = mpsc::channel(); + let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel(); + let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5); + let mode_interface = MpscModeLeafInterface { + request_rx, + reply_to_pus_tx: reply_tx_to_pus, + reply_to_parent_tx: reply_tx_to_parent, + }; + let (composite_request_tx, composite_request_rx) = mpsc::channel(); + let (hk_reply_tx, hk_reply_rx) = mpsc::channel(); + let (tm_tx, tm_rx) = mpsc::channel::(); + let shared_mgm_set = Arc::default(); + Self { + mode_request_tx: request_tx, + mode_reply_rx_to_pus: reply_rx_to_pus, + mode_reply_rx_to_parent: reply_rx_to_parent, + composite_request_tx, + tm_rx, + hk_reply_rx, + handler: MgmHandlerLis3Mdl::new( + UniqueApidTargetId::new(Apid::Acs as u16, 1), + "TEST_MGM", + mode_interface, + composite_request_rx, + hk_reply_tx, + TestSwitchHelper::default(), + tm_tx, + TestSpiInterface::default(), + shared_mgm_set, + ), + } + } + } + + #[test] + fn test_basic_handler() { + let mut testbench = MgmTestbench::new(); + assert_eq!(testbench.handler.com_interface.call_count, 0); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Off as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); + testbench.handler.periodic_operation(); + // Handler is OFF, no changes expected. + assert_eq!(testbench.handler.com_interface.call_count, 0); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Off as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); + } + + #[test] + fn test_normal_handler() { + let mut testbench = MgmTestbench::new(); + testbench + .mode_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, PUS_MODE_SERVICE.id()), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + )) + .expect("failed to send mode request"); + testbench.handler.periodic_operation(); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Normal as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0); + + // Verify power switch handling. + let mut switch_requests = testbench.handler.switch_helper.switch_requests.borrow_mut(); + assert_eq!(switch_requests.len(), 1); + let switch_req = switch_requests.pop_front().expect("no switch request"); + assert_eq!(switch_req.target_state, SwitchStateBinary::On); + assert_eq!(switch_req.switch_id, PcduSwitch::Mgm); + let mut switch_info_requests = testbench + .handler + .switch_helper + .switch_info_requests + .borrow_mut(); + assert_eq!(switch_info_requests.len(), 1); + let switch_info_req = switch_info_requests.pop_front().expect("no switch request"); + assert_eq!(switch_info_req, PcduSwitch::Mgm); + + let mode_reply = testbench + .mode_reply_rx_to_pus + .try_recv() + .expect("no mode reply generated"); + match mode_reply.message { + ModeReply::ModeReply(mode) => { + assert_eq!(mode.mode(), DeviceMode::Normal as u32); + assert_eq!(mode.submode(), 0); + } + _ => panic!("unexpected mode reply"), + } + // The device should have been polled once. + assert_eq!(testbench.handler.com_interface.call_count, 1); + let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap(); + assert!(mgm_set.x < 0.001); + assert!(mgm_set.y < 0.001); + assert!(mgm_set.z < 0.001); + assert!(mgm_set.valid); + } + + #[test] + fn test_normal_handler_mgm_set_conversion() { + let mut testbench = MgmTestbench::new(); + let raw_values = MgmLis3RawValues { + x: 1000, + y: -1000, + z: 1000, + }; + testbench.handler.com_interface.next_mgm_data = raw_values; + testbench + .mode_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, PUS_MODE_SERVICE.id()), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + )) + .expect("failed to send mode request"); + testbench.handler.periodic_operation(); + let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap(); + let expected_x = + raw_values.x as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + let expected_y = + raw_values.y as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + let expected_z = + raw_values.z as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + let x_diff = (mgm_set.x - expected_x).abs(); + let y_diff = (mgm_set.y - expected_y).abs(); + let z_diff = (mgm_set.z - expected_z).abs(); + assert!(x_diff < 0.001, "x diff too large: {}", x_diff); + assert!(y_diff < 0.001, "y diff too large: {}", y_diff); + assert!(z_diff < 0.001, "z diff too large: {}", z_diff); + assert!(mgm_set.valid); + } +} diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs index d84ad2e..5608bf3 100644 --- a/satrs-example/src/config.rs +++ b/satrs-example/src/config.rs @@ -122,7 +122,7 @@ pub mod mode_err { } pub mod components { - use satrs::request::UniqueApidTargetId; + use satrs::{request::UniqueApidTargetId, ComponentId}; use strum::EnumIter; #[derive(Copy, Clone, PartialEq, Eq, EnumIter)] @@ -132,6 +132,7 @@ pub mod components { Acs = 3, Cfdp = 4, Tmtc = 5, + Eps = 6, } // Component IDs for components with the PUS APID. @@ -150,6 +151,11 @@ pub mod components { Mgm0 = 0, } + #[derive(Copy, Clone, PartialEq, Eq)] + pub enum EpsId { + Pcdu = 0, + } + #[derive(Copy, Clone, PartialEq, Eq)] pub enum TmtcId { UdpServer = 0, @@ -172,10 +178,13 @@ pub mod components { UniqueApidTargetId::new(Apid::Sched as u16, 0); pub const MGM_HANDLER_0: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Mgm0 as u32); + pub const PCDU_HANDLER: UniqueApidTargetId = + UniqueApidTargetId::new(Apid::Eps as u16, EpsId::Pcdu as u32); pub const UDP_SERVER: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Tmtc as u16, TmtcId::UdpServer as u32); pub const TCP_SERVER: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Tmtc as u16, TmtcId::TcpServer as u32); + pub const NO_SENDER: ComponentId = ComponentId::MAX; } pub mod pool { @@ -224,7 +233,7 @@ pub mod pool { pub mod tasks { pub const FREQ_MS_UDP_TMTC: u64 = 200; - pub const FREQ_MS_EVENT_HANDLING: u64 = 400; pub const FREQ_MS_AOCS: u64 = 500; pub const FREQ_MS_PUS_STACK: u64 = 200; + pub const SIM_CLIENT_IDLE_DELAY_MS: u64 = 5; } diff --git a/satrs-example/src/eps/mod.rs b/satrs-example/src/eps/mod.rs new file mode 100644 index 0000000..351cf76 --- /dev/null +++ b/satrs-example/src/eps/mod.rs @@ -0,0 +1,195 @@ +use derive_new::new; +use std::{cell::RefCell, collections::VecDeque, sync::mpsc, time::Duration}; + +use satrs::{ + power::{ + PowerSwitchInfo, PowerSwitcherCommandSender, SwitchRequest, SwitchState, SwitchStateBinary, + }, + queue::GenericSendError, + request::{GenericMessage, MessageMetadata}, +}; +use satrs_minisim::eps::{PcduSwitch, SwitchMapWrapper}; +use thiserror::Error; + +use self::pcdu::SharedSwitchSet; + +pub mod pcdu; + +#[derive(new, Clone)] +pub struct PowerSwitchHelper { + switcher_tx: mpsc::SyncSender>, + shared_switch_set: SharedSwitchSet, +} + +#[derive(Debug, Error, Copy, Clone, PartialEq, Eq)] +pub enum SwitchCommandingError { + #[error("send error: {0}")] + Send(#[from] GenericSendError), +} + +#[derive(Debug, Error, Copy, Clone, PartialEq, Eq)] +pub enum SwitchInfoError { + /// This is a configuration error which should not occur. + #[error("switch ID not in map")] + SwitchIdNotInMap(PcduSwitch), + #[error("switch set invalid")] + SwitchSetInvalid, +} + +impl PowerSwitchInfo for PowerSwitchHelper { + type Error = SwitchInfoError; + + fn switch_state( + &self, + switch_id: PcduSwitch, + ) -> Result { + let switch_set = self + .shared_switch_set + .lock() + .expect("failed to lock switch set"); + if !switch_set.valid { + return Err(SwitchInfoError::SwitchSetInvalid); + } + + if let Some(state) = switch_set.switch_map.get(&switch_id) { + return Ok(*state); + } + Err(SwitchInfoError::SwitchIdNotInMap(switch_id)) + } + + fn switch_delay_ms(&self) -> Duration { + // Here, we could set device specific switch delays theoretically. Set it to this value + // for now. + Duration::from_millis(1000) + } +} + +impl PowerSwitcherCommandSender for PowerSwitchHelper { + type Error = SwitchCommandingError; + + fn send_switch_on_cmd( + &self, + requestor_info: satrs::request::MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + self.switcher_tx + .send_switch_on_cmd(requestor_info, switch_id)?; + Ok(()) + } + + fn send_switch_off_cmd( + &self, + requestor_info: satrs::request::MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + self.switcher_tx + .send_switch_off_cmd(requestor_info, switch_id)?; + Ok(()) + } +} + +#[derive(new)] +pub struct SwitchRequestInfo { + pub requestor_info: MessageMetadata, + pub switch_id: PcduSwitch, + pub target_state: satrs::power::SwitchStateBinary, +} + +// Test switch helper which can be used for unittests. +pub struct TestSwitchHelper { + pub switch_requests: RefCell>, + pub switch_info_requests: RefCell>, + pub switch_delay_request_count: u32, + pub next_switch_delay: Duration, + pub switch_map: RefCell, + pub switch_map_valid: bool, +} + +impl Default for TestSwitchHelper { + fn default() -> Self { + Self { + switch_requests: Default::default(), + switch_info_requests: Default::default(), + switch_delay_request_count: Default::default(), + next_switch_delay: Duration::from_millis(1000), + switch_map: Default::default(), + switch_map_valid: true, + } + } +} + +impl PowerSwitchInfo for TestSwitchHelper { + type Error = SwitchInfoError; + + fn switch_state( + &self, + switch_id: PcduSwitch, + ) -> Result { + let mut switch_info_requests_mut = self.switch_info_requests.borrow_mut(); + switch_info_requests_mut.push_back(switch_id); + if !self.switch_map_valid { + return Err(SwitchInfoError::SwitchSetInvalid); + } + let switch_map_mut = self.switch_map.borrow_mut(); + if let Some(state) = switch_map_mut.0.get(&switch_id) { + return Ok(*state); + } + Err(SwitchInfoError::SwitchIdNotInMap(switch_id)) + } + + fn switch_delay_ms(&self) -> Duration { + self.next_switch_delay + } +} + +impl PowerSwitcherCommandSender for TestSwitchHelper { + type Error = SwitchCommandingError; + + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + let mut switch_requests_mut = self.switch_requests.borrow_mut(); + switch_requests_mut.push_back(SwitchRequestInfo { + requestor_info, + switch_id, + target_state: SwitchStateBinary::On, + }); + // By default, the test helper immediately acknowledges the switch request by setting + // the appropriate switch state in the internal switch map. + let mut switch_map_mut = self.switch_map.borrow_mut(); + if let Some(switch_state) = switch_map_mut.0.get_mut(&switch_id) { + *switch_state = SwitchState::On; + } + Ok(()) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + let mut switch_requests_mut = self.switch_requests.borrow_mut(); + switch_requests_mut.push_back(SwitchRequestInfo { + requestor_info, + switch_id, + target_state: SwitchStateBinary::Off, + }); + // By default, the test helper immediately acknowledges the switch request by setting + // the appropriate switch state in the internal switch map. + let mut switch_map_mut = self.switch_map.borrow_mut(); + if let Some(switch_state) = switch_map_mut.0.get_mut(&switch_id) { + *switch_state = SwitchState::Off; + } + Ok(()) + } +} + +#[allow(dead_code)] +impl TestSwitchHelper { + // Helper function which can be used to force a switch to another state for test purposes. + pub fn set_switch_state(&mut self, switch: PcduSwitch, state: SwitchState) { + self.switch_map.get_mut().0.insert(switch, state); + } +} diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs new file mode 100644 index 0000000..908bfb2 --- /dev/null +++ b/satrs-example/src/eps/pcdu.rs @@ -0,0 +1,722 @@ +use std::{ + cell::RefCell, + collections::VecDeque, + sync::{mpsc, Arc, Mutex}, +}; + +use derive_new::new; +use num_enum::{IntoPrimitive, TryFromPrimitive}; +use satrs::{ + hk::{HkRequest, HkRequestVariant}, + mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, + power::SwitchRequest, + pus::{EcssTmSender, PusTmVariant}, + queue::{GenericSendError, GenericTargetedMessagingError}, + request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, + spacepackets::ByteConversionError, +}; +use satrs_example::{ + config::components::{NO_SENDER, PUS_MODE_SERVICE}, + DeviceMode, TimestampHelper, +}; +use satrs_minisim::{ + eps::{ + PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinaryWrapper, SwitchMapWrapper, + }, + SerializableSimMsgPayload, SimReply, SimRequest, +}; +use serde::{Deserialize, Serialize}; + +use crate::{ + acs::mgm::MpscModeLeafInterface, + hk::PusHkHelper, + pus::hk::{HkReply, HkReplyVariant}, + requests::CompositeRequest, +}; + +pub trait SerialInterface { + type Error: core::fmt::Debug; + + /// Send some data via the serial interface. + fn send(&self, data: &[u8]) -> Result<(), Self::Error>; + /// Receive all replies received on the serial interface so far. This function takes a closure + /// and call its for each received packet, passing the received packet into it. + fn try_recv_replies( + &self, + f: ReplyHandler, + ) -> Result<(), Self::Error>; +} + +#[derive(new)] +pub struct SerialInterfaceToSim { + pub sim_request_tx: mpsc::Sender, + pub sim_reply_rx: mpsc::Receiver, +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq, TryFromPrimitive, IntoPrimitive)] +#[repr(u32)] +pub enum SetId { + SwitcherSet = 0, +} + +impl SerialInterface for SerialInterfaceToSim { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + let request: PcduRequest = serde_json::from_slice(data).expect("expected a PCDU request"); + self.sim_request_tx + .send(SimRequest::new_with_epoch_time(request)) + .expect("failed to send request to simulation"); + Ok(()) + } + + fn try_recv_replies( + &self, + mut f: ReplyHandler, + ) -> Result<(), Self::Error> { + loop { + match self.sim_reply_rx.try_recv() { + Ok(reply) => { + let reply = serde_json::to_string(&reply).unwrap(); + f(reply.as_bytes()); + } + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + log::warn!("sim reply sender has disconnected"); + break; + } + }, + } + } + Ok(()) + } +} + +#[derive(Default)] +pub struct SerialInterfaceDummy { + // Need interior mutability here for both fields. + pub switch_map: RefCell, + pub reply_deque: RefCell>, +} + +impl SerialInterface for SerialInterfaceDummy { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + let pcdu_req: PcduRequest = serde_json::from_slice(data).unwrap(); + let switch_map_mut = &mut self.switch_map.borrow_mut().0; + match pcdu_req { + PcduRequest::SwitchDevice { switch, state } => { + match switch_map_mut.entry(switch) { + std::collections::hash_map::Entry::Occupied(mut val) => { + *val.get_mut() = state; + } + std::collections::hash_map::Entry::Vacant(vacant) => { + vacant.insert(state); + } + }; + } + PcduRequest::RequestSwitchInfo => { + let mut reply_deque_mut = self.reply_deque.borrow_mut(); + reply_deque_mut.push_back(SimReply::new(&PcduReply::SwitchInfo( + switch_map_mut.clone(), + ))); + } + }; + Ok(()) + } + + fn try_recv_replies( + &self, + mut f: ReplyHandler, + ) -> Result<(), Self::Error> { + if self.reply_queue_empty() { + return Ok(()); + } + loop { + let reply = self.get_next_reply_as_string(); + f(reply.as_bytes()); + if self.reply_queue_empty() { + break; + } + } + Ok(()) + } +} + +impl SerialInterfaceDummy { + fn get_next_reply_as_string(&self) -> String { + let mut reply_deque_mut = self.reply_deque.borrow_mut(); + let next_reply = reply_deque_mut.pop_front().unwrap(); + serde_json::to_string(&next_reply).unwrap() + } + + fn reply_queue_empty(&self) -> bool { + self.reply_deque.borrow().is_empty() + } +} + +pub enum SerialSimInterfaceWrapper { + Dummy(SerialInterfaceDummy), + Sim(SerialInterfaceToSim), +} + +impl SerialInterface for SerialSimInterfaceWrapper { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + match self { + SerialSimInterfaceWrapper::Dummy(dummy) => dummy.send(data), + SerialSimInterfaceWrapper::Sim(sim) => sim.send(data), + } + } + + fn try_recv_replies( + &self, + f: ReplyHandler, + ) -> Result<(), Self::Error> { + match self { + SerialSimInterfaceWrapper::Dummy(dummy) => dummy.try_recv_replies(f), + SerialSimInterfaceWrapper::Sim(sim) => sim.try_recv_replies(f), + } + } +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum OpCode { + RegularOp = 0, + PollAndRecvReplies = 1, +} + +#[derive(Clone, PartialEq, Eq, Default, Serialize, Deserialize)] +pub struct SwitchSet { + pub valid: bool, + pub switch_map: SwitchMap, +} + +pub type SharedSwitchSet = Arc>; + +/// Example PCDU device handler. +#[derive(new)] +#[allow(clippy::too_many_arguments)] +pub struct PcduHandler { + id: UniqueApidTargetId, + dev_str: &'static str, + mode_interface: MpscModeLeafInterface, + composite_request_rx: mpsc::Receiver>, + hk_reply_tx: mpsc::Sender>, + switch_request_rx: mpsc::Receiver>, + tm_sender: TmSender, + pub com_interface: ComInterface, + shared_switch_map: Arc>, + #[new(value = "PusHkHelper::new(id)")] + hk_helper: PusHkHelper, + #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] + mode_and_submode: ModeAndSubmode, + #[new(default)] + stamp_helper: TimestampHelper, + #[new(value = "[0; 256]")] + tm_buf: [u8; 256], +} + +impl PcduHandler { + pub fn periodic_operation(&mut self, op_code: OpCode) { + match op_code { + OpCode::RegularOp => { + self.stamp_helper.update_from_now(); + // Handle requests. + self.handle_composite_requests(); + self.handle_mode_requests(); + self.handle_switch_requests(); + // Poll the switch states and/or telemetry regularly here. + if self.mode() == DeviceMode::Normal as u32 || self.mode() == DeviceMode::On as u32 + { + self.handle_periodic_commands(); + } + } + OpCode::PollAndRecvReplies => { + self.poll_and_handle_replies(); + } + } + } + + pub fn handle_composite_requests(&mut self) { + loop { + match self.composite_request_rx.try_recv() { + Ok(ref msg) => match &msg.message { + CompositeRequest::Hk(hk_request) => { + self.handle_hk_request(&msg.requestor_info, hk_request) + } + // TODO: This object does not have actions (yet).. Still send back completion failure + // reply. + CompositeRequest::Action(_action_req) => {} + }, + + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!( + "{}: failed to receive composite request: {:?}", + self.dev_str, + e + ); + } else { + break; + } + } + } + } + } + + pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) { + match hk_request.variant { + HkRequestVariant::OneShot => { + if hk_request.unique_id == SetId::SwitcherSet as u32 { + if let Ok(hk_tm) = self.hk_helper.generate_hk_report_packet( + self.stamp_helper.stamp(), + SetId::SwitcherSet as u32, + &mut |hk_buf| { + // Send TM down as JSON. + let switch_map_snapshot = self + .shared_switch_map + .lock() + .expect("failed to lock switch map") + .clone(); + let switch_map_json = serde_json::to_string(&switch_map_snapshot) + .expect("failed to serialize switch map"); + if switch_map_json.len() > hk_buf.len() { + log::error!("switch map JSON too large for HK buffer"); + return Err(ByteConversionError::ToSliceTooSmall { + found: hk_buf.len(), + expected: switch_map_json.len(), + }); + } + Ok(switch_map_json.len()) + }, + &mut self.tm_buf, + ) { + self.tm_sender + .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) + .expect("failed to send HK TM"); + self.hk_reply_tx + .send(GenericMessage::new( + *requestor_info, + HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), + )) + .expect("failed to send HK reply"); + } + } + } + HkRequestVariant::EnablePeriodic => todo!(), + HkRequestVariant::DisablePeriodic => todo!(), + HkRequestVariant::ModifyCollectionInterval(_) => todo!(), + } + } + + pub fn handle_periodic_commands(&self) { + let pcdu_req = PcduRequest::RequestSwitchInfo; + let pcdu_req_ser = serde_json::to_string(&pcdu_req).unwrap(); + if let Err(_e) = self.com_interface.send(pcdu_req_ser.as_bytes()) { + log::warn!("polling PCDU switch info failed"); + } + } + + pub fn handle_mode_requests(&mut self) { + loop { + // TODO: Only allow one set mode request per cycle? + match self.mode_interface.request_rx.try_recv() { + Ok(msg) => { + let result = self.handle_mode_request(msg); + // TODO: Trigger event? + if result.is_err() { + log::warn!( + "{}: mode request failed with error {:?}", + self.dev_str, + result.err().unwrap() + ); + } + } + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); + } else { + break; + } + } + } + } + } + + pub fn handle_switch_requests(&mut self) { + loop { + match self.switch_request_rx.try_recv() { + Ok(switch_req) => match PcduSwitch::try_from(switch_req.message.switch_id()) { + Ok(pcdu_switch) => { + let pcdu_req = PcduRequest::SwitchDevice { + switch: pcdu_switch, + state: switch_req.message.target_state(), + }; + let pcdu_req_ser = serde_json::to_string(&pcdu_req).unwrap(); + self.com_interface + .send(pcdu_req_ser.as_bytes()) + .expect("failed to send switch request to PCDU"); + } + Err(e) => todo!("failed to convert switch ID {:?} to typed PCDU switch", e), + }, + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + log::warn!("switch request receiver has disconnected"); + break; + } + }, + }; + } + } + + pub fn poll_and_handle_replies(&mut self) { + if let Err(e) = self.com_interface.try_recv_replies(|reply| { + let sim_reply: SimReply = serde_json::from_slice(reply).expect("invalid reply format"); + let pcdu_reply = PcduReply::from_sim_message(&sim_reply).expect("invalid reply format"); + match pcdu_reply { + PcduReply::SwitchInfo(switch_info) => { + let switch_map_wrapper = + SwitchMapWrapper::from_binary_switch_map_ref(&switch_info); + let mut shared_switch_map = self + .shared_switch_map + .lock() + .expect("failed to lock switch map"); + shared_switch_map.switch_map = switch_map_wrapper.0; + shared_switch_map.valid = true; + } + } + }) { + log::warn!("receiving PCDU replies failed: {:?}", e); + } + } +} + +impl ModeProvider + for PcduHandler +{ + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +impl ModeRequestHandler + for PcduHandler +{ + type Error = ModeError; + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + ) -> Result<(), satrs::mode::ModeError> { + log::info!( + "{}: transitioning to mode {:?}", + self.dev_str, + mode_and_submode + ); + self.mode_and_submode = mode_and_submode; + if mode_and_submode.mode() == DeviceMode::Off as u32 { + self.shared_switch_map.lock().unwrap().valid = false; + } + self.handle_mode_reached(Some(requestor))?; + Ok(()) + } + + fn announce_mode(&self, _requestor_info: Option, _recursive: bool) { + log::info!( + "{} announcing mode: {:?}", + self.dev_str, + self.mode_and_submode + ); + } + + fn handle_mode_reached( + &mut self, + requestor: Option, + ) -> Result<(), Self::Error> { + self.announce_mode(requestor, false); + if let Some(requestor) = requestor { + if requestor.sender_id() == NO_SENDER { + return Ok(()); + } + if requestor.sender_id() != PUS_MODE_SERVICE.id() { + log::warn!( + "can not send back mode reply to sender {}", + requestor.sender_id() + ); + } else { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode()))?; + } + } + Ok(()) + } + + fn send_mode_reply( + &self, + requestor: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + if requestor.sender_id() != PUS_MODE_SERVICE.id() { + log::warn!( + "can not send back mode reply to sender {}", + requestor.sender_id() + ); + } + self.mode_interface + .reply_to_pus_tx + .send(GenericMessage::new(requestor, reply)) + .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; + Ok(()) + } + + fn handle_mode_info( + &mut self, + _requestor_info: MessageMetadata, + _info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use std::sync::mpsc; + + use satrs::{ + mode::ModeRequest, power::SwitchStateBinary, request::GenericMessage, tmtc::PacketAsVec, + }; + use satrs_example::config::components::{Apid, MGM_HANDLER_0}; + use satrs_minisim::eps::SwitchMapBinary; + + use super::*; + + #[derive(Default)] + pub struct SerialInterfaceTest { + pub inner: SerialInterfaceDummy, + pub send_queue: RefCell>>, + pub reply_queue: RefCell>, + } + + impl SerialInterface for SerialInterfaceTest { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + let mut send_queue_mut = self.send_queue.borrow_mut(); + send_queue_mut.push_back(data.to_vec()); + self.inner.send(data) + } + + fn try_recv_replies( + &self, + mut f: ReplyHandler, + ) -> Result<(), Self::Error> { + if self.inner.reply_queue_empty() { + return Ok(()); + } + loop { + let reply = self.inner.get_next_reply_as_string(); + self.reply_queue.borrow_mut().push_back(reply.clone()); + f(reply.as_bytes()); + if self.inner.reply_queue_empty() { + break; + } + } + Ok(()) + } + } + + pub struct PcduTestbench { + pub mode_request_tx: mpsc::Sender>, + pub mode_reply_rx_to_pus: mpsc::Receiver>, + pub mode_reply_rx_to_parent: mpsc::Receiver>, + pub composite_request_tx: mpsc::Sender>, + pub hk_reply_rx: mpsc::Receiver>, + pub tm_rx: mpsc::Receiver, + pub switch_request_tx: mpsc::Sender>, + pub handler: PcduHandler>, + } + + impl PcduTestbench { + pub fn new() -> Self { + let (mode_request_tx, mode_request_rx) = mpsc::channel(); + let (mode_reply_tx_to_pus, mode_reply_rx_to_pus) = mpsc::channel(); + let (mode_reply_tx_to_parent, mode_reply_rx_to_parent) = mpsc::sync_channel(5); + let mode_interface = MpscModeLeafInterface { + request_rx: mode_request_rx, + reply_to_pus_tx: mode_reply_tx_to_pus, + reply_to_parent_tx: mode_reply_tx_to_parent, + }; + let (composite_request_tx, composite_request_rx) = mpsc::channel(); + let (hk_reply_tx, hk_reply_rx) = mpsc::channel(); + let (tm_tx, tm_rx) = mpsc::channel::(); + let (switch_request_tx, switch_reqest_rx) = mpsc::channel(); + let shared_switch_map = Arc::new(Mutex::new(SwitchSet::default())); + Self { + mode_request_tx, + mode_reply_rx_to_pus, + mode_reply_rx_to_parent, + composite_request_tx, + hk_reply_rx, + tm_rx, + switch_request_tx, + handler: PcduHandler::new( + UniqueApidTargetId::new(Apid::Eps as u16, 0), + "TEST_PCDU", + mode_interface, + composite_request_rx, + hk_reply_tx, + switch_reqest_rx, + tm_tx, + SerialInterfaceTest::default(), + shared_switch_map, + ), + } + } + + pub fn verify_switch_info_req_was_sent(&self, expected_queue_len: usize) { + // Check that there is now communication happening. + let mut send_queue_mut = self.handler.com_interface.send_queue.borrow_mut(); + assert_eq!(send_queue_mut.len(), expected_queue_len); + let packet_sent = send_queue_mut.pop_front().unwrap(); + drop(send_queue_mut); + let pcdu_req: PcduRequest = serde_json::from_slice(&packet_sent).unwrap(); + assert_eq!(pcdu_req, PcduRequest::RequestSwitchInfo); + } + + pub fn verify_switch_req_was_sent( + &self, + expected_queue_len: usize, + switch_id: PcduSwitch, + target_state: SwitchStateBinary, + ) { + // Check that there is now communication happening. + let mut send_queue_mut = self.handler.com_interface.send_queue.borrow_mut(); + assert_eq!(send_queue_mut.len(), expected_queue_len); + let packet_sent = send_queue_mut.pop_front().unwrap(); + drop(send_queue_mut); + let pcdu_req: PcduRequest = serde_json::from_slice(&packet_sent).unwrap(); + assert_eq!( + pcdu_req, + PcduRequest::SwitchDevice { + switch: switch_id, + state: target_state + } + ) + } + + pub fn verify_switch_reply_received( + &self, + expected_queue_len: usize, + expected_map: SwitchMapBinary, + ) { + // Check that a switch reply was read back. + let mut reply_received_mut = self.handler.com_interface.reply_queue.borrow_mut(); + assert_eq!(reply_received_mut.len(), expected_queue_len); + let reply_received = reply_received_mut.pop_front().unwrap(); + let sim_reply: SimReply = serde_json::from_str(&reply_received).unwrap(); + let pcdu_reply = PcduReply::from_sim_message(&sim_reply).unwrap(); + assert_eq!(pcdu_reply, PcduReply::SwitchInfo(expected_map)); + } + } + + #[test] + fn test_basic_handler() { + let mut testbench = PcduTestbench::new(); + assert_eq!(testbench.handler.com_interface.send_queue.borrow().len(), 0); + assert_eq!( + testbench.handler.com_interface.reply_queue.borrow().len(), + 0 + ); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Off as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); + testbench.handler.periodic_operation(OpCode::RegularOp); + testbench + .handler + .periodic_operation(OpCode::PollAndRecvReplies); + // Handler is OFF, no changes expected. + assert_eq!(testbench.handler.com_interface.send_queue.borrow().len(), 0); + assert_eq!( + testbench.handler.com_interface.reply_queue.borrow().len(), + 0 + ); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Off as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); + } + + #[test] + fn test_normal_mode() { + let mut testbench = PcduTestbench::new(); + testbench + .mode_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, PUS_MODE_SERVICE.id()), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + )) + .expect("failed to send mode request"); + let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap(); + assert!(!switch_map_shared.valid); + drop(switch_map_shared); + testbench.handler.periodic_operation(OpCode::RegularOp); + testbench + .handler + .periodic_operation(OpCode::PollAndRecvReplies); + // Check correctness of mode. + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Normal as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0); + + testbench.verify_switch_info_req_was_sent(1); + testbench.verify_switch_reply_received(1, SwitchMapBinaryWrapper::default().0); + + let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap(); + assert!(switch_map_shared.valid); + drop(switch_map_shared); + } + + #[test] + fn test_switch_request_handling() { + let mut testbench = PcduTestbench::new(); + testbench + .mode_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, PUS_MODE_SERVICE.id()), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + )) + .expect("failed to send mode request"); + testbench + .switch_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, MGM_HANDLER_0.id()), + SwitchRequest::new(0, SwitchStateBinary::On), + )) + .expect("failed to send switch request"); + testbench.handler.periodic_operation(OpCode::RegularOp); + testbench + .handler + .periodic_operation(OpCode::PollAndRecvReplies); + + testbench.verify_switch_req_was_sent(2, PcduSwitch::Mgm, SwitchStateBinary::On); + testbench.verify_switch_info_req_was_sent(1); + let mut switch_map = SwitchMapBinaryWrapper::default().0; + *switch_map + .get_mut(&PcduSwitch::Mgm) + .expect("switch state setting failed") = SwitchStateBinary::On; + testbench.verify_switch_reply_received(1, switch_map); + + let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap(); + assert!(switch_map_shared.valid); + drop(switch_map_shared); + } +} diff --git a/satrs-example/src/hk.rs b/satrs-example/src/hk.rs index 0852d04..bfad5e8 100644 --- a/satrs-example/src/hk.rs +++ b/satrs-example/src/hk.rs @@ -1,7 +1,9 @@ use derive_new::new; use satrs::hk::UniqueId; use satrs::request::UniqueApidTargetId; -use satrs::spacepackets::ByteConversionError; +use satrs::spacepackets::ecss::hk; +use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; +use satrs::spacepackets::{ByteConversionError, SpHeader}; #[derive(Debug, new, Copy, Clone)] pub struct HkUniqueId { @@ -33,3 +35,35 @@ impl HkUniqueId { Ok(8) } } + +#[derive(new)] +pub struct PusHkHelper { + component_id: UniqueApidTargetId, +} + +impl PusHkHelper { + pub fn generate_hk_report_packet< + 'a, + 'b, + HkWriter: FnMut(&mut [u8]) -> Result, + >( + &self, + timestamp: &'a [u8], + set_id: u32, + hk_data_writer: &mut HkWriter, + buf: &'b mut [u8], + ) -> Result, ByteConversionError> { + let sec_header = + PusTmSecondaryHeader::new(3, hk::Subservice::TmHkPacket as u8, 0, 0, timestamp); + buf[0..4].copy_from_slice(&self.component_id.unique_id.to_be_bytes()); + buf[4..8].copy_from_slice(&set_id.to_be_bytes()); + let (_, second_half) = buf.split_at_mut(8); + let hk_data_len = hk_data_writer(second_half)?; + Ok(PusTmCreator::new( + SpHeader::new_from_apid(self.component_id.apid), + sec_header, + &buf[0..8 + hk_data_len], + true, + )) + } +} diff --git a/satrs-example/src/interface/mod.rs b/satrs-example/src/interface/mod.rs index d10d73f..efe9b69 100644 --- a/satrs-example/src/interface/mod.rs +++ b/satrs-example/src/interface/mod.rs @@ -1,3 +1,4 @@ //! This module contains all component related to the direct interface of the example. +pub mod sim_client_udp; pub mod tcp; pub mod udp; diff --git a/satrs-example/src/interface/sim_client_udp.rs b/satrs-example/src/interface/sim_client_udp.rs new file mode 100644 index 0000000..16db261 --- /dev/null +++ b/satrs-example/src/interface/sim_client_udp.rs @@ -0,0 +1,420 @@ +use std::{ + collections::HashMap, + net::{Ipv4Addr, SocketAddr, SocketAddrV4, UdpSocket}, + sync::mpsc, + time::Duration, +}; + +use satrs::pus::HandlingStatus; +use satrs_minisim::{ + udp::SIM_CTRL_PORT, SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimReply, + SimRequest, +}; +use satrs_minisim::{SimCtrlReply, SimCtrlRequest}; + +struct SimReplyMap(pub HashMap>); + +pub fn create_sim_client(sim_request_rx: mpsc::Receiver) -> Option { + match SimClientUdp::new( + SocketAddr::V4(SocketAddrV4::new(Ipv4Addr::LOCALHOST, SIM_CTRL_PORT)), + sim_request_rx, + ) { + Ok(sim_client) => { + log::info!("simulator client connection success"); + return Some(sim_client); + } + Err(e) => { + log::warn!("sim client creation error: {}", e); + } + } + None +} + +#[derive(thiserror::Error, Debug)] +pub enum SimClientCreationError { + #[error("io error: {0}")] + Io(#[from] std::io::Error), + #[error("timeout when trying to connect to sim UDP server")] + Timeout, + #[error("invalid ping reply when trying connection to UDP sim server")] + InvalidReplyJsonError(#[from] serde_json::Error), + #[error("invalid sim reply, not pong reply as expected: {0:?}")] + ReplyIsNotPong(SimReply), +} + +pub struct SimClientUdp { + udp_client: UdpSocket, + simulator_addr: SocketAddr, + sim_request_rx: mpsc::Receiver, + reply_map: SimReplyMap, + reply_buf: [u8; 4096], +} + +impl SimClientUdp { + pub fn new( + simulator_addr: SocketAddr, + sim_request_rx: mpsc::Receiver, + ) -> Result { + let mut reply_buf: [u8; 4096] = [0; 4096]; + let mut udp_client = UdpSocket::bind("127.0.0.1:0")?; + udp_client.set_read_timeout(Some(Duration::from_millis(100)))?; + Self::attempt_connection(&mut udp_client, simulator_addr, &mut reply_buf)?; + udp_client.set_nonblocking(true)?; + Ok(Self { + udp_client, + simulator_addr, + sim_request_rx, + reply_map: SimReplyMap(HashMap::new()), + reply_buf, + }) + } + + pub fn attempt_connection( + udp_client: &mut UdpSocket, + simulator_addr: SocketAddr, + reply_buf: &mut [u8], + ) -> Result<(), SimClientCreationError> { + let sim_req = SimRequest::new_with_epoch_time(SimCtrlRequest::Ping); + let sim_req_json = serde_json::to_string(&sim_req).expect("failed to serialize SimRequest"); + udp_client.send_to(sim_req_json.as_bytes(), simulator_addr)?; + match udp_client.recv(reply_buf) { + Ok(reply_len) => { + let sim_reply: SimReply = serde_json::from_slice(&reply_buf[0..reply_len])?; + if sim_reply.component() != SimComponent::SimCtrl { + return Err(SimClientCreationError::ReplyIsNotPong(sim_reply)); + } + let sim_ctrl_reply = + SimCtrlReply::from_sim_message(&sim_reply).expect("invalid SIM reply"); + match sim_ctrl_reply { + SimCtrlReply::InvalidRequest(_) => { + panic!("received invalid request reply from UDP sim server") + } + SimCtrlReply::Pong => Ok(()), + } + } + Err(e) => { + if e.kind() == std::io::ErrorKind::TimedOut + || e.kind() == std::io::ErrorKind::WouldBlock + { + Err(SimClientCreationError::Timeout) + } else { + Err(SimClientCreationError::Io(e)) + } + } + } + } + + pub fn operation(&mut self) -> HandlingStatus { + let mut no_sim_requests_handled = true; + let mut no_data_from_udp_server_received = true; + loop { + match self.sim_request_rx.try_recv() { + Ok(request) => { + let request_json = + serde_json::to_string(&request).expect("failed to serialize SimRequest"); + if let Err(e) = self + .udp_client + .send_to(request_json.as_bytes(), self.simulator_addr) + { + log::error!("error sending data to UDP SIM server: {}", e); + break; + } else { + no_sim_requests_handled = false; + } + } + Err(e) => match e { + mpsc::TryRecvError::Empty => { + break; + } + mpsc::TryRecvError::Disconnected => { + log::warn!("SIM request sender disconnected"); + break; + } + }, + } + } + loop { + match self.udp_client.recv(&mut self.reply_buf) { + Ok(recvd_bytes) => { + no_data_from_udp_server_received = false; + let sim_reply_result: serde_json::Result = + serde_json::from_slice(&self.reply_buf[0..recvd_bytes]); + match sim_reply_result { + Ok(sim_reply) => { + if let Some(sender) = self.reply_map.0.get(&sim_reply.component()) { + sender.send(sim_reply).expect("failed to send SIM reply"); + } else { + log::warn!( + "no recipient for SIM reply from component {:?}", + sim_reply.component() + ); + } + } + Err(e) => { + log::warn!("failed to deserialize SIM reply: {}", e); + } + } + } + Err(e) => { + if e.kind() == std::io::ErrorKind::WouldBlock + || e.kind() == std::io::ErrorKind::TimedOut + { + break; + } + log::error!("error receiving data from UDP SIM server: {}", e); + break; + } + } + } + if no_sim_requests_handled && no_data_from_udp_server_received { + return HandlingStatus::Empty; + } + HandlingStatus::HandledOne + } + + pub fn add_reply_recipient( + &mut self, + component: SimComponent, + reply_sender: mpsc::Sender, + ) { + self.reply_map.0.insert(component, reply_sender); + } +} + +#[cfg(test)] +pub mod tests { + use std::{ + collections::HashMap, + net::{SocketAddr, UdpSocket}, + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc, Arc, + }, + time::Duration, + }; + + use satrs_minisim::{ + eps::{PcduReply, PcduRequest}, + SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, + SimReply, SimRequest, + }; + + use super::SimClientUdp; + + struct UdpSimTestServer { + udp_server: UdpSocket, + request_tx: mpsc::Sender, + reply_rx: mpsc::Receiver, + last_sender: Option, + stop_signal: Arc, + recv_buf: [u8; 1024], + } + + impl UdpSimTestServer { + pub fn new( + request_tx: mpsc::Sender, + reply_rx: mpsc::Receiver, + stop_signal: Arc, + ) -> Self { + let udp_server = UdpSocket::bind("127.0.0.1:0").expect("creating UDP server failed"); + udp_server + .set_nonblocking(true) + .expect("failed to set UDP server to non-blocking"); + Self { + udp_server, + request_tx, + reply_rx, + last_sender: None, + stop_signal, + recv_buf: [0; 1024], + } + } + + pub fn operation(&mut self) { + loop { + let mut no_sim_replies_handled = true; + let mut no_data_received = true; + if self.stop_signal.load(Ordering::Relaxed) { + break; + } + if let Some(last_sender) = self.last_sender { + loop { + match self.reply_rx.try_recv() { + Ok(sim_reply) => { + let sim_reply_json = serde_json::to_string(&sim_reply) + .expect("failed to serialize SimReply"); + self.udp_server + .send_to(sim_reply_json.as_bytes(), last_sender) + .expect("failed to send reply to client from UDP server"); + no_sim_replies_handled = false; + } + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + panic!("reply sender disconnected") + } + }, + } + } + } + + loop { + match self.udp_server.recv_from(&mut self.recv_buf) { + Ok((read_bytes, from)) => { + let sim_request: SimRequest = + serde_json::from_slice(&self.recv_buf[0..read_bytes]) + .expect("failed to deserialize SimRequest"); + if sim_request.component() == SimComponent::SimCtrl { + // For a ping, we perform the reply handling here directly + let sim_ctrl_request = + SimCtrlRequest::from_sim_message(&sim_request) + .expect("failed to convert SimRequest to SimCtrlRequest"); + match sim_ctrl_request { + SimCtrlRequest::Ping => { + no_data_received = false; + self.last_sender = Some(from); + let sim_reply = SimReply::new(&SimCtrlReply::Pong); + let sim_reply_json = serde_json::to_string(&sim_reply) + .expect("failed to serialize SimReply"); + self.udp_server + .send_to(sim_reply_json.as_bytes(), from) + .expect( + "failed to send reply to client from UDP server", + ); + } + }; + } + // Forward each SIM request for testing purposes. + self.request_tx + .send(sim_request) + .expect("failed to send request"); + } + Err(e) => { + if e.kind() != std::io::ErrorKind::WouldBlock + && e.kind() != std::io::ErrorKind::TimedOut + { + panic!("UDP server error: {}", e); + } + break; + } + } + } + if no_sim_replies_handled && no_data_received { + std::thread::sleep(Duration::from_millis(5)); + } + } + } + + pub fn local_addr(&self) -> SocketAddr { + self.udp_server.local_addr().unwrap() + } + } + + #[test] + fn basic_connection_test() { + let (server_sim_request_tx, server_sim_request_rx) = mpsc::channel(); + let (_server_sim_reply_tx, server_sim_reply_rx) = mpsc::channel(); + let stop_signal = Arc::new(AtomicBool::new(false)); + let mut udp_server = UdpSimTestServer::new( + server_sim_request_tx, + server_sim_reply_rx, + stop_signal.clone(), + ); + let server_addr = udp_server.local_addr(); + let (_client_sim_req_tx, client_sim_req_rx) = mpsc::channel(); + // Need to spawn the simulator UDP server before calling the client constructor. + let jh0 = std::thread::spawn(move || { + udp_server.operation(); + }); + // Creating the client also performs the connection test. + SimClientUdp::new(server_addr, client_sim_req_rx).unwrap(); + let sim_request = server_sim_request_rx + .recv_timeout(Duration::from_millis(50)) + .expect("no SIM request received"); + let ping_request = SimCtrlRequest::from_sim_message(&sim_request) + .expect("failed to create SimCtrlRequest"); + assert_eq!(ping_request, SimCtrlRequest::Ping); + // Stop the server. + stop_signal.store(true, Ordering::Relaxed); + jh0.join().unwrap(); + } + + #[test] + fn basic_request_reply_test() { + let (server_sim_request_tx, server_sim_request_rx) = mpsc::channel(); + let (server_sim_reply_tx, sever_sim_reply_rx) = mpsc::channel(); + let stop_signal = Arc::new(AtomicBool::new(false)); + let mut udp_server = UdpSimTestServer::new( + server_sim_request_tx, + sever_sim_reply_rx, + stop_signal.clone(), + ); + let server_addr = udp_server.local_addr(); + let (client_sim_req_tx, client_sim_req_rx) = mpsc::channel(); + let (client_pcdu_reply_tx, client_pcdu_reply_rx) = mpsc::channel(); + // Need to spawn the simulator UDP server before calling the client constructor. + let jh0 = std::thread::spawn(move || { + udp_server.operation(); + }); + + // Creating the client also performs the connection test. + let mut client = SimClientUdp::new(server_addr, client_sim_req_rx).unwrap(); + client.add_reply_recipient(SimComponent::Pcdu, client_pcdu_reply_tx); + + let sim_request = server_sim_request_rx + .recv_timeout(Duration::from_millis(50)) + .expect("no SIM request received"); + let ping_request = SimCtrlRequest::from_sim_message(&sim_request) + .expect("failed to create SimCtrlRequest"); + assert_eq!(ping_request, SimCtrlRequest::Ping); + + let pcdu_req = PcduRequest::RequestSwitchInfo; + client_sim_req_tx + .send(SimRequest::new_with_epoch_time(pcdu_req)) + .expect("send failed"); + client.operation(); + + // Check that the request arrives properly at the server. + let sim_request = server_sim_request_rx + .recv_timeout(Duration::from_millis(50)) + .expect("no SIM request received"); + let req_recvd_on_server = + PcduRequest::from_sim_message(&sim_request).expect("failed to create SimCtrlRequest"); + matches!(req_recvd_on_server, PcduRequest::RequestSwitchInfo); + + // We inject the reply ourselves. + let pcdu_reply = PcduReply::SwitchInfo(HashMap::new()); + server_sim_reply_tx + .send(SimReply::new(&pcdu_reply)) + .expect("sending PCDU reply failed"); + + // Now we verify that the reply is sent by the UDP server back to the client, and then + // forwarded by the clients internal map. + let mut pcdu_reply_received = false; + for _ in 0..3 { + client.operation(); + + match client_pcdu_reply_rx.try_recv() { + Ok(sim_reply) => { + assert_eq!(sim_reply.component(), SimComponent::Pcdu); + let pcdu_reply_from_client = PcduReply::from_sim_message(&sim_reply) + .expect("failed to create PcduReply"); + assert_eq!(pcdu_reply_from_client, pcdu_reply); + pcdu_reply_received = true; + break; + } + Err(e) => match e { + mpsc::TryRecvError::Empty => std::thread::sleep(Duration::from_millis(10)), + mpsc::TryRecvError::Disconnected => panic!("reply sender disconnected"), + }, + } + } + if !pcdu_reply_received { + panic!("no reply received"); + } + + // Stop the server. + stop_signal.store(true, Ordering::Relaxed); + jh0.join().unwrap(); + } +} diff --git a/satrs-example/src/lib.rs b/satrs-example/src/lib.rs index a224fe5..889bdc5 100644 --- a/satrs-example/src/lib.rs +++ b/satrs-example/src/lib.rs @@ -9,12 +9,12 @@ pub enum DeviceMode { Normal = 2, } -pub struct TimeStampHelper { +pub struct TimestampHelper { stamper: CdsTime, time_stamp: [u8; 7], } -impl TimeStampHelper { +impl TimestampHelper { pub fn stamp(&self) -> &[u8] { &self.time_stamp } @@ -29,7 +29,7 @@ impl TimeStampHelper { } } -impl Default for TimeStampHelper { +impl Default for TimestampHelper { fn default() -> Self { Self { stamper: CdsTime::now_with_u16_days().expect("creating time stamper failed"), diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 02138c5..317e3f0 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -1,4 +1,5 @@ mod acs; +mod eps; mod events; mod hk; mod interface; @@ -7,6 +8,10 @@ mod pus; mod requests; mod tmtc; +use crate::eps::pcdu::{ + PcduHandler, SerialInterfaceDummy, SerialInterfaceToSim, SerialSimInterfaceWrapper, +}; +use crate::eps::PowerSwitchHelper; use crate::events::EventHandler; use crate::interface::udp::DynamicUdpTmHandler; use crate::pus::stack::PusStack; @@ -16,15 +21,21 @@ use log::info; use pus::test::create_test_service_dynamic; use satrs::hal::std::tcp_server::ServerConfig; use satrs::hal::std::udp_server::UdpTcServer; -use satrs::request::GenericMessage; +use satrs::pus::HandlingStatus; +use satrs::request::{GenericMessage, MessageMetadata}; use satrs::tmtc::{PacketSenderWithSharedPool, SharedPacketPool}; use satrs_example::config::pool::{create_sched_tc_pool, create_static_pools}; use satrs_example::config::tasks::{ - FREQ_MS_AOCS, FREQ_MS_EVENT_HANDLING, FREQ_MS_PUS_STACK, FREQ_MS_UDP_TMTC, + FREQ_MS_AOCS, FREQ_MS_PUS_STACK, FREQ_MS_UDP_TMTC, SIM_CLIENT_IDLE_DELAY_MS, }; use satrs_example::config::{OBSW_SERVER_ADDR, PACKET_ID_VALIDATOR, SERVER_PORT}; +use satrs_example::DeviceMode; -use crate::acs::mgm::{MgmHandlerLis3Mdl, MpscModeLeafInterface, SpiDummyInterface}; +use crate::acs::mgm::{ + MgmHandlerLis3Mdl, MpscModeLeafInterface, SpiDummyInterface, SpiSimInterface, + SpiSimInterfaceWrapper, +}; +use crate::interface::sim_client_udp::create_sim_client; use crate::interface::tcp::{SyncTcpTmSource, TcpTask}; use crate::interface::udp::{StaticUdpTmHandler, UdpTmtcServer}; use crate::logger::setup_logger; @@ -36,12 +47,14 @@ use crate::pus::scheduler::{create_scheduler_service_dynamic, create_scheduler_s use crate::pus::test::create_test_service_static; use crate::pus::{PusTcDistributor, PusTcMpscRouter}; use crate::requests::{CompositeRequest, GenericRequestRouter}; -use satrs::mode::ModeRequest; +use satrs::mode::{Mode, ModeAndSubmode, ModeRequest}; use satrs::pus::event_man::EventRequestWithToken; use satrs::spacepackets::{time::cds::CdsTime, time::TimeWriter}; -use satrs_example::config::components::{MGM_HANDLER_0, TCP_SERVER, UDP_SERVER}; +use satrs_example::config::components::{ + MGM_HANDLER_0, NO_SENDER, PCDU_HANDLER, TCP_SERVER, UDP_SERVER, +}; use std::net::{IpAddr, SocketAddr}; -use std::sync::mpsc; +use std::sync::{mpsc, Mutex}; use std::sync::{Arc, RwLock}; use std::thread; use std::time::Duration; @@ -60,9 +73,20 @@ fn static_tmtc_pool_main() { let tm_sink_tx_sender = PacketSenderWithSharedPool::new(tm_sink_tx.clone(), shared_tm_pool_wrapper.clone()); + let (sim_request_tx, sim_request_rx) = mpsc::channel(); + let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); + let (pcdu_sim_reply_tx, pcdu_sim_reply_rx) = mpsc::channel(); + let mut opt_sim_client = create_sim_client(sim_request_rx); + let (mgm_handler_composite_tx, mgm_handler_composite_rx) = - mpsc::channel::>(); - let (mgm_handler_mode_tx, mgm_handler_mode_rx) = mpsc::channel::>(); + mpsc::sync_channel::>(10); + let (pcdu_handler_composite_tx, pcdu_handler_composite_rx) = + mpsc::sync_channel::>(30); + + let (mgm_handler_mode_tx, mgm_handler_mode_rx) = + mpsc::sync_channel::>(5); + let (pcdu_handler_mode_tx, pcdu_handler_mode_rx) = + mpsc::sync_channel::>(5); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let mut request_map = GenericRequestRouter::default(); @@ -72,6 +96,12 @@ fn static_tmtc_pool_main() { request_map .mode_router_map .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx); + request_map + .composite_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx); + request_map + .mode_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_mode_tx.clone()); // This helper structure is used by all telecommand providers which need to send telecommands // to the TC source. @@ -195,26 +225,76 @@ fn static_tmtc_pool_main() { ); let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = - mpsc::channel(); + mpsc::sync_channel(5); + + let shared_switch_set = Arc::new(Mutex::default()); + let (switch_request_tx, switch_request_rx) = mpsc::sync_channel(20); + let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone()); - let dummy_spi_interface = SpiDummyInterface::default(); let shared_mgm_set = Arc::default(); - let mode_leaf_interface = MpscModeLeafInterface { + let mgm_mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, - reply_tx_to_pus: pus_mode_reply_tx, - reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx, + reply_to_pus_tx: pus_mode_reply_tx.clone(), + reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, + }; + + let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() { + sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx); + SpiSimInterfaceWrapper::Sim(SpiSimInterface { + sim_request_tx: sim_request_tx.clone(), + sim_reply_rx: mgm_sim_reply_rx, + }) + } else { + SpiSimInterfaceWrapper::Dummy(SpiDummyInterface::default()) }; let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", - mode_leaf_interface, + mgm_mode_leaf_interface, mgm_handler_composite_rx, - pus_hk_reply_tx, - tm_sink_tx, - dummy_spi_interface, + pus_hk_reply_tx.clone(), + switch_helper.clone(), + tm_sink_tx.clone(), + mgm_spi_interface, shared_mgm_set, ); + let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = + mpsc::sync_channel(10); + let pcdu_mode_leaf_interface = MpscModeLeafInterface { + request_rx: pcdu_handler_mode_rx, + reply_to_pus_tx: pus_mode_reply_tx, + reply_to_parent_tx: pcdu_handler_mode_reply_to_parent_tx, + }; + let pcdu_serial_interface = if let Some(sim_client) = opt_sim_client.as_mut() { + sim_client.add_reply_recipient(satrs_minisim::SimComponent::Pcdu, pcdu_sim_reply_tx); + SerialSimInterfaceWrapper::Sim(SerialInterfaceToSim::new( + sim_request_tx.clone(), + pcdu_sim_reply_rx, + )) + } else { + SerialSimInterfaceWrapper::Dummy(SerialInterfaceDummy::default()) + }; + + let mut pcdu_handler = PcduHandler::new( + PCDU_HANDLER, + "PCDU", + pcdu_mode_leaf_interface, + pcdu_handler_composite_rx, + pus_hk_reply_tx, + switch_request_rx, + tm_sink_tx, + pcdu_serial_interface, + shared_switch_set, + ); + // The PCDU is a critical component which should be in normal mode immediately. + pcdu_handler_mode_tx + .send(GenericMessage::new( + MessageMetadata::new(0, NO_SENDER), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)), + )) + .expect("sending initial mode request failed"); + info!("Starting TMTC and UDP task"); let jh_udp_tmtc = thread::Builder::new() .name("SATRS tmtc-udp".to_string()) @@ -247,14 +327,20 @@ fn static_tmtc_pool_main() { }) .unwrap(); - info!("Starting event handling task"); - let jh_event_handling = thread::Builder::new() - .name("sat-rs events".to_string()) - .spawn(move || loop { - event_handler.periodic_operation(); - thread::sleep(Duration::from_millis(FREQ_MS_EVENT_HANDLING)); - }) - .unwrap(); + let mut opt_jh_sim_client = None; + if let Some(mut sim_client) = opt_sim_client { + info!("Starting UDP sim client task"); + opt_jh_sim_client = Some( + thread::Builder::new() + .name("sat-rs sim adapter".to_string()) + .spawn(move || loop { + if sim_client.operation() == HandlingStatus::Empty { + std::thread::sleep(Duration::from_millis(SIM_CLIENT_IDLE_DELAY_MS)); + } + }) + .unwrap(), + ); + } info!("Starting AOCS thread"); let jh_aocs = thread::Builder::new() @@ -265,10 +351,26 @@ fn static_tmtc_pool_main() { }) .unwrap(); + info!("Starting EPS thread"); + let jh_eps = thread::Builder::new() + .name("sat-rs eps".to_string()) + .spawn(move || loop { + // TODO: We should introduce something like a fixed timeslot helper to allow a more + // declarative API. It would also be very useful for the AOCS task. + pcdu_handler.periodic_operation(eps::pcdu::OpCode::RegularOp); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(300)); + }) + .unwrap(); + info!("Starting PUS handler thread"); let jh_pus_handler = thread::Builder::new() .name("sat-rs pus".to_string()) .spawn(move || loop { + event_handler.periodic_operation(); pus_stack.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_PUS_STACK)); }) @@ -283,10 +385,13 @@ fn static_tmtc_pool_main() { jh_tm_funnel .join() .expect("Joining TM Funnel thread failed"); - jh_event_handling - .join() - .expect("Joining Event Manager thread failed"); + if let Some(jh_sim_client) = opt_jh_sim_client { + jh_sim_client + .join() + .expect("Joining SIM client thread failed"); + } jh_aocs.join().expect("Joining AOCS thread failed"); + jh_eps.join().expect("Joining EPS thread failed"); jh_pus_handler .join() .expect("Joining PUS handler thread failed"); @@ -295,22 +400,38 @@ fn static_tmtc_pool_main() { #[allow(dead_code)] fn dyn_tmtc_pool_main() { let (tc_source_tx, tc_source_rx) = mpsc::channel(); - let (tm_funnel_tx, tm_funnel_rx) = mpsc::channel(); + let (tm_sink_tx, tm_sink_rx) = mpsc::channel(); let (tm_server_tx, tm_server_rx) = mpsc::channel(); + let (sim_request_tx, sim_request_rx) = mpsc::channel(); + let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); + let (pcdu_sim_reply_tx, pcdu_sim_reply_rx) = mpsc::channel(); + let mut opt_sim_client = create_sim_client(sim_request_rx); + // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let (mgm_handler_composite_tx, mgm_handler_composite_rx) = - mpsc::channel::>(); - let (mgm_handler_mode_tx, mgm_handler_mode_rx) = mpsc::channel::>(); + mpsc::sync_channel::>(5); + let (pcdu_handler_composite_tx, pcdu_handler_composite_rx) = + mpsc::sync_channel::>(10); + let (mgm_handler_mode_tx, mgm_handler_mode_rx) = + mpsc::sync_channel::>(5); + let (pcdu_handler_mode_tx, pcdu_handler_mode_rx) = + mpsc::sync_channel::>(10); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let mut request_map = GenericRequestRouter::default(); request_map .composite_router_map - .insert(MGM_HANDLER_0.raw(), mgm_handler_composite_tx); + .insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx); request_map .mode_router_map - .insert(MGM_HANDLER_0.raw(), mgm_handler_mode_tx); + .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx); + request_map + .composite_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx); + request_map + .mode_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_mode_tx.clone()); // Create event handling components // These sender handles are used to send event requests, for example to enable or disable @@ -319,7 +440,7 @@ fn dyn_tmtc_pool_main() { let (event_request_tx, event_request_rx) = mpsc::channel::(); // The event task is the core handler to perform the event routing and TM handling as specified // in the sat-rs documentation. - let mut event_handler = EventHandler::new(tm_funnel_tx.clone(), event_rx, event_request_rx); + let mut event_handler = EventHandler::new(tm_sink_tx.clone(), event_rx, event_request_rx); let (pus_test_tx, pus_test_rx) = mpsc::channel(); let (pus_event_tx, pus_event_rx) = mpsc::channel(); @@ -342,30 +463,30 @@ fn dyn_tmtc_pool_main() { }; let pus_test_service = - create_test_service_dynamic(tm_funnel_tx.clone(), event_tx.clone(), pus_test_rx); + create_test_service_dynamic(tm_sink_tx.clone(), event_tx.clone(), pus_test_rx); let pus_scheduler_service = create_scheduler_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), tc_source_tx.clone(), pus_sched_rx, create_sched_tc_pool(), ); let pus_event_service = - create_event_service_dynamic(tm_funnel_tx.clone(), pus_event_rx, event_request_tx); + create_event_service_dynamic(tm_sink_tx.clone(), pus_event_rx, event_request_tx); let pus_action_service = create_action_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), pus_action_rx, request_map.clone(), pus_action_reply_rx, ); let pus_hk_service = create_hk_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), pus_hk_rx, request_map.clone(), pus_hk_reply_rx, ); let pus_mode_service = create_mode_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), pus_mode_rx, request_map, pus_mode_reply_rx, @@ -381,7 +502,7 @@ fn dyn_tmtc_pool_main() { let mut tmtc_task = TcSourceTaskDynamic::new( tc_source_rx, - PusTcDistributor::new(tm_funnel_tx.clone(), pus_router), + PusTcDistributor::new(tm_sink_tx.clone(), pus_router), ); let sock_addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT); @@ -410,28 +531,77 @@ fn dyn_tmtc_pool_main() { ) .expect("tcp server creation failed"); - let mut tm_funnel = TmSinkDynamic::new(sync_tm_tcp_source, tm_funnel_rx, tm_server_tx); + let mut tm_funnel = TmSinkDynamic::new(sync_tm_tcp_source, tm_sink_rx, tm_server_tx); + + let shared_switch_set = Arc::new(Mutex::default()); + let (switch_request_tx, switch_request_rx) = mpsc::sync_channel(20); + let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone()); let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = - mpsc::channel(); - let dummy_spi_interface = SpiDummyInterface::default(); + mpsc::sync_channel(5); let shared_mgm_set = Arc::default(); let mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, - reply_tx_to_pus: pus_mode_reply_tx, - reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx, + reply_to_pus_tx: pus_mode_reply_tx.clone(), + reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, + }; + + let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() { + sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx); + SpiSimInterfaceWrapper::Sim(SpiSimInterface { + sim_request_tx: sim_request_tx.clone(), + sim_reply_rx: mgm_sim_reply_rx, + }) + } else { + SpiSimInterfaceWrapper::Dummy(SpiDummyInterface::default()) }; let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", mode_leaf_interface, mgm_handler_composite_rx, - pus_hk_reply_tx, - tm_funnel_tx, - dummy_spi_interface, + pus_hk_reply_tx.clone(), + switch_helper.clone(), + tm_sink_tx.clone(), + mgm_spi_interface, shared_mgm_set, ); + let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = + mpsc::sync_channel(10); + let pcdu_mode_leaf_interface = MpscModeLeafInterface { + request_rx: pcdu_handler_mode_rx, + reply_to_pus_tx: pus_mode_reply_tx, + reply_to_parent_tx: pcdu_handler_mode_reply_to_parent_tx, + }; + let pcdu_serial_interface = if let Some(sim_client) = opt_sim_client.as_mut() { + sim_client.add_reply_recipient(satrs_minisim::SimComponent::Pcdu, pcdu_sim_reply_tx); + SerialSimInterfaceWrapper::Sim(SerialInterfaceToSim::new( + sim_request_tx.clone(), + pcdu_sim_reply_rx, + )) + } else { + SerialSimInterfaceWrapper::Dummy(SerialInterfaceDummy::default()) + }; + let mut pcdu_handler = PcduHandler::new( + PCDU_HANDLER, + "PCDU", + pcdu_mode_leaf_interface, + pcdu_handler_composite_rx, + pus_hk_reply_tx, + switch_request_rx, + tm_sink_tx, + pcdu_serial_interface, + shared_switch_set, + ); + // The PCDU is a critical component which should be in normal mode immediately. + pcdu_handler_mode_tx + .send(GenericMessage::new( + MessageMetadata::new(0, NO_SENDER), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)), + )) + .expect("sending initial mode request failed"); + info!("Starting TMTC and UDP task"); let jh_udp_tmtc = thread::Builder::new() .name("sat-rs tmtc-udp".to_string()) @@ -464,14 +634,20 @@ fn dyn_tmtc_pool_main() { }) .unwrap(); - info!("Starting event handling task"); - let jh_event_handling = thread::Builder::new() - .name("sat-rs events".to_string()) - .spawn(move || loop { - event_handler.periodic_operation(); - thread::sleep(Duration::from_millis(FREQ_MS_EVENT_HANDLING)); - }) - .unwrap(); + let mut opt_jh_sim_client = None; + if let Some(mut sim_client) = opt_sim_client { + info!("Starting UDP sim client task"); + opt_jh_sim_client = Some( + thread::Builder::new() + .name("sat-rs sim adapter".to_string()) + .spawn(move || loop { + if sim_client.operation() == HandlingStatus::Empty { + std::thread::sleep(Duration::from_millis(SIM_CLIENT_IDLE_DELAY_MS)); + } + }) + .unwrap(), + ); + } info!("Starting AOCS thread"); let jh_aocs = thread::Builder::new() @@ -482,11 +658,27 @@ fn dyn_tmtc_pool_main() { }) .unwrap(); + info!("Starting EPS thread"); + let jh_eps = thread::Builder::new() + .name("sat-rs eps".to_string()) + .spawn(move || loop { + // TODO: We should introduce something like a fixed timeslot helper to allow a more + // declarative API. It would also be very useful for the AOCS task. + pcdu_handler.periodic_operation(eps::pcdu::OpCode::RegularOp); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(300)); + }) + .unwrap(); + info!("Starting PUS handler thread"); let jh_pus_handler = thread::Builder::new() .name("sat-rs pus".to_string()) .spawn(move || loop { pus_stack.periodic_operation(); + event_handler.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_PUS_STACK)); }) .unwrap(); @@ -500,10 +692,13 @@ fn dyn_tmtc_pool_main() { jh_tm_funnel .join() .expect("Joining TM Funnel thread failed"); - jh_event_handling - .join() - .expect("Joining Event Manager thread failed"); + if let Some(jh_sim_client) = opt_jh_sim_client { + jh_sim_client + .join() + .expect("Joining SIM client thread failed"); + } jh_aocs.join().expect("Joining AOCS thread failed"); + jh_eps.join().expect("Joining EPS thread failed"); jh_pus_handler .join() .expect("Joining PUS handler thread failed"); diff --git a/satrs-example/src/pus/action.rs b/satrs-example/src/pus/action.rs index 03d362c..238f1c5 100644 --- a/satrs-example/src/pus/action.rs +++ b/satrs-example/src/pus/action.rs @@ -341,7 +341,7 @@ mod tests { let (tm_funnel_tx, tm_funnel_rx) = mpsc::channel(); let (pus_action_tx, pus_action_rx) = mpsc::channel(); let (action_reply_tx, action_reply_rx) = mpsc::channel(); - let (action_req_tx, action_req_rx) = mpsc::channel(); + let (action_req_tx, action_req_rx) = mpsc::sync_channel(10); let verif_reporter = TestVerificationReporter::new(owner_id); let mut generic_req_router = GenericRequestRouter::default(); generic_req_router diff --git a/satrs-example/src/pus/hk.rs b/satrs-example/src/pus/hk.rs index 0092241..c0d21bf 100644 --- a/satrs-example/src/pus/hk.rs +++ b/satrs-example/src/pus/hk.rs @@ -12,6 +12,7 @@ use satrs::pus::{ PusPacketHandlingError, PusReplyHandler, PusServiceHelper, PusTcToRequestConverter, }; use satrs::request::{GenericMessage, UniqueApidTargetId}; +use satrs::res_code::ResultU16; use satrs::spacepackets::ecss::tc::PusTcReader; use satrs::spacepackets::ecss::{hk, PusPacket, PusServiceId}; use satrs::tmtc::{PacketAsVec, PacketSenderWithSharedPool}; @@ -32,8 +33,10 @@ pub struct HkReply { } #[derive(Clone, PartialEq, Debug)] +#[allow(dead_code)] pub enum HkReplyVariant { Ack, + Failed(ResultU16), } #[derive(Default)] @@ -69,6 +72,15 @@ impl PusReplyHandler for HkReplyHandler { .completion_success(tm_sender, started_token, time_stamp) .expect("sending completion success verification failed"); } + HkReplyVariant::Failed(failure_code) => { + verification_handler + .completion_failure( + tm_sender, + started_token, + FailParams::new(time_stamp, &failure_code, &[]), + ) + .expect("sending completion success verification failed"); + } }; Ok(true) } diff --git a/satrs-example/src/pus/mod.rs b/satrs-example/src/pus/mod.rs index f305308..b2e7f8a 100644 --- a/satrs-example/src/pus/mod.rs +++ b/satrs-example/src/pus/mod.rs @@ -19,7 +19,7 @@ use satrs::tmtc::{PacketAsVec, PacketInPool}; use satrs::ComponentId; use satrs_example::config::components::PUS_ROUTING_SERVICE; use satrs_example::config::{tmtc_err, CustomPusServiceId}; -use satrs_example::TimeStampHelper; +use satrs_example::TimestampHelper; use std::fmt::Debug; use std::sync::mpsc::{self, Sender}; @@ -53,7 +53,7 @@ pub struct PusTcDistributor { pub tm_sender: TmSender, pub verif_reporter: VerificationReporter, pub pus_router: PusTcMpscRouter, - stamp_helper: TimeStampHelper, + stamp_helper: TimestampHelper, } impl PusTcDistributor { @@ -66,7 +66,7 @@ impl PusTcDistributor { PUS_ROUTING_SERVICE.apid, ), pus_router, - stamp_helper: TimeStampHelper::default(), + stamp_helper: TimestampHelper::default(), } } diff --git a/satrs-example/src/requests.rs b/satrs-example/src/requests.rs index 445e05e..316a486 100644 --- a/satrs-example/src/requests.rs +++ b/satrs-example/src/requests.rs @@ -28,8 +28,9 @@ pub enum CompositeRequest { pub struct GenericRequestRouter { pub id: ComponentId, // All messages which do not have a dedicated queue. - pub composite_router_map: HashMap>>, - pub mode_router_map: HashMap>>, + pub composite_router_map: + HashMap>>, + pub mode_router_map: HashMap>>, } impl Default for GenericRequestRouter { diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index cf5848a..6b8c32a 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -11,6 +11,8 @@ serde_json = "1" log = "0.4" thiserror = "1" fern = "0.5" +strum = { version = "0.26", features = ["derive"] } +num_enum = "0.7" humantime = "2" [dependencies.asynchronix] diff --git a/satrs-minisim/README.md b/satrs-minisim/README.md new file mode 100644 index 0000000..42f949d --- /dev/null +++ b/satrs-minisim/README.md @@ -0,0 +1,32 @@ +sat-rs minisim +====== + +This crate contains a mini-simulator based on the open-source discrete-event simulation framework +[asynchronix](https://github.com/asynchronics/asynchronix). + +Right now, this crate is primarily used together with the +[`satrs-example` application](https://egit.irs.uni-stuttgart.de/rust/sat-rs/src/branch/main/satrs-example) +to simulate the devices connected to the example application. + +You can simply run this application using + +```sh +cargo run +``` + +or + +```sh +cargo run -p satrs-minisim +``` + +in the workspace. The mini simulator uses the UDP port 7303 to exchange simulation requests and +simulation replies with any other application. + +The simulator was designed in a modular way to be scalable and adaptable to other communication +schemes. This might allow it to serve a mini-simulator for other example applications which +still have similar device handlers. + +The following graph shows the high-level architecture of the mini-simulator. + +Mini simulator architecture diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 77b8cb0..2403487 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -6,14 +6,17 @@ use asynchronix::{ }; use satrs::power::SwitchStateBinary; use satrs_minisim::{ - acs::{MgmReply, MgmSensorValues, MgtDipole, MgtHkSet, MgtReply, MGT_GEN_MAGNETIC_FIELD}, + acs::{ + lis3mdl::MgmLis3MdlReply, MgmReplyCommon, MgmReplyProvider, MgmSensorValuesMicroTesla, + MgtDipole, MgtHkSet, MgtReply, MGT_GEN_MAGNETIC_FIELD, + }, SimReply, }; use crate::time::current_millis; -// Earth magnetic field varies between -30 uT and 30 uT -const AMPLITUDE_MGM: f32 = 0.03; +// Earth magnetic field varies between roughly -30 uT and 30 uT +const AMPLITUDE_MGM_UT: f32 = 30.0; // Lets start with a simple frequency here. const FREQUENCY_MGM: f32 = 1.0; const PHASE_X: f32 = 0.0; @@ -23,38 +26,37 @@ const PHASE_Z: f32 = 0.2; /// Simple model for a magnetometer where the measure magnetic fields are modeled with sine waves. /// -/// Please note that that a more realistic MGM model wouold include the following components -/// which are not included here to simplify the model: -/// -/// 1. It would probably generate signed [i16] values which need to be converted to SI units -/// because it is a digital sensor -/// 2. It would sample the magnetic field at a high fixed rate. This might not be possible for -/// a general purpose OS, but self self-sampling at a relatively high rate (20-40 ms) might -/// stil lbe possible. -pub struct MagnetometerModel { +/// An ideal sensor would sample the magnetic field at a high fixed rate. This might not be +/// possible for a general purpose OS, but self self-sampling at a relatively high rate (20-40 ms) +/// might still be possible and is probably sufficient for many OBSW needs. +pub struct MagnetometerModel { pub switch_state: SwitchStateBinary, pub periodicity: Duration, - pub external_mag_field: Option, + pub external_mag_field: Option, pub reply_sender: mpsc::Sender, + pub phatom: std::marker::PhantomData, } -impl MagnetometerModel { - pub fn new(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { +impl MagnetometerModel { + pub fn new_for_lis3mdl(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { Self { switch_state: SwitchStateBinary::Off, periodicity, external_mag_field: None, reply_sender, + phatom: std::marker::PhantomData, } } +} +impl MagnetometerModel { pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { self.switch_state = switch_state; } pub async fn send_sensor_values(&mut self, _: (), scheduler: &Scheduler) { self.reply_sender - .send(SimReply::new(MgmReply { + .send(ReplyProvider::create_mgm_reply(MgmReplyCommon { switch_state: self.switch_state, sensor_values: self.calculate_current_mgm_tuple(current_millis(scheduler.time())), })) @@ -63,23 +65,23 @@ impl MagnetometerModel { // Devices like magnetorquers generate a strong magnetic field which overrides the default // model for the measured magnetic field. - pub async fn apply_external_magnetic_field(&mut self, field: MgmSensorValues) { + pub async fn apply_external_magnetic_field(&mut self, field: MgmSensorValuesMicroTesla) { self.external_mag_field = Some(field); } - fn calculate_current_mgm_tuple(&self, time_ms: u64) -> MgmSensorValues { + fn calculate_current_mgm_tuple(&self, time_ms: u64) -> MgmSensorValuesMicroTesla { if SwitchStateBinary::On == self.switch_state { if let Some(ext_field) = self.external_mag_field { return ext_field; } let base_sin_val = 2.0 * PI * FREQUENCY_MGM * (time_ms as f32 / 1000.0); - return MgmSensorValues { - x: AMPLITUDE_MGM * (base_sin_val + PHASE_X).sin(), - y: AMPLITUDE_MGM * (base_sin_val + PHASE_Y).sin(), - z: AMPLITUDE_MGM * (base_sin_val + PHASE_Z).sin(), + return MgmSensorValuesMicroTesla { + x: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_X).sin(), + y: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Y).sin(), + z: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Z).sin(), }; } - MgmSensorValues { + MgmSensorValuesMicroTesla { x: 0.0, y: 0.0, z: 0.0, @@ -87,13 +89,13 @@ impl MagnetometerModel { } } -impl Model for MagnetometerModel {} +impl Model for MagnetometerModel {} pub struct MagnetorquerModel { switch_state: SwitchStateBinary, torquing: bool, torque_dipole: MgtDipole, - pub gen_magnetic_field: Output, + pub gen_magnetic_field: Output, reply_sender: mpsc::Sender, } @@ -146,14 +148,14 @@ impl MagnetorquerModel { pub fn send_housekeeping_data(&mut self) { self.reply_sender - .send(SimReply::new(MgtReply::Hk(MgtHkSet { + .send(SimReply::new(&MgtReply::Hk(MgtHkSet { dipole: self.torque_dipole, torquing: self.torquing, }))) .unwrap(); } - fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValues { + fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValuesMicroTesla { // Simplified model: Just returns some fixed magnetic field for now. // Later, we could make this more fancy by incorporating the commanded dipole. MGT_GEN_MAGNETIC_FIELD @@ -179,9 +181,12 @@ pub mod tests { use satrs::power::SwitchStateBinary; use satrs_minisim::{ - acs::{MgmReply, MgmRequest, MgtDipole, MgtHkSet, MgtReply, MgtRequest}, + acs::{ + lis3mdl::{self, MgmLis3MdlReply}, + MgmRequestLis3Mdl, MgtDipole, MgtHkSet, MgtReply, MgtRequest, + }, eps::PcduSwitch, - SerializableSimMsgPayload, SimMessageProvider, SimRequest, SimTarget, + SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, }; use crate::{eps::tests::switch_device_on, test_helpers::SimTestbench}; @@ -189,7 +194,7 @@ pub mod tests { #[test] fn test_basic_mgm_request() { let mut sim_testbench = SimTestbench::new(); - let request = SimRequest::new_with_epoch_time(MgmRequest::RequestSensorData); + let request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); sim_testbench .send_request(request) .expect("sending MGM request failed"); @@ -198,13 +203,13 @@ pub mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Mgm); - let reply = MgmReply::from_sim_message(&sim_reply) + assert_eq!(sim_reply.component(), SimComponent::MgmLis3Mdl); + let reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); - assert_eq!(reply.switch_state, SwitchStateBinary::Off); - assert_eq!(reply.sensor_values.x, 0.0); - assert_eq!(reply.sensor_values.y, 0.0); - assert_eq!(reply.sensor_values.z, 0.0); + assert_eq!(reply.common.switch_state, SwitchStateBinary::Off); + assert_eq!(reply.common.sensor_values.x, 0.0); + assert_eq!(reply.common.sensor_values.y, 0.0); + assert_eq!(reply.common.sensor_values.z, 0.0); } #[test] @@ -212,7 +217,7 @@ pub mod tests { let mut sim_testbench = SimTestbench::new(); switch_device_on(&mut sim_testbench, PcduSwitch::Mgm); - let mut request = SimRequest::new_with_epoch_time(MgmRequest::RequestSensorData); + let mut request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); sim_testbench .send_request(request) .expect("sending MGM request failed"); @@ -221,12 +226,12 @@ pub mod tests { let mut sim_reply_res = sim_testbench.try_receive_next_reply(); assert!(sim_reply_res.is_some()); let mut sim_reply = sim_reply_res.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Mgm); - let first_reply = MgmReply::from_sim_message(&sim_reply) + assert_eq!(sim_reply.component(), SimComponent::MgmLis3Mdl); + let first_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); sim_testbench.step_by(Duration::from_millis(50)); - request = SimRequest::new_with_epoch_time(MgmRequest::RequestSensorData); + request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); sim_testbench .send_request(request) .expect("sending MGM request failed"); @@ -236,8 +241,24 @@ pub mod tests { assert!(sim_reply_res.is_some()); sim_reply = sim_reply_res.unwrap(); - let second_reply = MgmReply::from_sim_message(&sim_reply) + let second_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); + let x_conv_back = second_reply.raw.x as f32 + * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS + * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; + let y_conv_back = second_reply.raw.y as f32 + * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS + * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; + let z_conv_back = second_reply.raw.z as f32 + * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS + * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; + let diff_x = (second_reply.common.sensor_values.x - x_conv_back).abs(); + assert!(diff_x < 0.01, "diff x too large: {}", diff_x); + let diff_y = (second_reply.common.sensor_values.y - y_conv_back).abs(); + assert!(diff_y < 0.01, "diff y too large: {}", diff_y); + let diff_z = (second_reply.common.sensor_values.z - z_conv_back).abs(); + assert!(diff_z < 0.01, "diff z too large: {}", diff_z); + // assert_eq!(second_reply.raw_reply, SwitchStateBinary::On); // Check that the values are changing. assert!(first_reply != second_reply); } diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs index 9255932..4fe1495 100644 --- a/satrs-minisim/src/controller.rs +++ b/satrs-minisim/src/controller.rs @@ -5,10 +5,10 @@ use asynchronix::{ time::{Clock, MonotonicTime, SystemClock}, }; use satrs_minisim::{ - acs::{MgmRequest, MgtRequest}, + acs::{lis3mdl::MgmLis3MdlReply, MgmRequestLis3Mdl, MgtRequest}, eps::PcduRequest, - SerializableSimMsgPayload, SimCtrlReply, SimCtrlRequest, SimMessageProvider, SimReply, - SimRequest, SimRequestError, SimTarget, + SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, + SimReply, SimRequest, SimRequestError, }; use crate::{ @@ -16,13 +16,20 @@ use crate::{ eps::PcduModel, }; +const WARNING_FOR_STALE_DATA: bool = false; + +const SIM_CTRL_REQ_WIRETAPPING: bool = false; +const MGM_REQ_WIRETAPPING: bool = false; +const PCDU_REQ_WIRETAPPING: bool = false; +const MGT_REQ_WIRETAPPING: bool = false; + // The simulation controller processes requests and drives the simulation. pub struct SimController { pub sys_clock: SystemClock, pub request_receiver: mpsc::Receiver, pub reply_sender: mpsc::Sender, pub simulation: Simulation, - pub mgm_addr: Address, + pub mgm_addr: Address>, pub pcdu_addr: Address, pub mgt_addr: Address, } @@ -33,7 +40,7 @@ impl SimController { request_receiver: mpsc::Receiver, reply_sender: mpsc::Sender, simulation: Simulation, - mgm_addr: Address, + mgm_addr: Address>, pcdu_addr: Address, mgt_addr: Address, ) -> Self { @@ -67,14 +74,14 @@ impl SimController { loop { match self.request_receiver.try_recv() { Ok(request) => { - if request.timestamp < old_timestamp { + if request.timestamp < old_timestamp && WARNING_FOR_STALE_DATA { log::warn!("stale data with timestamp {:?} received", request.timestamp); } - if let Err(e) = match request.target() { - SimTarget::SimCtrl => self.handle_ctrl_request(&request), - SimTarget::Mgm => self.handle_mgm_request(&request), - SimTarget::Mgt => self.handle_mgt_request(&request), - SimTarget::Pcdu => self.handle_pcdu_request(&request), + if let Err(e) = match request.component() { + SimComponent::SimCtrl => self.handle_ctrl_request(&request), + SimComponent::MgmLis3Mdl => self.handle_mgm_request(&request), + SimComponent::Mgt => self.handle_mgt_request(&request), + SimComponent::Pcdu => self.handle_pcdu_request(&request), } { self.handle_invalid_request_with_valid_target(e, &request) } @@ -91,19 +98,26 @@ impl SimController { fn handle_ctrl_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let sim_ctrl_request = SimCtrlRequest::from_sim_message(request)?; + if SIM_CTRL_REQ_WIRETAPPING { + log::info!("received sim ctrl request: {:?}", sim_ctrl_request); + } match sim_ctrl_request { SimCtrlRequest::Ping => { self.reply_sender - .send(SimReply::new(SimCtrlReply::Pong)) + .send(SimReply::new(&SimCtrlReply::Pong)) .expect("sending reply from sim controller failed"); } } Ok(()) } + fn handle_mgm_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { - let mgm_request = MgmRequest::from_sim_message(request)?; + let mgm_request = MgmRequestLis3Mdl::from_sim_message(request)?; + if MGM_REQ_WIRETAPPING { + log::info!("received MGM request: {:?}", mgm_request); + } match mgm_request { - MgmRequest::RequestSensorData => { + MgmRequestLis3Mdl::RequestSensorData => { self.simulation.send_event( MagnetometerModel::send_sensor_values, (), @@ -116,6 +130,9 @@ impl SimController { fn handle_pcdu_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let pcdu_request = PcduRequest::from_sim_message(request)?; + if PCDU_REQ_WIRETAPPING { + log::info!("received PCDU request: {:?}", pcdu_request); + } match pcdu_request { PcduRequest::RequestSwitchInfo => { self.simulation @@ -134,6 +151,9 @@ impl SimController { fn handle_mgt_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let mgt_request = MgtRequest::from_sim_message(request)?; + if MGT_REQ_WIRETAPPING { + log::info!("received MGT request: {:?}", mgt_request); + } match mgt_request { MgtRequest::ApplyTorque { duration, dipole } => self.simulation.send_event( MagnetorquerModel::apply_torque, @@ -156,11 +176,11 @@ impl SimController { ) { log::warn!( "received invalid {:?} request: {:?}", - request.target(), + request.component(), error ); self.reply_sender - .send(SimReply::new(SimCtrlReply::from(error))) + .send(SimReply::new(&SimCtrlReply::from(error))) .expect("sending reply from sim controller failed"); } } @@ -183,7 +203,7 @@ mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::SimCtrl); + assert_eq!(sim_reply.component(), SimComponent::SimCtrl); let reply = SimCtrlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); assert_eq!(reply, SimCtrlReply::Pong); diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs index ebbeb4e..c07e290 100644 --- a/satrs-minisim/src/eps.rs +++ b/satrs-minisim/src/eps.rs @@ -1,4 +1,4 @@ -use std::{collections::HashMap, sync::mpsc, time::Duration}; +use std::{sync::mpsc, time::Duration}; use asynchronix::{ model::{Model, Output}, @@ -6,14 +6,14 @@ use asynchronix::{ }; use satrs::power::SwitchStateBinary; use satrs_minisim::{ - eps::{PcduReply, PcduSwitch, SwitchMap}, + eps::{PcduReply, PcduSwitch, SwitchMapBinaryWrapper}, SimReply, }; pub const SWITCH_INFO_DELAY_MS: u64 = 10; pub struct PcduModel { - pub switcher_map: SwitchMap, + pub switcher_map: SwitchMapBinaryWrapper, pub mgm_switch: Output, pub mgt_switch: Output, pub reply_sender: mpsc::Sender, @@ -21,12 +21,8 @@ pub struct PcduModel { impl PcduModel { pub fn new(reply_sender: mpsc::Sender) -> Self { - let mut switcher_map = HashMap::new(); - switcher_map.insert(PcduSwitch::Mgm, SwitchStateBinary::Off); - switcher_map.insert(PcduSwitch::Mgt, SwitchStateBinary::Off); - Self { - switcher_map, + switcher_map: Default::default(), mgm_switch: Output::new(), mgt_switch: Output::new(), reply_sender, @@ -44,7 +40,7 @@ impl PcduModel { } pub fn send_switch_info(&mut self) { - let reply = SimReply::new(PcduReply::SwitchInfo(self.switcher_map.clone())); + let reply = SimReply::new(&PcduReply::SwitchInfo(self.switcher_map.0.clone())); self.reply_sender.send(reply).unwrap(); } @@ -54,6 +50,7 @@ impl PcduModel { ) { let val = self .switcher_map + .0 .get_mut(&switch_and_target_state.0) .unwrap_or_else(|| panic!("switch {:?} not found", switch_and_target_state.0)); *val = switch_and_target_state.1; @@ -76,7 +73,8 @@ pub(crate) mod tests { use std::time::Duration; use satrs_minisim::{ - eps::PcduRequest, SerializableSimMsgPayload, SimMessageProvider, SimRequest, SimTarget, + eps::{PcduRequest, SwitchMapBinary}, + SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, }; use crate::test_helpers::SimTestbench; @@ -105,14 +103,11 @@ pub(crate) mod tests { switch_device(sim_testbench, switch, SwitchStateBinary::On); } - pub(crate) fn get_all_off_switch_map() -> SwitchMap { - let mut switcher_map = SwitchMap::new(); - switcher_map.insert(super::PcduSwitch::Mgm, super::SwitchStateBinary::Off); - switcher_map.insert(super::PcduSwitch::Mgt, super::SwitchStateBinary::Off); - switcher_map + pub(crate) fn get_all_off_switch_map() -> SwitchMapBinary { + SwitchMapBinaryWrapper::default().0 } - fn check_switch_state(sim_testbench: &mut SimTestbench, expected_switch_map: &SwitchMap) { + fn check_switch_state(sim_testbench: &mut SimTestbench, expected_switch_map: &SwitchMapBinary) { let request = SimRequest::new_with_epoch_time(PcduRequest::RequestSwitchInfo); sim_testbench .send_request(request) @@ -122,7 +117,7 @@ pub(crate) mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Pcdu); + assert_eq!(sim_reply.component(), SimComponent::Pcdu); let pcdu_reply = PcduReply::from_sim_message(&sim_reply) .expect("failed to deserialize PCDU switch info"); match pcdu_reply { @@ -157,7 +152,7 @@ pub(crate) mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Pcdu); + assert_eq!(sim_reply.component(), SimComponent::Pcdu); let pcdu_reply = PcduReply::from_sim_message(&sim_reply) .expect("failed to deserialize PCDU switch info"); match pcdu_reply { diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 2762326..b3b7d44 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -1,19 +1,18 @@ use asynchronix::time::MonotonicTime; +use num_enum::{IntoPrimitive, TryFromPrimitive}; use serde::{de::DeserializeOwned, Deserialize, Serialize}; -pub const SIM_CTRL_UDP_PORT: u16 = 7303; - -#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] -pub enum SimTarget { +#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash)] +pub enum SimComponent { SimCtrl, - Mgm, + MgmLis3Mdl, Mgt, Pcdu, } #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct SimMessage { - pub target: SimTarget, + pub target: SimComponent, pub payload: String, } @@ -37,10 +36,10 @@ pub enum SimMessageType { pub trait SerializableSimMsgPayload: Serialize + DeserializeOwned + Sized { - const TARGET: SimTarget; + const TARGET: SimComponent; fn from_sim_message(sim_message: &P) -> Result> { - if sim_message.target() == Self::TARGET { + if sim_message.component() == Self::TARGET { return Ok(serde_json::from_str(sim_message.payload())?); } Err(SimMessageError::TargetRequestMissmatch(sim_message.clone())) @@ -49,7 +48,7 @@ pub trait SerializableSimMsgPayload: pub trait SimMessageProvider: Serialize + DeserializeOwned + Clone + Sized { fn msg_type(&self) -> SimMessageType; - fn target(&self) -> SimTarget; + fn component(&self) -> SimComponent; fn payload(&self) -> &String; fn from_raw_data(data: &[u8]) -> serde_json::Result { serde_json::from_slice(data) @@ -78,7 +77,7 @@ impl SimRequest { } impl SimMessageProvider for SimRequest { - fn target(&self) -> SimTarget { + fn component(&self) -> SimComponent { self.inner.target } fn payload(&self) -> &String { @@ -91,25 +90,25 @@ impl SimMessageProvider for SimRequest { } /// A generic simulation reply type. Right now, the payload data is expected to be -/// JSON, which might be changed inthe future. +/// JSON, which might be changed in the future. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct SimReply { inner: SimMessage, } impl SimReply { - pub fn new>(serializable_reply: T) -> Self { + pub fn new>(serializable_reply: &T) -> Self { Self { inner: SimMessage { target: T::TARGET, - payload: serde_json::to_string(&serializable_reply).unwrap(), + payload: serde_json::to_string(serializable_reply).unwrap(), }, } } } impl SimMessageProvider for SimReply { - fn target(&self) -> SimTarget { + fn component(&self) -> SimComponent { self.inner.target } fn payload(&self) -> &String { @@ -126,7 +125,7 @@ pub enum SimCtrlRequest { } impl SerializableSimMsgPayload for SimCtrlRequest { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } pub type SimReplyError = SimMessageError; @@ -151,7 +150,7 @@ pub enum SimCtrlReply { } impl SerializableSimMsgPayload for SimCtrlReply { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } impl From for SimCtrlReply { @@ -162,19 +161,82 @@ impl From for SimCtrlReply { pub mod eps { use super::*; + use satrs::power::{SwitchState, SwitchStateBinary}; use std::collections::HashMap; + use strum::{EnumIter, IntoEnumIterator}; - use satrs::power::SwitchStateBinary; + pub type SwitchMap = HashMap; + pub type SwitchMapBinary = HashMap; - pub type SwitchMap = HashMap; + pub struct SwitchMapWrapper(pub SwitchMap); + pub struct SwitchMapBinaryWrapper(pub SwitchMapBinary); - #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash)] + #[derive( + Debug, + Copy, + Clone, + PartialEq, + Eq, + Serialize, + Deserialize, + Hash, + EnumIter, + IntoPrimitive, + TryFromPrimitive, + )] + #[repr(u16)] pub enum PcduSwitch { Mgm = 0, Mgt = 1, } - #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + impl Default for SwitchMapBinaryWrapper { + fn default() -> Self { + let mut switch_map = SwitchMapBinary::default(); + for entry in PcduSwitch::iter() { + switch_map.insert(entry, SwitchStateBinary::Off); + } + Self(switch_map) + } + } + + impl Default for SwitchMapWrapper { + fn default() -> Self { + let mut switch_map = SwitchMap::default(); + for entry in PcduSwitch::iter() { + switch_map.insert(entry, SwitchState::Unknown); + } + Self(switch_map) + } + } + + impl SwitchMapWrapper { + pub fn new_with_init_switches_off() -> Self { + let mut switch_map = SwitchMap::default(); + for entry in PcduSwitch::iter() { + switch_map.insert(entry, SwitchState::Off); + } + Self(switch_map) + } + + pub fn from_binary_switch_map_ref(switch_map: &SwitchMapBinary) -> Self { + Self( + switch_map + .iter() + .map(|(key, value)| (*key, SwitchState::from(*value))) + .collect(), + ) + } + } + + #[derive(Debug, Copy, Clone)] + #[repr(u8)] + pub enum PcduRequestId { + SwitchDevice = 0, + RequestSwitchInfo = 1, + } + + #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] pub enum PcduRequest { SwitchDevice { switch: PcduSwitch, @@ -184,16 +246,17 @@ pub mod eps { } impl SerializableSimMsgPayload for PcduRequest { - const TARGET: SimTarget = SimTarget::Pcdu; + const TARGET: SimComponent = SimComponent::Pcdu; } - #[derive(Debug, Clone, Serialize, Deserialize)] + #[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)] pub enum PcduReply { - SwitchInfo(SwitchMap), + // Ack, + SwitchInfo(SwitchMapBinary), } impl SerializableSimMsgPayload for PcduReply { - const TARGET: SimTarget = SimTarget::Pcdu; + const TARGET: SimComponent = SimComponent::Pcdu; } } @@ -204,40 +267,116 @@ pub mod acs { use super::*; + pub trait MgmReplyProvider: Send + 'static { + fn create_mgm_reply(common: MgmReplyCommon) -> SimReply; + } + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] - pub enum MgmRequest { + pub enum MgmRequestLis3Mdl { RequestSensorData, } - impl SerializableSimMsgPayload for MgmRequest { - const TARGET: SimTarget = SimTarget::Mgm; + impl SerializableSimMsgPayload for MgmRequestLis3Mdl { + const TARGET: SimComponent = SimComponent::MgmLis3Mdl; } // Normally, small magnetometers generate their output as a signed 16 bit raw format or something // similar which needs to be converted to a signed float value with physical units. We will - // simplify this now and generate the signed float values directly. + // simplify this now and generate the signed float values directly. The unit is micro tesla. #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmSensorValues { + pub struct MgmSensorValuesMicroTesla { pub x: f32, pub y: f32, pub z: f32, } #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmReply { + pub struct MgmReplyCommon { pub switch_state: SwitchStateBinary, - pub sensor_values: MgmSensorValues, + pub sensor_values: MgmSensorValuesMicroTesla, } - impl SerializableSimMsgPayload for MgmReply { - const TARGET: SimTarget = SimTarget::Mgm; - } - - pub const MGT_GEN_MAGNETIC_FIELD: MgmSensorValues = MgmSensorValues { - x: 0.03, - y: -0.03, - z: 0.03, + pub const MGT_GEN_MAGNETIC_FIELD: MgmSensorValuesMicroTesla = MgmSensorValuesMicroTesla { + x: 30.0, + y: -30.0, + z: 30.0, }; + pub const ALL_ONES_SENSOR_VAL: i16 = 0xffff_u16 as i16; + + pub mod lis3mdl { + use super::*; + + // Field data register scaling + pub const GAUSS_TO_MICROTESLA_FACTOR: u32 = 100; + pub const FIELD_LSB_PER_GAUSS_4_SENS: f32 = 1.0 / 6842.0; + pub const FIELD_LSB_PER_GAUSS_8_SENS: f32 = 1.0 / 3421.0; + pub const FIELD_LSB_PER_GAUSS_12_SENS: f32 = 1.0 / 2281.0; + pub const FIELD_LSB_PER_GAUSS_16_SENS: f32 = 1.0 / 1711.0; + + #[derive(Default, Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgmLis3RawValues { + pub x: i16, + pub y: i16, + pub z: i16, + } + + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgmLis3MdlReply { + pub common: MgmReplyCommon, + // Raw sensor values which are transmitted by the LIS3 device in little-endian + // order. + pub raw: MgmLis3RawValues, + } + + impl MgmLis3MdlReply { + pub fn new(common: MgmReplyCommon) -> Self { + match common.switch_state { + SwitchStateBinary::Off => Self { + common, + raw: MgmLis3RawValues { + x: ALL_ONES_SENSOR_VAL, + y: ALL_ONES_SENSOR_VAL, + z: ALL_ONES_SENSOR_VAL, + }, + }, + SwitchStateBinary::On => { + let mut raw_reply: [u8; 7] = [0; 7]; + let raw_x: i16 = (common.sensor_values.x + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_y: i16 = (common.sensor_values.y + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_z: i16 = (common.sensor_values.z + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + // The first byte is a dummy byte. + raw_reply[1..3].copy_from_slice(&raw_x.to_be_bytes()); + raw_reply[3..5].copy_from_slice(&raw_y.to_be_bytes()); + raw_reply[5..7].copy_from_slice(&raw_z.to_be_bytes()); + Self { + common, + raw: MgmLis3RawValues { + x: raw_x, + y: raw_y, + z: raw_z, + }, + } + } + } + } + } + + impl SerializableSimMsgPayload for MgmLis3MdlReply { + const TARGET: SimComponent = SimComponent::MgmLis3Mdl; + } + + impl MgmReplyProvider for MgmLis3MdlReply { + fn create_mgm_reply(common: MgmReplyCommon) -> SimReply { + SimReply::new(&Self::new(common)) + } + } + } // Simple model using i16 values. #[derive(Default, Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -262,7 +401,7 @@ pub mod acs { } impl SerializableSimMsgPayload for MgtRequest { - const TARGET: SimTarget = SimTarget::Mgt; + const TARGET: SimComponent = SimComponent::Mgt; } #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -279,78 +418,12 @@ pub mod acs { } impl SerializableSimMsgPayload for MgtReply { - const TARGET: SimTarget = SimTarget::Mgm; + const TARGET: SimComponent = SimComponent::MgmLis3Mdl; } } pub mod udp { - use std::{ - net::{SocketAddr, UdpSocket}, - time::Duration, - }; - - use thiserror::Error; - - use crate::{SimReply, SimRequest}; - - #[derive(Error, Debug)] - pub enum ReceptionError { - #[error("IO error: {0}")] - Io(#[from] std::io::Error), - #[error("Serde JSON error: {0}")] - SerdeJson(#[from] serde_json::Error), - } - - pub struct SimUdpClient { - socket: UdpSocket, - pub reply_buf: [u8; 4096], - } - - impl SimUdpClient { - pub fn new( - server_addr: &SocketAddr, - non_blocking: bool, - read_timeot_ms: Option, - ) -> std::io::Result { - let socket = UdpSocket::bind("127.0.0.1:0")?; - socket.set_nonblocking(non_blocking)?; - socket - .connect(server_addr) - .expect("could not connect to server addr"); - if let Some(read_timeout) = read_timeot_ms { - // Set a read timeout so the test does not hang on failures. - socket.set_read_timeout(Some(Duration::from_millis(read_timeout)))?; - } - Ok(Self { - socket, - reply_buf: [0; 4096], - }) - } - - pub fn set_nonblocking(&self, non_blocking: bool) -> std::io::Result<()> { - self.socket.set_nonblocking(non_blocking) - } - - pub fn set_read_timeout(&self, read_timeout_ms: u64) -> std::io::Result<()> { - self.socket - .set_read_timeout(Some(Duration::from_millis(read_timeout_ms))) - } - - pub fn send_request(&self, sim_request: &SimRequest) -> std::io::Result { - self.socket.send( - &serde_json::to_vec(sim_request).expect("conversion of request to vector failed"), - ) - } - - pub fn recv_raw(&mut self) -> std::io::Result { - self.socket.recv(&mut self.reply_buf) - } - - pub fn recv_sim_reply(&mut self) -> Result { - let read_len = self.recv_raw()?; - Ok(serde_json::from_slice(&self.reply_buf[0..read_len])?) - } - } + pub const SIM_CTRL_PORT: u16 = 7303; } #[cfg(test)] @@ -363,7 +436,7 @@ pub mod tests { } impl SerializableSimMsgPayload for DummyRequest { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -372,13 +445,13 @@ pub mod tests { } impl SerializableSimMsgPayload for DummyReply { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } #[test] fn test_basic_request() { let sim_request = SimRequest::new_with_epoch_time(DummyRequest::Ping); - assert_eq!(sim_request.target(), SimTarget::SimCtrl); + assert_eq!(sim_request.component(), SimComponent::SimCtrl); assert_eq!(sim_request.msg_type(), SimMessageType::Request); let dummy_request = DummyRequest::from_sim_message(&sim_request).expect("deserialization failed"); @@ -387,8 +460,8 @@ pub mod tests { #[test] fn test_basic_reply() { - let sim_reply = SimReply::new(DummyReply::Pong); - assert_eq!(sim_reply.target(), SimTarget::SimCtrl); + let sim_reply = SimReply::new(&DummyReply::Pong); + assert_eq!(sim_reply.component(), SimComponent::SimCtrl); assert_eq!(sim_reply.msg_type(), SimMessageType::Reply); let dummy_request = DummyReply::from_sim_message(&sim_reply).expect("deserialization failed"); diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index 59701f7..4b6c240 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -3,7 +3,8 @@ use asynchronix::simulation::{Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, SystemClock}; use controller::SimController; use eps::PcduModel; -use satrs_minisim::{SimReply, SimRequest, SIM_CTRL_UDP_PORT}; +use satrs_minisim::udp::SIM_CTRL_PORT; +use satrs_minisim::{SimReply, SimRequest}; use std::sync::mpsc; use std::thread; use std::time::{Duration, SystemTime}; @@ -30,7 +31,8 @@ fn create_sim_controller( request_receiver: mpsc::Receiver, ) -> SimController { // Instantiate models and their mailboxes. - let mgm_model = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); + let mgm_model = + MagnetometerModel::new_for_lis3mdl(Duration::from_millis(50), reply_sender.clone()); let mgm_mailbox = Mailbox::new(); let mgm_addr = mgm_mailbox.address(); @@ -112,9 +114,9 @@ fn main() { }); let mut udp_server = - SimUdpServer::new(SIM_CTRL_UDP_PORT, request_sender, reply_receiver, 200, None) + SimUdpServer::new(SIM_CTRL_PORT, request_sender, reply_receiver, 200, None) .expect("could not create UDP request server"); - log::info!("starting UDP server on port {}", SIM_CTRL_UDP_PORT); + log::info!("starting UDP server on port {}", SIM_CTRL_PORT); // This thread manages the simulator UDP server. let udp_tc_thread = thread::spawn(move || { udp_server.run(); diff --git a/satrs-minisim/src/udp.rs b/satrs-minisim/src/udp.rs index ad50672..e177547 100644 --- a/satrs-minisim/src/udp.rs +++ b/satrs-minisim/src/udp.rs @@ -150,6 +150,7 @@ impl SimUdpServer { mod tests { use std::{ io::ErrorKind, + net::{SocketAddr, UdpSocket}, sync::{ atomic::{AtomicBool, Ordering}, mpsc, Arc, @@ -159,7 +160,6 @@ mod tests { use satrs_minisim::{ eps::{PcduReply, PcduRequest}, - udp::{ReceptionError, SimUdpClient}, SimCtrlReply, SimCtrlRequest, SimReply, SimRequest, }; @@ -171,8 +171,57 @@ mod tests { // Wait time to ensure even possibly laggy systems like CI servers can run the tests. const SERVER_WAIT_TIME_MS: u64 = 50; + #[derive(thiserror::Error, Debug)] + pub enum ReceptionError { + #[error("IO error: {0}")] + Io(#[from] std::io::Error), + #[error("Serde JSON error: {0}")] + SerdeJson(#[from] serde_json::Error), + } + + pub struct SimUdpTestClient { + socket: UdpSocket, + pub reply_buf: [u8; 4096], + } + + impl SimUdpTestClient { + pub fn new( + server_addr: &SocketAddr, + non_blocking: bool, + read_timeot_ms: Option, + ) -> std::io::Result { + let socket = UdpSocket::bind("127.0.0.1:0")?; + socket.set_nonblocking(non_blocking)?; + socket + .connect(server_addr) + .expect("could not connect to server addr"); + if let Some(read_timeout) = read_timeot_ms { + // Set a read timeout so the test does not hang on failures. + socket.set_read_timeout(Some(Duration::from_millis(read_timeout)))?; + } + Ok(Self { + socket, + reply_buf: [0; 4096], + }) + } + + pub fn send_request(&self, sim_request: &SimRequest) -> std::io::Result { + self.socket.send( + &serde_json::to_vec(sim_request).expect("conversion of request to vector failed"), + ) + } + + pub fn recv_raw(&mut self) -> std::io::Result { + self.socket.recv(&mut self.reply_buf) + } + + pub fn recv_sim_reply(&mut self) -> Result { + let read_len = self.recv_raw()?; + Ok(serde_json::from_slice(&self.reply_buf[0..read_len])?) + } + } struct UdpTestbench { - client: SimUdpClient, + client: SimUdpTestClient, stop_signal: Arc, request_receiver: mpsc::Receiver, reply_sender: mpsc::Sender, @@ -197,7 +246,7 @@ mod tests { let server_addr = server.server_addr()?; Ok(( Self { - client: SimUdpClient::new( + client: SimUdpTestClient::new( &server_addr, client_non_blocking, client_read_timeout_ms, @@ -295,7 +344,7 @@ mod tests { .send_request(&SimRequest::new_with_epoch_time(SimCtrlRequest::Ping)) .expect("sending request failed"); - let sim_reply = SimReply::new(PcduReply::SwitchInfo(get_all_off_switch_map())); + let sim_reply = SimReply::new(&PcduReply::SwitchInfo(get_all_off_switch_map())); udp_testbench.send_reply(&sim_reply); udp_testbench.check_next_sim_reply(&sim_reply); @@ -320,7 +369,7 @@ mod tests { .expect("sending request failed"); // Send a reply to the server, ensure it gets forwarded to the client. - let sim_reply = SimReply::new(PcduReply::SwitchInfo(get_all_off_switch_map())); + let sim_reply = SimReply::new(&PcduReply::SwitchInfo(get_all_off_switch_map())); udp_testbench.send_reply(&sim_reply); std::thread::sleep(Duration::from_millis(SERVER_WAIT_TIME_MS)); @@ -339,7 +388,7 @@ mod tests { let server_thread = std::thread::spawn(move || udp_server.run()); // Send a reply to the server. The client is not connected, so it won't get forwarded. - let sim_reply = SimReply::new(PcduReply::SwitchInfo(get_all_off_switch_map())); + let sim_reply = SimReply::new(&PcduReply::SwitchInfo(get_all_off_switch_map())); udp_testbench.send_reply(&sim_reply); std::thread::sleep(Duration::from_millis(10)); @@ -366,7 +415,7 @@ mod tests { let server_thread = std::thread::spawn(move || udp_server.run()); // The server only caches up to 3 replies. - let sim_reply = SimReply::new(SimCtrlReply::Pong); + let sim_reply = SimReply::new(&SimCtrlReply::Pong); for _ in 0..4 { udp_testbench.send_reply(&sim_reply); } diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 1e1fda1..cb2648a 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -1,22 +1,17 @@ +use core::time::Duration; + +use derive_new::new; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +#[cfg(feature = "std")] +#[allow(unused_imports)] +pub use std_mod::*; -/// Generic trait for a device capable of switching itself on or off. -pub trait PowerSwitch { - type Error; - - fn switch_on(&mut self) -> Result<(), Self::Error>; - fn switch_off(&mut self) -> Result<(), Self::Error>; - - fn is_switch_on(&self) -> bool { - self.switch_state() == SwitchState::On - } - - fn switch_state(&self) -> SwitchState; -} +use crate::request::MessageMetadata; #[derive(Debug, Eq, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum SwitchState { Off = 0, On = 1, @@ -26,6 +21,7 @@ pub enum SwitchState { #[derive(Debug, Eq, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum SwitchStateBinary { Off = 0, On = 1, @@ -63,76 +59,254 @@ impl From for SwitchState { pub type SwitchId = u16; /// Generic trait for a device capable of turning on and off switches. -pub trait PowerSwitcherCommandSender { - type Error; +pub trait PowerSwitcherCommandSender> { + type Error: core::fmt::Debug; - fn send_switch_on_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error>; - fn send_switch_off_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error>; + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchType, + ) -> Result<(), Self::Error>; + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchType, + ) -> Result<(), Self::Error>; } -pub trait PowerSwitchInfo { - type Error; +pub trait PowerSwitchInfo { + type Error: core::fmt::Debug; /// Retrieve the switch state - fn get_switch_state(&mut self, switch_id: SwitchId) -> Result; + fn switch_state(&self, switch_id: SwitchType) -> Result; - fn get_is_switch_on(&mut self, switch_id: SwitchId) -> Result { - Ok(self.get_switch_state(switch_id)? == SwitchState::On) + fn is_switch_on(&self, switch_id: SwitchType) -> Result { + Ok(self.switch_state(switch_id)? == SwitchState::On) } /// The maximum delay it will take to change a switch. /// /// This may take into account the time to send a command, wait for it to be executed, and /// see the switch changed. - fn switch_delay_ms(&self) -> u32; + fn switch_delay_ms(&self) -> Duration; +} + +#[derive(new)] +pub struct SwitchRequest { + switch_id: SwitchId, + target_state: SwitchStateBinary, +} + +impl SwitchRequest { + pub fn switch_id(&self) -> SwitchId { + self.switch_id + } + + pub fn target_state(&self) -> SwitchStateBinary { + self.target_state + } +} + +#[cfg(feature = "std")] +pub mod std_mod { + use std::sync::mpsc; + + use crate::{ + queue::GenericSendError, + request::{GenericMessage, MessageMetadata}, + }; + + use super::*; + + pub type MpscSwitchCmdSender = mpsc::Sender>; + pub type MpscSwitchCmdSenderBounded = mpsc::SyncSender>; + + impl> PowerSwitcherCommandSender for MpscSwitchCmdSender { + type Error = GenericSendError; + + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchType, + ) -> Result<(), Self::Error> { + self.send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id.into(), SwitchStateBinary::On), + )) + .map_err(|_| GenericSendError::RxDisconnected) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchType, + ) -> Result<(), Self::Error> { + self.send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id.into(), SwitchStateBinary::Off), + )) + .map_err(|_| GenericSendError::RxDisconnected) + } + } + + impl> PowerSwitcherCommandSender for MpscSwitchCmdSenderBounded { + type Error = GenericSendError; + + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchType, + ) -> Result<(), Self::Error> { + self.try_send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id.into(), SwitchStateBinary::On), + )) + .map_err(|e| match e { + mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), + mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected, + }) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchType, + ) -> Result<(), Self::Error> { + self.try_send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id.into(), SwitchStateBinary::Off), + )) + .map_err(|e| match e { + mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), + mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected, + }) + } + } } #[cfg(test)] mod tests { - #![allow(dead_code)] + // TODO: Add unittests for PowerSwitcherCommandSender impls for mpsc. + + use std::sync::mpsc::{self, TryRecvError}; + + use crate::{queue::GenericSendError, request::GenericMessage, ComponentId}; + use super::*; - use std::boxed::Box; - struct Pcdu { - switch_rx: std::sync::mpsc::Receiver<(SwitchId, u16)>, + const TEST_REQ_ID: u32 = 2; + const TEST_SENDER_ID: ComponentId = 5; + + const TEST_SWITCH_ID: u16 = 0x1ff; + + fn common_checks(request: &GenericMessage) { + assert_eq!(request.requestor_info.sender_id(), TEST_SENDER_ID); + assert_eq!(request.requestor_info.request_id(), TEST_REQ_ID); + assert_eq!(request.message.switch_id(), TEST_SWITCH_ID); } - #[derive(Eq, PartialEq)] - enum DeviceState { - OFF, - SwitchingPower, - ON, - SETUP, - IDLE, - } - struct MyComplexDevice { - power_switcher: Box>, - power_info: Box>, - switch_id: SwitchId, - some_state: u16, - dev_state: DeviceState, - mode: u32, - submode: u16, + #[test] + fn test_comand_switch_sending_mpsc_regular_on_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + switch_cmd_tx + .send_switch_on_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::On); } - impl MyComplexDevice { - pub fn periodic_op(&mut self) { - // .. mode command coming in - let mode = 1; - if mode == 1 { - if self.dev_state == DeviceState::OFF { - self.power_switcher - .send_switch_on_cmd(self.switch_id) - .expect("sending siwthc cmd failed"); - self.dev_state = DeviceState::SwitchingPower; - } - if self.dev_state == DeviceState::SwitchingPower { - if self.power_info.get_is_switch_on(0).unwrap() { - self.dev_state = DeviceState::ON; - self.mode = 1; - } - } - } - } + #[test] + fn test_comand_switch_sending_mpsc_regular_off_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + switch_cmd_tx + .send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::Off); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_rx_disconnected() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + drop(switch_cmd_rx); + let result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::RxDisconnected); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_on_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(3); + switch_cmd_tx + .send_switch_on_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::On); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_off_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(3); + switch_cmd_tx + .send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::Off); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_rx_disconnected() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(1); + drop(switch_cmd_rx); + let result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::RxDisconnected); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_queue_full() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(1); + let mut result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_ok()); + result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::QueueFull(None)); + matches!(switch_cmd_rx.try_recv(), Err(TryRecvError::Empty)); } }