diff --git a/docs.sh b/docs.sh new file mode 100755 index 0000000..37563d2 --- /dev/null +++ b/docs.sh @@ -0,0 +1,3 @@ +#!/bin/sh +export RUSTDOCFLAGS="--cfg docsrs --generate-link-to-definition -Z unstable-options" +cargo +nightly doc --all-features --open diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index e3ab71f..0665e23 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -1,7 +1,7 @@ use derive_new::new; use satrs::hk::{HkRequest, HkRequestVariant}; +use satrs::mode_tree::{ModeChild, ModeNode}; use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender}; -use satrs::queue::{GenericSendError, GenericTargetedMessagingError}; use satrs_example::{DeviceMode, TimestampHelper}; use satrs_minisim::acs::lis3mdl::{ MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, @@ -16,6 +16,7 @@ use std::time::Duration; use satrs::mode::{ ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler, + ModeRequestHandlerMpscBounded, }; use satrs::pus::{EcssTmSender, PusTmVariant}; use satrs::request::{GenericMessage, MessageMetadata, UniqueApidTargetId}; @@ -171,7 +172,7 @@ pub struct MgmHandlerLis3Mdl< > { id: UniqueApidTargetId, dev_str: &'static str, - mode_interface: MpscModeLeafInterface, + mode_node: ModeRequestHandlerMpscBounded, composite_request_rx: mpsc::Receiver>, hk_reply_tx: mpsc::Sender>, switch_helper: SwitchHelper, @@ -275,25 +276,26 @@ impl< 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() - ); + match self.mode_node.try_recv_mode_request() { + Ok(opt_msg) => { + if let Some(msg) = opt_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 { + Err(e) => match e { + satrs::queue::GenericReceiveError::Empty => (), + satrs::queue::GenericReceiveError::TxDisconnected(e) => { log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); - } else { - break; } - } + }, } } } @@ -386,6 +388,7 @@ impl< &mut self, requestor: MessageMetadata, mode_and_submode: ModeAndSubmode, + _forced: bool, ) -> Result<(), satrs::mode::ModeError> { log::info!( "{}: transitioning to mode {:?}", @@ -448,10 +451,9 @@ impl< requestor.sender_id() ); } - self.mode_interface - .reply_to_pus_tx - .send(GenericMessage::new(requestor, reply)) - .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; + self.mode_node + .send_mode_reply(requestor, reply) + .map_err(ModeError::Send)?; Ok(()) } @@ -464,17 +466,46 @@ impl< } } +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > ModeNode for MgmHandlerLis3Mdl +{ + fn id(&self) -> satrs::ComponentId { + self.id.into() + } +} + +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > ModeChild for MgmHandlerLis3Mdl +{ + type Sender = mpsc::SyncSender>; + + fn add_mode_parent(&mut self, id: satrs::ComponentId, reply_sender: Self::Sender) { + self.mode_node.add_message_target(id, reply_sender); + } +} + #[cfg(test)] mod tests { - use std::sync::{mpsc, Arc}; + use std::{ + collections::HashMap, + sync::{mpsc, Arc}, + }; use satrs::{ mode::{ModeReply, ModeRequest}, + mode_tree::ModeParent, power::SwitchStateBinary, request::{GenericMessage, UniqueApidTargetId}, tmtc::PacketAsVec, + ComponentId, }; - use satrs_example::config::components::Apid; + use satrs_example::config::components::{Apid, MGM_ASSEMBLY}; use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest}; @@ -503,7 +534,7 @@ mod tests { } pub struct MgmTestbench { - pub mode_request_tx: mpsc::Sender>, + pub mode_request_tx: mpsc::SyncSender>, pub mode_reply_rx_to_pus: mpsc::Receiver>, pub mode_reply_rx_to_parent: mpsc::Receiver>, pub composite_request_tx: mpsc::Sender>, @@ -513,38 +544,76 @@ mod tests { MgmHandlerLis3Mdl, TestSwitchHelper>, } + #[derive(Default)] + pub struct MgmAssemblyMock( + pub HashMap>>, + ); + + impl ModeNode for MgmAssemblyMock { + fn id(&self) -> satrs::ComponentId { + PUS_MODE_SERVICE.into() + } + } + + impl ModeParent for MgmAssemblyMock { + type Sender = mpsc::SyncSender>; + + fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) { + self.0.insert(id, request_sender); + } + } + + #[derive(Default)] + pub struct PusMock { + pub request_sender_map: HashMap>>, + } + + impl ModeNode for PusMock { + fn id(&self) -> satrs::ComponentId { + PUS_MODE_SERVICE.into() + } + } + + impl ModeParent for PusMock { + type Sender = mpsc::SyncSender>; + + fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) { + self.request_sender_map.insert(id, request_sender); + } + } + 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 (request_tx, request_rx) = mpsc::sync_channel(5); + let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::sync_channel(5); 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 id = UniqueApidTargetId::new(Apid::Acs as u16, 1); + let mode_node = ModeRequestHandlerMpscBounded::new(id.into(), request_rx); 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(); + let mut handler = MgmHandlerLis3Mdl::new( + id, + "TEST_MGM", + mode_node, + composite_request_rx, + hk_reply_tx, + TestSwitchHelper::default(), + tm_tx, + TestSpiInterface::default(), + shared_mgm_set, + ); + handler.add_mode_parent(PUS_MODE_SERVICE.into(), reply_tx_to_pus); + handler.add_mode_parent(MGM_ASSEMBLY.into(), reply_tx_to_parent); 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, + handler, 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, - ), } } } @@ -575,7 +644,10 @@ mod tests { .mode_request_tx .send(GenericMessage::new( MessageMetadata::new(0, PUS_MODE_SERVICE.id()), - ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + ModeRequest::SetMode { + mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0), + forced: false, + }, )) .expect("failed to send mode request"); testbench.handler.periodic_operation(); @@ -633,7 +705,10 @@ mod tests { .mode_request_tx .send(GenericMessage::new( MessageMetadata::new(0, PUS_MODE_SERVICE.id()), - ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + ModeRequest::SetMode { + mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0), + forced: false, + }, )) .expect("failed to send mode request"); testbench.handler.periodic_operation(); diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs index 5608bf3..ff1e2c8 100644 --- a/satrs-example/src/config.rs +++ b/satrs-example/src/config.rs @@ -149,6 +149,7 @@ pub mod components { #[derive(Copy, Clone, PartialEq, Eq)] pub enum AcsId { Mgm0 = 0, + Assembly = 1, } #[derive(Copy, Clone, PartialEq, Eq)] @@ -176,6 +177,8 @@ pub mod components { UniqueApidTargetId::new(Apid::GenericPus as u16, PusId::PusHk as u32); pub const PUS_SCHED_SERVICE: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Sched as u16, 0); + pub const MGM_ASSEMBLY: UniqueApidTargetId = + UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Assembly as u32); pub const MGM_HANDLER_0: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Mgm0 as u32); pub const PCDU_HANDLER: UniqueApidTargetId = diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 908bfb2..efcb55b 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -11,7 +11,7 @@ use satrs::{ mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, power::SwitchRequest, pus::{EcssTmSender, PusTmVariant}, - queue::{GenericSendError, GenericTargetedMessagingError}, + queue::GenericSendError, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, spacepackets::ByteConversionError, }; @@ -412,6 +412,7 @@ impl ModeRequestHandler &mut self, requestor: MessageMetadata, mode_and_submode: ModeAndSubmode, + _forced: bool, ) -> Result<(), satrs::mode::ModeError> { log::info!( "{}: transitioning to mode {:?}", @@ -469,7 +470,7 @@ impl ModeRequestHandler self.mode_interface .reply_to_pus_tx .send(GenericMessage::new(requestor, reply)) - .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; + .map_err(|_| GenericSendError::RxDisconnected)?; Ok(()) } @@ -660,7 +661,10 @@ mod tests { .mode_request_tx .send(GenericMessage::new( MessageMetadata::new(0, PUS_MODE_SERVICE.id()), - ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + ModeRequest::SetMode { + mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0), + forced: false, + }, )) .expect("failed to send mode request"); let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap(); @@ -692,7 +696,10 @@ mod tests { .mode_request_tx .send(GenericMessage::new( MessageMetadata::new(0, PUS_MODE_SERVICE.id()), - ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + ModeRequest::SetMode { + mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0), + forced: false, + }, )) .expect("failed to send mode request"); testbench diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 317e3f0..5d7fcbf 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -21,6 +21,7 @@ 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::mode_tree::connect_mode_nodes; use satrs::pus::HandlingStatus; use satrs::request::{GenericMessage, MessageMetadata}; use satrs::tmtc::{PacketSenderWithSharedPool, SharedPacketPool}; @@ -47,7 +48,7 @@ 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::{Mode, ModeAndSubmode, ModeRequest}; +use satrs::mode::{Mode, ModeAndSubmode, ModeRequest, ModeRequestHandlerMpscBounded}; use satrs::pus::event_man::EventRequestWithToken; use satrs::spacepackets::{time::cds::CdsTime, time::TimeWriter}; use satrs_example::config::components::{ @@ -93,9 +94,6 @@ fn static_tmtc_pool_main() { request_map .composite_router_map .insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx); - 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); @@ -232,12 +230,8 @@ fn static_tmtc_pool_main() { let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone()); let shared_mgm_set = Arc::default(); - let mgm_mode_leaf_interface = MpscModeLeafInterface { - request_rx: mgm_handler_mode_rx, - reply_to_pus_tx: pus_mode_reply_tx.clone(), - reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, - }; - + let mgm_mode_node = + ModeRequestHandlerMpscBounded::new(MGM_HANDLER_0.into(), mgm_handler_mode_rx); 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 { @@ -250,7 +244,7 @@ fn static_tmtc_pool_main() { let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", - mgm_mode_leaf_interface, + mgm_mode_node, mgm_handler_composite_rx, pus_hk_reply_tx.clone(), switch_helper.clone(), @@ -258,6 +252,13 @@ fn static_tmtc_pool_main() { mgm_spi_interface, shared_mgm_set, ); + // Connect PUS mode service to device handler. + connect_mode_nodes( + &mut pus_stack.mode_srv, + mgm_handler_mode_tx, + &mut mgm_handler, + mgm_handler_mode_reply_to_parent_tx, + ); let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = mpsc::sync_channel(10); @@ -291,7 +292,10 @@ fn static_tmtc_pool_main() { pcdu_handler_mode_tx .send(GenericMessage::new( MessageMetadata::new(0, NO_SENDER), - ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)), + ModeRequest::SetMode { + mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0), + forced: false, + }, )) .expect("sending initial mode request failed"); @@ -423,9 +427,9 @@ fn dyn_tmtc_pool_main() { request_map .composite_router_map .insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx); - request_map - .mode_router_map - .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx); + //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); @@ -540,11 +544,8 @@ fn dyn_tmtc_pool_main() { let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = mpsc::sync_channel(5); let shared_mgm_set = Arc::default(); - let mode_leaf_interface = MpscModeLeafInterface { - request_rx: mgm_handler_mode_rx, - reply_to_pus_tx: pus_mode_reply_tx.clone(), - reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, - }; + let mgm_mode_node = + ModeRequestHandlerMpscBounded::new(MGM_HANDLER_0.into(), mgm_handler_mode_rx); 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); @@ -558,7 +559,7 @@ fn dyn_tmtc_pool_main() { let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", - mode_leaf_interface, + mgm_mode_node, mgm_handler_composite_rx, pus_hk_reply_tx.clone(), switch_helper.clone(), @@ -566,6 +567,12 @@ fn dyn_tmtc_pool_main() { mgm_spi_interface, shared_mgm_set, ); + connect_mode_nodes( + &mut pus_stack.mode_srv, + mgm_handler_mode_tx, + &mut mgm_handler, + mgm_handler_mode_reply_to_parent_tx, + ); let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = mpsc::sync_channel(10); @@ -598,7 +605,10 @@ fn dyn_tmtc_pool_main() { pcdu_handler_mode_tx .send(GenericMessage::new( MessageMetadata::new(0, NO_SENDER), - ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)), + ModeRequest::SetMode { + mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0), + forced: false, + }, )) .expect("sending initial mode request failed"); diff --git a/satrs-example/src/pus/mode.rs b/satrs-example/src/pus/mode.rs index 56ae9d4..181680c 100644 --- a/satrs-example/src/pus/mode.rs +++ b/satrs-example/src/pus/mode.rs @@ -1,4 +1,5 @@ use derive_new::new; +use satrs::mode_tree::{ModeNode, ModeParent}; use satrs::tmtc::{PacketAsVec, PacketSenderWithSharedPool}; use std::sync::mpsc; use std::time::Duration; @@ -110,6 +111,7 @@ impl PusReplyHandler for ModeReplyHandler { ), )?; } + ModeReply::ModeInfo(_mode_and_submode) => (), }; Ok(true) } @@ -190,7 +192,13 @@ impl PusTcToRequestConverter for ModeRequestCo } let mode_and_submode = ModeAndSubmode::from_be_bytes(&tc.user_data()[4..]) .expect("mode and submode extraction failed"); - Ok((active_request, ModeRequest::SetMode(mode_and_submode))) + Ok(( + active_request, + ModeRequest::SetMode { + mode_and_submode, + forced: false, + }, + )) } Subservice::TcReadMode => Ok((active_request, ModeRequest::ReadMode)), Subservice::TcAnnounceMode => Ok((active_request, ModeRequest::AnnounceMode)), @@ -268,6 +276,27 @@ pub struct ModeServiceWrapper, } +impl ModeNode + for ModeServiceWrapper +{ + fn id(&self) -> ComponentId { + self.service.service_helper.id() + } +} + +impl ModeParent + for ModeServiceWrapper +{ + type Sender = mpsc::SyncSender>; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) { + self.service + .request_router + .mode_router_map + .insert(id, request_sender); + } +} + impl TargetedPusService for ModeServiceWrapper { @@ -346,7 +375,13 @@ mod tests { let (_active_req, req) = testbench .convert(token, &[], TEST_APID, TEST_UNIQUE_ID_0) .expect("conversion has failed"); - assert_eq!(req, ModeRequest::SetMode(mode_and_submode)); + assert_eq!( + req, + ModeRequest::SetMode { + mode_and_submode, + forced: false + } + ); } #[test] diff --git a/satrs-example/src/pus/stack.rs b/satrs-example/src/pus/stack.rs index 3594bd6..67c3b57 100644 --- a/satrs-example/src/pus/stack.rs +++ b/satrs-example/src/pus/stack.rs @@ -15,12 +15,12 @@ use super::{ // for targeted services.. #[derive(new)] pub struct PusStack { - test_srv: TestCustomServiceWrapper, - hk_srv_wrapper: HkServiceWrapper, - event_srv: EventServiceWrapper, - action_srv_wrapper: ActionServiceWrapper, - schedule_srv: SchedulingServiceWrapper, - mode_srv: ModeServiceWrapper, + pub test_srv: TestCustomServiceWrapper, + pub hk_srv_wrapper: HkServiceWrapper, + pub event_srv: EventServiceWrapper, + pub action_srv_wrapper: ActionServiceWrapper, + pub schedule_srv: SchedulingServiceWrapper, + pub mode_srv: ModeServiceWrapper, } impl diff --git a/satrs/CHANGELOG.md b/satrs/CHANGELOG.md index bb92cb7..b50049f 100644 --- a/satrs/CHANGELOG.md +++ b/satrs/CHANGELOG.md @@ -20,6 +20,17 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - `StaticHeaplessMemoryPool` which can be grown with user-provided static buffers. - Scheduling table for systems with a standard runtime +- Mode Tree Feature which allows building a network of mode components which can send mode + messages to each other. +- Added first helper features like the `SubsystemExecutionHelper` and the + `SubsystemCommandingHelper` which allows to build subsystem components. Subsystem components + are able to execute mode sequences and perform target keeping based on a declarative table + format. +- Added `DevManagerCommandingHelper` which performs some of the boilerplate logik required + by Assembly and Device Management components. This includes forwarding mode requests and + handling mode replies. +- First basic health module with `HealthState`s and the `HealthTableProvider` trait. These + components are important for any FDIR components which get added in the future. # [v0.2.1] 2024-05-19 diff --git a/satrs/Cargo.toml b/satrs/Cargo.toml index c333c71..88ee316 100644 --- a/satrs/Cargo.toml +++ b/satrs/Cargo.toml @@ -2,7 +2,7 @@ name = "satrs" version = "0.2.1" edition = "2021" -rust-version = "1.71.1" +rust-version = "1.82.0" authors = ["Robin Mueller "] description = "A framework to build software for remote systems" homepage = "https://absatsw.irs.uni-stuttgart.de/projects/sat-rs/" diff --git a/satrs/src/dev_mgmt.rs b/satrs/src/dev_mgmt.rs new file mode 100644 index 0000000..b9eca0a --- /dev/null +++ b/satrs/src/dev_mgmt.rs @@ -0,0 +1,448 @@ +use crate::{ + mode::{ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender}, + mode_tree::{ModeStoreProvider, ModeStoreVec}, + queue::{GenericSendError, GenericTargetedMessagingError}, + request::{GenericMessage, RequestId}, + ComponentId, +}; +use core::fmt::Debug; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct ActiveModeCommandContext { + pub target_mode: ModeAndSubmode, + pub active_request_id: RequestId, +} + +#[derive(Debug, Default, PartialEq, Eq)] +pub enum DevManagerHelperResult { + #[default] + Idle, + Busy, + ModeCommandingDone(ActiveModeCommandContext), +} + +#[derive(Debug)] +pub enum DevManagerHelperError { + ChildNotInStore, +} + +pub trait DevManagerUserHook: Debug { + fn send_mode_cmd_to_child( + &self, + request_id: RequestId, + target_id: ComponentId, + mode: ModeAndSubmode, + forced: bool, + children_mode_store: &mut ModeStoreVec, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericSendError>; + + fn send_mode_cmds_to_children( + &self, + request_id: RequestId, + commanded_parent_mode: ModeAndSubmode, + forced: bool, + children_mode_store: &mut ModeStoreVec, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericSendError>; +} + +#[derive(Debug, Default)] +pub struct TransparentDevManagerHook {} + +impl DevManagerUserHook for TransparentDevManagerHook { + fn send_mode_cmds_to_children( + &self, + request_id: RequestId, + commanded_parent_mode: ModeAndSubmode, + forced: bool, + children_mode_store: &mut ModeStoreVec, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericSendError> { + for child in children_mode_store { + mode_req_sender.send_mode_request( + request_id, + child.id(), + ModeRequest::SetMode { + mode_and_submode: commanded_parent_mode, + forced, + }, + )?; + child.awaiting_reply = true; + } + Ok(()) + } + + fn send_mode_cmd_to_child( + &self, + request_id: RequestId, + target_id: ComponentId, + mode: ModeAndSubmode, + forced: bool, + children_mode_store: &mut ModeStoreVec, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericSendError> { + let mut_val = children_mode_store + .get_mut(target_id) + .ok_or(GenericSendError::TargetDoesNotExist(target_id))?; + mut_val.awaiting_reply = true; + mode_req_sender.send_mode_request( + request_id, + target_id, + ModeRequest::SetMode { + mode_and_submode: mode, + forced, + }, + )?; + Ok(()) + } +} + +#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] +pub enum DevManagerCommandingState { + #[default] + Idle, + AwaitingReplies(ActiveModeCommandContext), +} + +impl DevManagerCommandingState { + fn new_active_cmd(mode_and_submode: ModeAndSubmode, active_request_id: RequestId) -> Self { + DevManagerCommandingState::AwaitingReplies(ActiveModeCommandContext { + target_mode: mode_and_submode, + active_request_id, + }) + } +} + +/// A generic helper for manager components which manage child components in a mode tree. +/// +/// Mode commands are usually forwarded to all children components transparently. +/// For example, this could be used in an Assembly component which manages multiple redundant +/// child components. It can also be used inside a manager component which only manages one device. +#[derive(Debug, Default)] +pub struct DevManagerCommandingHelper { + /// The IDs, modes and reply awaition status of all children are tracked in this data + /// structure. + pub children_mode_store: ModeStoreVec, + pub user_hook: UserHook, + pub state: DevManagerCommandingState, +} + +impl DevManagerCommandingHelper { + pub fn new(user_hook: UserHook) -> Self { + Self { + children_mode_store: Default::default(), + user_hook, + state: Default::default(), + } + } + + pub fn send_mode_cmd_to_one_child( + &mut self, + request_id: RequestId, + target_id: ComponentId, + mode_and_submode: ModeAndSubmode, + forced: bool, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericSendError> { + self.state = DevManagerCommandingState::new_active_cmd(mode_and_submode, request_id); + self.user_hook.send_mode_cmd_to_child( + request_id, + target_id, + mode_and_submode, + forced, + &mut self.children_mode_store, + mode_req_sender, + )?; + Ok(()) + } + + pub fn send_mode_cmd_to_all_children( + &mut self, + request_id: RequestId, + mode_and_submode: ModeAndSubmode, + forced: bool, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericSendError> { + self.state = DevManagerCommandingState::new_active_cmd(mode_and_submode, request_id); + self.user_hook.send_mode_cmds_to_children( + request_id, + mode_and_submode, + forced, + &mut self.children_mode_store, + mode_req_sender, + )?; + Ok(()) + } + + pub fn target_mode(&self) -> Option { + match self.state { + DevManagerCommandingState::Idle => None, + DevManagerCommandingState::AwaitingReplies(context) => Some(context.target_mode), + } + } + + pub fn state(&self) -> DevManagerCommandingState { + self.state + } + + pub fn send_announce_mode_cmd_to_children( + &self, + request_id: RequestId, + mode_req_sender: &impl ModeRequestSender, + recursive: bool, + ) -> Result<(), GenericTargetedMessagingError> { + let mut request = ModeRequest::AnnounceMode; + if recursive { + request = ModeRequest::AnnounceModeRecursive; + } + for child in self.children_mode_store.0.iter() { + mode_req_sender.send_mode_request(request_id, child.id(), request)?; + } + Ok(()) + } + + pub fn add_mode_child(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { + self.children_mode_store.add_component(target_id, mode); + } + + /// Helper method which counts the number of children which have a certain mode. + pub fn count_number_of_children_with_mode(&self, mode_and_submode: ModeAndSubmode) -> usize { + let mut children_in_target_mode = 0; + for child in &self.children_mode_store { + if child.mode_and_submode() == mode_and_submode { + children_in_target_mode += 1; + } + } + children_in_target_mode + } + + pub fn handle_mode_reply( + &mut self, + mode_reply: &GenericMessage, + ) -> Result { + let context = match self.state { + DevManagerCommandingState::Idle => return Ok(DevManagerHelperResult::Idle), + DevManagerCommandingState::AwaitingReplies(active_mode_command_context) => { + Some(active_mode_command_context) + } + }; + if !self + .children_mode_store + .has_component(mode_reply.sender_id()) + { + return Err(DevManagerHelperError::ChildNotInStore); + } + let mut generic_mode_reply_handler = |mode_and_submode: Option| { + // Tying the reply awaition to the request ID ensures that something like replies + // belonging to older requests do not interfere with the completion handling of + // the mode commanding. This is important for forced mode commands. + let mut handle_awaition = false; + if let DevManagerCommandingState::AwaitingReplies { .. } = self.state { + handle_awaition = true; + } + let still_awating_replies = self.children_mode_store.mode_reply_handler( + mode_reply.sender_id(), + mode_and_submode, + handle_awaition, + ); + // It is okay to unwrap: If awaition should be handled, the returned value should + // always be some valid value. + if handle_awaition && !still_awating_replies.unwrap() { + self.state = DevManagerCommandingState::Idle; + return Ok(DevManagerHelperResult::ModeCommandingDone(context.unwrap())); + } + Ok(DevManagerHelperResult::Busy) + }; + match mode_reply.message { + ModeReply::ModeInfo(mode_and_submode) | ModeReply::ModeReply(mode_and_submode) => { + generic_mode_reply_handler(Some(mode_and_submode)) + } + ModeReply::CantReachMode(_result_u16) => generic_mode_reply_handler(None), + ModeReply::WrongMode { + expected: _, + reached, + } => generic_mode_reply_handler(Some(reached)), + } + } +} + +#[cfg(test)] +mod tests { + use crate::{ + mode::{tests::ModeReqSenderMock, UNKNOWN_MODE}, + request::MessageMetadata, + }; + + use super::*; + + pub enum ExampleId { + Id1 = 1, + Id2 = 2, + } + + pub enum ExampleMode { + Mode1 = 1, + Mode2 = 2, + } + + #[test] + fn test_basic() { + let assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); + assert_eq!(assy_helper.state(), DevManagerCommandingState::Idle); + } + + #[test] + fn test_mode_announce() { + let mut assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); + let mode_req_sender = ModeReqSenderMock::default(); + assy_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE); + assy_helper.add_mode_child(ExampleId::Id2 as u64, UNKNOWN_MODE); + assy_helper + .send_announce_mode_cmd_to_children(1, &mode_req_sender, false) + .unwrap(); + assert_eq!(mode_req_sender.requests.borrow().len(), 2); + let mut req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id1 as u64); + assert_eq!(req.request_id, 1); + assert_eq!(req.request, ModeRequest::AnnounceMode); + req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id2 as u64); + assert_eq!(req.request_id, 1); + assert_eq!(req.request, ModeRequest::AnnounceMode); + } + + #[test] + fn test_mode_announce_recursive() { + let mut assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); + let mode_req_sender = ModeReqSenderMock::default(); + assy_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE); + assy_helper.add_mode_child(ExampleId::Id2 as u64, UNKNOWN_MODE); + assy_helper + .send_announce_mode_cmd_to_children(1, &mode_req_sender, true) + .unwrap(); + assert_eq!(mode_req_sender.requests.borrow().len(), 2); + let mut req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id1 as u64); + assert_eq!(req.request_id, 1); + assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); + req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id2 as u64); + assert_eq!(req.request_id, 1); + assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); + } + + #[test] + fn test_mode_commanding_one_child() { + let mut dev_mgmt_helper = + DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); + let mode_req_sender = ModeReqSenderMock::default(); + dev_mgmt_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE); + let expected_mode = ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); + dev_mgmt_helper + .send_mode_cmd_to_one_child( + 1, + ExampleId::Id1 as u64, + expected_mode, + false, + &mode_req_sender, + ) + .unwrap(); + assert_eq!(mode_req_sender.requests.borrow().len(), 1); + let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id1 as u64); + assert_eq!(req.request_id, 1); + assert_eq!( + req.request, + ModeRequest::SetMode { + mode_and_submode: expected_mode, + forced: false + } + ); + matches!( + dev_mgmt_helper.state(), + DevManagerCommandingState::AwaitingReplies { .. } + ); + if let DevManagerCommandingState::AwaitingReplies(ctx) = dev_mgmt_helper.state() { + assert_eq!(ctx.target_mode, expected_mode); + assert_eq!(ctx.active_request_id, 1); + } + let reply = GenericMessage::new( + MessageMetadata::new(1, ExampleId::Id1 as u64), + ModeReply::ModeReply(expected_mode), + ); + if let DevManagerHelperResult::ModeCommandingDone(ActiveModeCommandContext { + target_mode, + active_request_id, + }) = dev_mgmt_helper.handle_mode_reply(&reply).unwrap() + { + assert_eq!(target_mode, expected_mode); + assert_eq!(active_request_id, 1); + } + matches!(dev_mgmt_helper.state(), DevManagerCommandingState::Idle); + } + + #[test] + fn test_mode_commanding_multi_child() { + let mut dev_mgmt_helper = + DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); + let mode_req_sender = ModeReqSenderMock::default(); + dev_mgmt_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE); + dev_mgmt_helper.add_mode_child(ExampleId::Id2 as u64, UNKNOWN_MODE); + let expected_mode = ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0); + dev_mgmt_helper + .send_mode_cmd_to_all_children(1, expected_mode, false, &mode_req_sender) + .unwrap(); + assert_eq!(mode_req_sender.requests.borrow().len(), 2); + let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id1 as u64); + assert_eq!(req.request_id, 1); + assert_eq!( + req.request, + ModeRequest::SetMode { + mode_and_submode: expected_mode, + forced: false + } + ); + let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req.target_id, ExampleId::Id2 as u64); + assert_eq!(req.request_id, 1); + assert_eq!( + req.request, + ModeRequest::SetMode { + mode_and_submode: expected_mode, + forced: false + } + ); + matches!( + dev_mgmt_helper.state(), + DevManagerCommandingState::AwaitingReplies { .. } + ); + if let DevManagerCommandingState::AwaitingReplies(ctx) = dev_mgmt_helper.state() { + assert_eq!(ctx.target_mode, expected_mode); + assert_eq!(ctx.active_request_id, 1); + } + + let reply = GenericMessage::new( + MessageMetadata::new(1, ExampleId::Id1 as u64), + ModeReply::ModeReply(expected_mode), + ); + assert_eq!( + dev_mgmt_helper.handle_mode_reply(&reply).unwrap(), + DevManagerHelperResult::Busy + ); + let reply = GenericMessage::new( + MessageMetadata::new(1, ExampleId::Id2 as u64), + ModeReply::ModeReply(expected_mode), + ); + if let DevManagerHelperResult::ModeCommandingDone(ActiveModeCommandContext { + target_mode, + active_request_id, + }) = dev_mgmt_helper.handle_mode_reply(&reply).unwrap() + { + assert_eq!(target_mode, expected_mode); + assert_eq!(active_request_id, 1); + } + matches!(dev_mgmt_helper.state(), DevManagerCommandingState::Idle); + } +} diff --git a/satrs/src/health.rs b/satrs/src/health.rs new file mode 100644 index 0000000..9d32457 --- /dev/null +++ b/satrs/src/health.rs @@ -0,0 +1,39 @@ +use crate::ComponentId; + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum HealthState { + Healthy = 1, + Faulty = 2, + PermanentFaulty = 3, + ExternalControl = 4, + NeedsRecovery = 5, +} + +pub trait HealthTableProvider { + fn health(&self, id: ComponentId) -> Option; + fn set_health(&mut self, id: ComponentId, health: HealthState); +} + +#[cfg(feature = "std")] +#[derive(Debug, Clone)] +pub struct HealthTableMapSync( + std::sync::Arc>>, +); + +#[cfg(feature = "std")] +impl HealthTableMapSync { + pub fn new(health_table: hashbrown::HashMap) -> Self { + Self(std::sync::Arc::new(std::sync::Mutex::new(health_table))) + } +} + +#[cfg(feature = "std")] +impl HealthTableProvider for HealthTableMapSync { + fn health(&self, id: ComponentId) -> Option { + self.0.lock().unwrap().get(&id).copied() + } + + fn set_health(&mut self, id: ComponentId, health: HealthState) { + self.0.lock().unwrap().insert(id, health); + } +} diff --git a/satrs/src/lib.rs b/satrs/src/lib.rs index 1801be5..5b81fe2 100644 --- a/satrs/src/lib.rs +++ b/satrs/src/lib.rs @@ -22,14 +22,21 @@ extern crate downcast_rs; #[cfg(any(feature = "std", test))] extern crate std; +pub mod action; +#[cfg(feature = "alloc")] +pub mod dev_mgmt; pub mod encoding; pub mod event_man; pub mod events; #[cfg(feature = "std")] pub mod executable; pub mod hal; +pub mod health; +pub mod hk; +pub mod mode; #[cfg(feature = "std")] pub mod mode_tree; +pub mod params; pub mod pool; pub mod power; pub mod pus; @@ -38,14 +45,11 @@ pub mod request; pub mod res_code; #[cfg(feature = "alloc")] pub mod scheduling; +#[cfg(feature = "alloc")] +pub mod subsystem; pub mod time; pub mod tmtc; -pub mod action; -pub mod hk; -pub mod mode; -pub mod params; - pub use spacepackets; use spacepackets::PacketId; diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs index 01beac9..b9bfa84 100644 --- a/satrs/src/mode.rs +++ b/satrs/src/mode.rs @@ -11,8 +11,10 @@ pub use alloc_mod::*; pub use std_mod::*; use crate::{ - queue::GenericTargetedMessagingError, - request::{GenericMessage, MessageMetadata, MessageReceiver, MessageReceiverWithId, RequestId}, + queue::{GenericReceiveError, GenericSendError}, + request::{ + GenericMessage, MessageMetadata, MessageReceiverProvider, MessageReceiverWithId, RequestId, + }, ComponentId, }; @@ -26,6 +28,11 @@ pub struct ModeAndSubmode { submode: Submode, } +pub const INVALID_MODE_VAL: Mode = Mode::MAX; +pub const UNKNOWN_MODE_VAL: Mode = Mode::MAX - 1; +pub const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(INVALID_MODE_VAL, 0); +pub const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(UNKNOWN_MODE_VAL, 0); + impl ModeAndSubmode { pub const RAW_LEN: usize = size_of::() + size_of::(); @@ -111,7 +118,10 @@ impl TargetedModeCommand { pub enum ModeRequest { /// Mode information. Can be used to notify other components of changed modes. ModeInfo(ModeAndSubmode), - SetMode(ModeAndSubmode), + SetMode { + mode_and_submode: ModeAndSubmode, + forced: bool, + }, ReadMode, AnnounceMode, AnnounceModeRecursive, @@ -127,6 +137,8 @@ pub struct TargetedModeRequest { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum ModeReply { + /// Mode information. Can be used to notify other components of changed modes. + ModeInfo(ModeAndSubmode), /// Reply to a mode request to confirm the commanded mode was reached. ModeReply(ModeAndSubmode), // Can not reach the commanded mode. Contains a reason as a [ResultU16]. @@ -147,34 +159,33 @@ pub trait ModeRequestSender { request_id: RequestId, target_id: ComponentId, request: ModeRequest, - ) -> Result<(), GenericTargetedMessagingError>; + ) -> Result<(), GenericSendError>; } pub trait ModeRequestReceiver { fn try_recv_mode_request( &self, - ) -> Result>, GenericTargetedMessagingError>; + ) -> Result>, GenericReceiveError>; } -impl> ModeRequestReceiver +impl> ModeRequestReceiver for MessageReceiverWithId { fn try_recv_mode_request( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.try_recv_message() } } -#[derive(Debug, Clone)] +#[derive(Debug, Clone, thiserror::Error)] pub enum ModeError { - Messaging(GenericTargetedMessagingError), -} - -impl From for ModeError { - fn from(value: GenericTargetedMessagingError) -> Self { - Self::Messaging(value) - } + #[error("Messaging send error: {0}")] + Send(#[from] GenericSendError), + #[error("Messaging receive error: {0}")] + Receive(#[from] GenericReceiveError), + #[error("busy with other mode request")] + Busy, } pub trait ModeProvider { @@ -196,6 +207,7 @@ pub trait ModeRequestHandler: ModeProvider { &mut self, requestor: MessageMetadata, mode_and_submode: ModeAndSubmode, + forced: bool, ) -> Result<(), Self::Error>; fn announce_mode(&self, requestor_info: Option, recursive: bool); @@ -222,9 +234,10 @@ pub trait ModeRequestHandler: ModeProvider { request: GenericMessage, ) -> Result<(), Self::Error> { match request.message { - ModeRequest::SetMode(mode_and_submode) => { - self.start_transition(request.requestor_info, mode_and_submode) - } + ModeRequest::SetMode { + mode_and_submode, + forced, + } => self.start_transition(request.requestor_info, mode_and_submode, forced), ModeRequest::ReadMode => self.send_mode_reply( request.requestor_info, ModeReply::ModeReply(self.mode_and_submode()), @@ -243,15 +256,16 @@ pub trait ModeRequestHandler: ModeProvider { } pub trait ModeReplyReceiver { - fn try_recv_mode_reply( - &self, - ) -> Result>, GenericTargetedMessagingError>; + fn try_recv_mode_reply(&self) + -> Result>, GenericReceiveError>; } -impl> ModeReplyReceiver for MessageReceiverWithId { +impl> ModeReplyReceiver + for MessageReceiverWithId +{ fn try_recv_mode_reply( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.try_recv_message() } } @@ -264,24 +278,28 @@ pub trait ModeReplySender { &self, requestor_info: MessageMetadata, reply: ModeReply, - ) -> Result<(), GenericTargetedMessagingError>; + ) -> Result<(), GenericSendError>; } #[cfg(feature = "alloc")] pub mod alloc_mod { - use crate::request::{ - MessageSender, MessageSenderAndReceiver, MessageSenderMap, RequestAndReplySenderAndReceiver, + use crate::{ + queue::{GenericReceiveError, GenericSendError}, + request::{ + MessageSenderAndReceiver, MessageSenderMap, MessageSenderProvider, + MessageSenderStoreProvider, RequestAndReplySenderAndReceiver, + }, }; use super::*; - impl> MessageSenderMap { + impl> MessageSenderMap { pub fn send_mode_reply( &self, requestor_info: MessageMetadata, target_id: ComponentId, request: ModeReply, - ) -> Result<(), GenericTargetedMessagingError> { + ) -> Result<(), GenericSendError> { self.send_message(requestor_info, target_id, request) } @@ -290,8 +308,13 @@ pub mod alloc_mod { } } - impl, R: MessageReceiver> ModeReplySender - for MessageSenderAndReceiver + impl< + From, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeReplySender + for MessageSenderAndReceiver { fn local_channel_id(&self) -> ComponentId { self.local_channel_id_generic() @@ -301,8 +324,8 @@ pub mod alloc_mod { &self, requestor_info: MessageMetadata, request: ModeReply, - ) -> Result<(), GenericTargetedMessagingError> { - self.message_sender_map.send_mode_reply( + ) -> Result<(), GenericSendError> { + self.message_sender_store.send_message( MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()), requestor_info.sender_id(), request, @@ -310,37 +333,67 @@ pub mod alloc_mod { } } - impl, R: MessageReceiver> ModeReplyReceiver - for MessageSenderAndReceiver + impl< + To, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeReplyReceiver + for MessageSenderAndReceiver { fn try_recv_mode_reply( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.message_receiver.try_recv_message() } } impl< - REQUEST, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, - > RequestAndReplySenderAndReceiver + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > + RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { - pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: S1) { - self.reply_sender_map + pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: ReplySender) { + self.reply_sender_store .add_message_target(target_id, reply_sender) } } impl< - REQUEST, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, - > ModeReplySender for RequestAndReplySenderAndReceiver + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeReplySender + for RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + ModeReply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { fn local_channel_id(&self) -> ComponentId { self.local_channel_id_generic() @@ -349,42 +402,56 @@ pub mod alloc_mod { fn send_mode_reply( &self, requestor_info: MessageMetadata, - request: ModeReply, - ) -> Result<(), GenericTargetedMessagingError> { - self.reply_sender_map.send_mode_reply( + reply: ModeReply, + ) -> Result<(), GenericSendError> { + self.reply_sender_store.send_message( MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()), requestor_info.sender_id(), - request, + reply, ) } } impl< - REQUEST, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, > ModeReplyReceiver - for RequestAndReplySenderAndReceiver + for RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + ModeReply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { fn try_recv_mode_reply( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.reply_receiver.try_recv_message() } } /// Helper type definition for a mode handler which can handle mode requests. - pub type ModeRequestHandlerInterface = - MessageSenderAndReceiver; + pub type ModeRequestHandlerInterface = + MessageSenderAndReceiver; - impl, R: MessageReceiver> - ModeRequestHandlerInterface + impl< + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeRequestHandlerInterface { pub fn try_recv_mode_request( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.try_recv_message() } @@ -392,7 +459,7 @@ pub mod alloc_mod { &self, requestor_info: MessageMetadata, reply: ModeReply, - ) -> Result<(), GenericTargetedMessagingError> { + ) -> Result<(), GenericSendError> { self.send_message( requestor_info.request_id(), requestor_info.sender_id(), @@ -403,12 +470,18 @@ pub mod alloc_mod { /// Helper type defintion for a mode handler object which can send mode requests and receive /// mode replies. - pub type ModeRequestorInterface = MessageSenderAndReceiver; + pub type ModeRequestorInterface = + MessageSenderAndReceiver; - impl, R: MessageReceiver> ModeRequestorInterface { + impl< + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + RequestSenderStore: MessageSenderStoreProvider, + > ModeRequestorInterface + { pub fn try_recv_mode_reply( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.try_recv_message() } @@ -417,23 +490,38 @@ pub mod alloc_mod { request_id: RequestId, target_id: ComponentId, reply: ModeRequest, - ) -> Result<(), GenericTargetedMessagingError> { + ) -> Result<(), GenericSendError> { self.send_message(request_id, target_id, reply) } } /// Helper type defintion for a mode handler object which can both send mode requests and /// process mode requests. - pub type ModeInterface = - RequestAndReplySenderAndReceiver; + pub type ModeInterface< + ReqSender, + ReqReceiver, + ReqSenderStore, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > = RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + ModeReply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + >; - impl> MessageSenderMap { + impl> MessageSenderMap { pub fn send_mode_request( &self, requestor_info: MessageMetadata, target_id: ComponentId, request: ModeRequest, - ) -> Result<(), GenericTargetedMessagingError> { + ) -> Result<(), GenericSendError> { self.send_message(requestor_info, target_id, request) } @@ -442,35 +530,28 @@ pub mod alloc_mod { } } - /* - impl> ModeRequestSender for MessageSenderMapWithId { - fn local_channel_id(&self) -> ComponentId { - self.local_channel_id - } - - fn send_mode_request( - &self, - request_id: RequestId, - target_id: ComponentId, - request: ModeRequest, - ) -> Result<(), GenericTargetedMessagingError> { - self.send_message(request_id, target_id, request) - } - } - */ - - impl, R: MessageReceiver> ModeRequestReceiver - for MessageSenderAndReceiver + impl< + To, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeRequestReceiver + for MessageSenderAndReceiver { fn try_recv_mode_request( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.message_receiver.try_recv_message() } } - impl, R: MessageReceiver> ModeRequestSender - for MessageSenderAndReceiver + impl< + From, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeRequestSender + for MessageSenderAndReceiver { fn local_channel_id(&self) -> ComponentId { self.local_channel_id_generic() @@ -481,8 +562,8 @@ pub mod alloc_mod { request_id: RequestId, target_id: ComponentId, request: ModeRequest, - ) -> Result<(), GenericTargetedMessagingError> { - self.message_sender_map.send_mode_request( + ) -> Result<(), GenericSendError> { + self.message_sender_store.send_message( MessageMetadata::new(request_id, self.local_channel_id()), target_id, request, @@ -491,27 +572,50 @@ pub mod alloc_mod { } impl< - REPLY, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, - > RequestAndReplySenderAndReceiver + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > + RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { - pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: S0) { - self.request_sender_map + pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: ReqSender) { + self.request_sender_store .add_message_target(target_id, request_sender) } } impl< - REPLY, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, > ModeRequestSender - for RequestAndReplySenderAndReceiver + for RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { fn local_channel_id(&self) -> ComponentId { self.local_channel_id_generic() @@ -522,8 +626,8 @@ pub mod alloc_mod { request_id: RequestId, target_id: ComponentId, request: ModeRequest, - ) -> Result<(), GenericTargetedMessagingError> { - self.request_sender_map.send_mode_request( + ) -> Result<(), GenericSendError> { + self.request_sender_store.send_message( MessageMetadata::new(request_id, self.local_channel_id()), target_id, request, @@ -532,17 +636,28 @@ pub mod alloc_mod { } impl< - REPLY, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, > ModeRequestReceiver - for RequestAndReplySenderAndReceiver + for RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { fn try_recv_mode_request( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.request_receiver.try_recv_message() } } @@ -552,39 +667,97 @@ pub mod alloc_mod { pub mod std_mod { use std::sync::mpsc; + use crate::request::{MessageSenderList, OneMessageSender}; + use super::*; pub type ModeRequestHandlerMpsc = ModeRequestHandlerInterface< mpsc::Sender>, mpsc::Receiver>, + MessageSenderList>>, >; pub type ModeRequestHandlerMpscBounded = ModeRequestHandlerInterface< mpsc::SyncSender>, mpsc::Receiver>, + MessageSenderList>>, >; - pub type ModeRequestorMpsc = ModeRequestorInterface< + pub type ModeRequestorOneChildMpsc = ModeRequestorInterface< mpsc::Sender>, mpsc::Receiver>, + OneMessageSender>>, >; - pub type ModeRequestorBoundedMpsc = ModeRequestorInterface< + pub type ModeRequestorOneChildBoundedMpsc = ModeRequestorInterface< mpsc::SyncSender>, mpsc::Receiver>, + OneMessageSender>>, + >; + pub type ModeRequestorChildListMpsc = ModeRequestorInterface< + mpsc::Sender>, + mpsc::Receiver>, + MessageSenderList>>, + >; + pub type ModeRequestorChildListBoundedMpsc = ModeRequestorInterface< + mpsc::SyncSender>, + mpsc::Receiver>, + MessageSenderList>>, >; pub type ModeRequestorAndHandlerMpsc = ModeInterface< mpsc::Sender>, - mpsc::Receiver>, - mpsc::Sender>, mpsc::Receiver>, + MessageSenderList>>, + mpsc::Sender>, + mpsc::Receiver>, + MessageSenderList>>, >; pub type ModeRequestorAndHandlerMpscBounded = ModeInterface< mpsc::SyncSender>, - mpsc::Receiver>, - mpsc::SyncSender>, mpsc::Receiver>, + MessageSenderList>>, + mpsc::SyncSender>, + mpsc::Receiver>, + MessageSenderList>>, >; } #[cfg(test)] -mod tests {} +pub(crate) mod tests { + use core::cell::RefCell; + use std::collections::VecDeque; + + use crate::{request::RequestId, ComponentId}; + + use super::*; + + pub struct ModeReqWrapper { + pub request_id: RequestId, + pub target_id: ComponentId, + pub request: ModeRequest, + } + + #[derive(Default)] + pub struct ModeReqSenderMock { + pub requests: RefCell>, + } + + impl ModeRequestSender for ModeReqSenderMock { + fn local_channel_id(&self) -> crate::ComponentId { + 0 + } + + fn send_mode_request( + &self, + request_id: RequestId, + target_id: ComponentId, + request: ModeRequest, + ) -> Result<(), GenericSendError> { + self.requests.borrow_mut().push_back(ModeReqWrapper { + request_id, + target_id, + request, + }); + Ok(()) + } + } +} diff --git a/satrs/src/mode_tree.rs b/satrs/src/mode_tree.rs index 1cddd32..e058378 100644 --- a/satrs/src/mode_tree.rs +++ b/satrs/src/mode_tree.rs @@ -2,10 +2,57 @@ use alloc::vec::Vec; use hashbrown::HashMap; use crate::{ - mode::{Mode, ModeAndSubmode, Submode}, + mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, Submode}, + request::MessageSenderProvider, ComponentId, }; +#[cfg(feature = "alloc")] +pub use alloc_mod::*; + +/// Common trait for node modes which can have mode parents or mode children. +pub trait ModeNode { + fn id(&self) -> ComponentId; +} +/// Trait which denotes that an object is a parent in a mode tree. +/// +/// A mode parent is capable of sending mode requests to child objects and has a unique component +/// ID. +pub trait ModeParent: ModeNode { + type Sender: MessageSenderProvider; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender); +} + +/// Trait which denotes that an object is a child in a mode tree. +/// +/// A child is capable of sending mode replies to parent objects and has a unique component ID. +pub trait ModeChild: ModeNode { + type Sender: MessageSenderProvider; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: Self::Sender); +} + +/// Utility method which connects a mode tree parent object to a child object by calling +/// [ModeParent::add_mode_child] on the [parent][ModeParent] and calling +/// [ModeChild::add_mode_parent] on the [child][ModeChild]. +/// +/// # Arguments +/// +/// * `parent` - The parent object which implements [ModeParent]. +/// * `request_sender` - Sender object to send mode requests to the child. +/// * `child` - The child object which implements [ModeChild]. +/// * `reply_sender` - Sender object to send mode replies to the parent. +pub fn connect_mode_nodes( + parent: &mut impl ModeParent, + request_sender: ReqSender, + child: &mut impl ModeChild, + reply_sender: ReplySender, +) { + parent.add_mode_child(child.id(), request_sender); + child.add_mode_parent(parent.id(), reply_sender); +} + #[derive(Debug, Copy, Clone, PartialEq, Eq)] pub enum TableEntryType { /// Target table containing information of the expected children modes for given mode. @@ -15,23 +62,553 @@ pub enum TableEntryType { Sequence, } -pub struct ModeTableEntry { +/// Common fields required for both target and sequence table entries. +/// +/// The most important parameters here are the target ID which this entry belongs to, and the mode +/// and submode the entry either will be commanded to for sequence table entries or which will be +/// monitored for target table entries. +#[derive(Debug, Copy, Clone)] +pub struct ModeTableEntryCommon { /// Name of respective table entry. pub name: &'static str, - /// Target channel ID. - pub channel_id: ComponentId, + /// Target component ID. + pub target_id: ComponentId, + /// Has a different meaning depending on whether this is a sequence table or a target table. + /// + /// - For sequence tables, this denotes the mode which will be commanded + /// - For target tables, this is the mode which the target children should have and which + /// might be monitored depending on configuration. pub mode_submode: ModeAndSubmode, + /// This mask allows to specify multiple allowed submodes for a given mode. pub allowed_submode_mask: Option, +} + +impl ModeTableEntryCommon { + pub fn set_allowed_submode_mask(&mut self, mask: Submode) { + self.allowed_submode_mask = Some(mask); + } + + pub fn allowed_submode_mask(&self) -> Option { + self.allowed_submode_mask + } +} + +/// An entry for the target tables. +#[derive(Debug)] +pub struct TargetTableEntry { + pub common: ModeTableEntryCommon, + pub monitor_state: bool, +} + +impl TargetTableEntry { + pub fn new( + name: &'static str, + target_id: ComponentId, + mode_submode: ModeAndSubmode, + allowed_submode_mask: Option, + ) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode_submode, + allowed_submode_mask, + }, + monitor_state: true, + } + } + + pub fn new_with_precise_submode( + name: &'static str, + target_id: ComponentId, + mode_submode: ModeAndSubmode, + ) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode_submode, + allowed_submode_mask: None, + }, + monitor_state: true, + } + } + + delegate::delegate! { + to self.common { + pub fn set_allowed_submode_mask(&mut self, mask: Submode); + pub fn allowed_submode_mask(&self) -> Option; + } + } +} + +/// An entry for the sequence tables. +/// +/// The [Self::check_success] field specifies that a mode sequence executor should check that the +/// target mode was actually reached before executing the next sequence. +#[derive(Debug)] +pub struct SequenceTableEntry { + pub common: ModeTableEntryCommon, pub check_success: bool, } -pub struct ModeTableMapValue { - /// Name for a given mode table entry. - pub name: &'static str, - pub entries: Vec, +impl SequenceTableEntry { + pub fn new( + name: &'static str, + target_id: ComponentId, + mode_submode: ModeAndSubmode, + check_success: bool, + ) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode_submode, + allowed_submode_mask: None, + }, + check_success, + } + } + + delegate::delegate! { + to self.common { + pub fn set_allowed_submode_mask(&mut self, mask: Submode); + pub fn allowed_submode_mask(&self) -> Option; + } + } } -pub type ModeTable = HashMap; +#[derive(Debug, thiserror::Error)] +#[error("target {0} not in mode store")] +pub struct TargetNotInModeStoreError(pub ComponentId); + +/// Mode store value type. +#[derive(Debug, Copy, Clone)] +pub struct ModeStoreValue { + /// ID of the mode component. + id: ComponentId, + /// Current mode and submode of the component. + pub mode_and_submode: ModeAndSubmode, + /// State information to track whether a reply should be awaited for the mode component. + pub awaiting_reply: bool, +} + +impl ModeStoreValue { + pub fn new(id: ComponentId, mode_and_submode: ModeAndSubmode) -> Self { + Self { + id, + mode_and_submode, + awaiting_reply: false, + } + } + + pub fn id(&self) -> ComponentId { + self.id + } + + pub fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +pub trait ModeStoreProvider { + fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode); + + fn has_component(&self, target_id: ComponentId) -> bool; + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue>; + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue>; + + /// Generic handler for mode replies received from child components. + /// + /// Implementation should clear the awaition flag if the `handle_reply_awaition` argument is + /// true and returns whether any children are still awaiting replies. If the flag is not set + fn mode_reply_handler_with_reply_awaition( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + ) -> bool { + self.mode_reply_handler(sender_id, reported_mode_and_submode, true) + .unwrap_or(false) + } + + fn mode_reply_handler_without_reply_awaition( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + ) { + self.mode_reply_handler(sender_id, reported_mode_and_submode, false); + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + with_reply_awaition: bool, + ) -> Option; +} + +#[cfg(feature = "alloc")] +pub mod alloc_mod { + use super::*; + + #[derive(Debug)] + pub struct TargetTablesMapValue { + /// Name for a given mode table entry. + pub name: &'static str, + /// Optional fallback mode if the target mode can not be kept. + pub fallback_mode: Option, + /// These are the rows of the a target table. + pub entries: Vec, + } + + impl TargetTablesMapValue { + pub fn new(name: &'static str, fallback_mode: Option) -> Self { + Self { + name, + fallback_mode, + entries: Default::default(), + } + } + + pub fn add_entry(&mut self, entry: TargetTableEntry) { + self.entries.push(entry); + } + } + + /// One sequence of a [SequenceTablesMapValue] in a [SequenceModeTables]. + /// + /// It contains all mode requests which need to be executed for a sequence step and it also + /// associates a [Self::name] with the sequence. + #[derive(Debug)] + pub struct SequenceTableMapTable { + /// Name for a given mode sequence. + pub name: &'static str, + /// These are the rows of the a sequence table. + pub entries: Vec, + } + + impl SequenceTableMapTable { + pub fn new(name: &'static str) -> Self { + Self { + name, + entries: Default::default(), + } + } + + pub fn add_entry(&mut self, entry: SequenceTableEntry) { + self.entries.push(entry); + } + } + + /// A sequence table entry. + /// + /// This is simply a list of [SequenceTableMapTable]s which also associates a [Self::name] + /// with the sequence. The order of sub-tables in the list also specifies the execution order + /// in the mode sequence. + #[derive(Debug)] + pub struct SequenceTablesMapValue { + /// Name for a given mode sequence. + pub name: &'static str, + /// Each sequence can consists of multiple sequences that are executed consecutively. + pub entries: Vec, + } + + impl SequenceTablesMapValue { + pub fn new(name: &'static str) -> Self { + Self { + name, + entries: Default::default(), + } + } + + pub fn add_sequence_table(&mut self, entry: SequenceTableMapTable) { + self.entries.push(entry); + } + } + + #[derive(Debug, Default)] + pub struct TargetModeTables(pub HashMap); + + impl TargetModeTables { + pub fn name(&self, mode: Mode) -> Option<&'static str> { + self.0.get(&mode).map(|value| value.name) + } + } + + impl SequenceModeTables { + pub fn name(&self, mode: Mode) -> Option<&'static str> { + self.0.get(&mode).map(|value| value.name) + } + + pub fn name_of_sequence(&self, mode: Mode, seq_idx: usize) -> Option<&'static str> { + self.0 + .get(&mode) + .map(|value| value.entries.get(seq_idx).map(|v| v.name))? + } + } + + /// This is the core data structure used to store mode sequence tables. + /// + /// A mode sequence table specifies which commands have to be sent in which order + /// to reach a certain [Mode]. Therefore, it simply maps a [Mode] to a [SequenceTablesMapValue]. + #[derive(Debug, Default)] + pub struct SequenceModeTables(pub HashMap); + + /// Mode store which tracks the [mode information][ModeStoreValue] inside a [Vec] + #[derive(Debug, Default)] + pub struct ModeStoreVec(pub alloc::vec::Vec); + + impl<'a> IntoIterator for &'a ModeStoreVec { + type Item = &'a ModeStoreValue; + type IntoIter = std::slice::Iter<'a, ModeStoreValue>; + + fn into_iter(self) -> Self::IntoIter { + self.0.iter() + } + } + + impl<'a> IntoIterator for &'a mut ModeStoreVec { + type Item = &'a mut ModeStoreValue; + type IntoIter = std::slice::IterMut<'a, ModeStoreValue>; + + fn into_iter(self) -> Self::IntoIter { + self.0.iter_mut() + } + } + + /// Mode store which tracks the mode information inside a [hashbrown::HashMap] + #[derive(Debug, Default)] + pub struct ModeStoreMap(pub hashbrown::HashMap); + + impl<'a> IntoIterator for &'a ModeStoreMap { + type Item = (&'a ComponentId, &'a ModeStoreValue); + type IntoIter = hashbrown::hash_map::Iter<'a, ComponentId, ModeStoreValue>; + + fn into_iter(self) -> Self::IntoIter { + self.0.iter() + } + } + + impl ModeStoreProvider for ModeStoreVec { + fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { + self.0.push(ModeStoreValue::new(target_id, mode)); + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.0.iter().any(|val| val.id == target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.0.iter().find(|val| val.id == target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.0.iter_mut().find(|val| val.id == target_id) + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + handle_reply_awaition: bool, + ) -> Option { + let mut still_awating_replies = None; + if handle_reply_awaition { + still_awating_replies = Some(false); + } + self.0.iter_mut().for_each(|val| { + if val.id() == sender_id { + if let Some(mode_and_submode) = reported_mode_and_submode { + val.mode_and_submode = mode_and_submode; + } + if handle_reply_awaition { + val.awaiting_reply = false; + } + } + if handle_reply_awaition && val.awaiting_reply { + still_awating_replies = Some(true); + } + }); + still_awating_replies + } + } + + impl ModeStoreProvider for ModeStoreMap { + fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { + self.0 + .insert(target_id, ModeStoreValue::new(target_id, mode)); + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.0.contains_key(&target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.0.get(&target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.0.get_mut(&target_id) + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + handle_reply_awaition: bool, + ) -> Option { + let mut still_awating_replies = None; + if handle_reply_awaition { + still_awating_replies = Some(false); + } + for val in self.0.values_mut() { + if val.id() == sender_id { + if let Some(mode_and_submode) = reported_mode_and_submode { + val.mode_and_submode = mode_and_submode; + } + if handle_reply_awaition { + val.awaiting_reply = false; + } + } + if handle_reply_awaition && val.awaiting_reply { + still_awating_replies = Some(true); + } + } + still_awating_replies + } + } +} #[cfg(test)] -mod tests {} +mod tests { + use super::*; + + fn generic_test(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + assert!(mode_store.has_component(1)); + assert!(mode_store.has_component(2)); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(0, 0) + ); + assert!(!mode_store.get(1).unwrap().awaiting_reply); + assert!(!mode_store.get(2).unwrap().awaiting_reply); + assert_eq!(mode_store.get(1).unwrap().id, 1); + assert_eq!(mode_store.get(2).unwrap().id, 2); + assert!(mode_store.get(3).is_none()); + assert!(mode_store.get_mut(3).is_none()); + } + + fn generic_reply_handling_with_reply_awaition(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.get_mut(1).unwrap().awaiting_reply = true; + mode_store.get_mut(2).unwrap().awaiting_reply = true; + let mut reply_awation_pending = + mode_store.mode_reply_handler_with_reply_awaition(1, Some(ModeAndSubmode::new(2, 0))); + assert!(reply_awation_pending); + reply_awation_pending = mode_store.mode_reply_handler_with_reply_awaition(2, None); + assert!(!reply_awation_pending); + assert!(!mode_store.get(1).unwrap().awaiting_reply); + assert!(!mode_store.get(2).unwrap().awaiting_reply); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(2, 0) + ); + assert_eq!( + mode_store.get(2).unwrap().mode_and_submode(), + ModeAndSubmode::new(1, 0) + ); + } + + fn generic_reply_handling_test_no_reply_awaition(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.get_mut(1).unwrap().awaiting_reply = true; + mode_store.get_mut(2).unwrap().awaiting_reply = true; + mode_store.mode_reply_handler_without_reply_awaition(1, Some(ModeAndSubmode::new(2, 0))); + mode_store.mode_reply_handler_without_reply_awaition(2, None); + assert!(mode_store.get(1).unwrap().awaiting_reply); + assert!(mode_store.get(2).unwrap().awaiting_reply); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(2, 0) + ); + assert_eq!( + mode_store.get(2).unwrap().mode_and_submode(), + ModeAndSubmode::new(1, 0) + ); + } + + fn generic_reply_handling_with_reply_awaition_2(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.get_mut(1).unwrap().awaiting_reply = true; + mode_store.get_mut(2).unwrap().awaiting_reply = true; + let mut reply_awation_pending = + mode_store.mode_reply_handler(1, Some(ModeAndSubmode::new(2, 0)), true); + assert!(reply_awation_pending.unwrap()); + reply_awation_pending = mode_store.mode_reply_handler(2, None, true); + assert!(!reply_awation_pending.unwrap()); + assert!(!mode_store.get(1).unwrap().awaiting_reply); + assert!(!mode_store.get(2).unwrap().awaiting_reply); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(2, 0) + ); + assert_eq!( + mode_store.get(2).unwrap().mode_and_submode(), + ModeAndSubmode::new(1, 0) + ); + } + + #[test] + fn test_vec_mode_store() { + let mut mode_store = ModeStoreVec::default(); + generic_test(&mut mode_store); + } + + #[test] + fn test_map_mode_store() { + let mut mode_store = ModeStoreMap::default(); + generic_test(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_vec_with_reply_awaition() { + let mut mode_store = ModeStoreVec::default(); + generic_reply_handling_with_reply_awaition(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_vec_with_reply_awaition_2() { + let mut mode_store = ModeStoreVec::default(); + generic_reply_handling_with_reply_awaition_2(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_map_with_reply_awaition() { + let mut mode_store = ModeStoreMap::default(); + generic_reply_handling_with_reply_awaition(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_map_with_reply_awaition_2() { + let mut mode_store = ModeStoreMap::default(); + generic_reply_handling_with_reply_awaition_2(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_vec_no_reply_awaition() { + let mut mode_store = ModeStoreVec::default(); + generic_reply_handling_test_no_reply_awaition(&mut mode_store); + } + #[test] + fn test_generic_reply_handler_map_no_reply_awaition() { + let mut mode_store = ModeStoreMap::default(); + generic_reply_handling_test_no_reply_awaition(&mut mode_store); + } +} diff --git a/satrs/src/pus/action.rs b/satrs/src/pus/action.rs index 6bcd270..7a048b1 100644 --- a/satrs/src/pus/action.rs +++ b/satrs/src/pus/action.rs @@ -66,9 +66,10 @@ impl GenericActionReplyPus { pub mod alloc_mod { use crate::{ action::ActionRequest, - queue::GenericTargetedMessagingError, + queue::{GenericReceiveError, GenericSendError}, request::{ - GenericMessage, MessageReceiver, MessageSender, MessageSenderAndReceiver, RequestId, + GenericMessage, MessageReceiverProvider, MessageSenderAndReceiver, + MessageSenderProvider, MessageSenderStoreProvider, RequestId, }, ComponentId, }; @@ -76,15 +77,18 @@ pub mod alloc_mod { use super::ActionReplyPus; /// Helper type definition for a mode handler which can handle mode requests. - pub type ActionRequestHandlerInterface = - MessageSenderAndReceiver; + pub type ActionRequestHandlerInterface = + MessageSenderAndReceiver; - impl, R: MessageReceiver> - ActionRequestHandlerInterface + impl< + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + ReplySender: MessageSenderStoreProvider, + > ActionRequestHandlerInterface { pub fn try_recv_action_request( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.try_recv_message() } @@ -93,22 +97,31 @@ pub mod alloc_mod { request_id: RequestId, target_id: ComponentId, reply: ActionReplyPus, - ) -> Result<(), GenericTargetedMessagingError> { + ) -> Result<(), GenericSendError> { self.send_message(request_id, target_id, reply) } } /// Helper type defintion for a mode handler object which can send mode requests and receive /// mode replies. - pub type ActionRequestorInterface = - MessageSenderAndReceiver; + pub type ActionRequestorInterface = + MessageSenderAndReceiver< + ActionRequest, + ActionReplyPus, + Sender, + Receiver, + RequestSenderStore, + >; - impl, R: MessageReceiver> - ActionRequestorInterface + impl< + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + RequestSenderStore: MessageSenderStoreProvider, + > ActionRequestorInterface { pub fn try_recv_action_reply( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.try_recv_message() } @@ -117,7 +130,7 @@ pub mod alloc_mod { request_id: RequestId, target_id: ComponentId, request: ActionRequest, - ) -> Result<(), GenericTargetedMessagingError> { + ) -> Result<(), GenericSendError> { self.send_message(request_id, target_id, request) } } @@ -132,6 +145,7 @@ pub mod std_mod { verification::{self, TcStateToken}, ActivePusRequestStd, ActiveRequestProvider, DefaultActiveRequestMap, }, + request::{MessageSenderMap, OneMessageSender}, ComponentId, }; @@ -174,22 +188,38 @@ pub mod std_mod { } pub type DefaultActiveActionRequestMap = DefaultActiveRequestMap; - pub type ActionRequestHandlerMpsc = ActionRequestHandlerInterface< + pub type ActionRequestHandlerOneSenderMpsc = ActionRequestHandlerInterface< mpsc::Sender>, mpsc::Receiver>, + OneMessageSender< + GenericMessage, + mpsc::Sender>, + >, >; - pub type ActionRequestHandlerMpscBounded = ActionRequestHandlerInterface< + pub type ActionRequestHandlerOneSenderMpscBounded = ActionRequestHandlerInterface< mpsc::SyncSender>, mpsc::Receiver>, + OneMessageSender< + GenericMessage, + mpsc::SyncSender>, + >, >; - pub type ActionRequestorMpsc = ActionRequestorInterface< + pub type ActionRequestorWithSenderMapMpsc = ActionRequestorInterface< mpsc::Sender>, mpsc::Receiver>, + MessageSenderMap< + GenericMessage, + mpsc::Sender>, + >, >; - pub type ActionRequestorBoundedMpsc = ActionRequestorInterface< + pub type ActionRequestorWithSenderMapBoundedMpsc = ActionRequestorInterface< mpsc::SyncSender>, mpsc::Receiver>, + MessageSenderMap< + GenericMessage, + mpsc::SyncSender>, + >, >; } diff --git a/satrs/src/pus/mod.rs b/satrs/src/pus/mod.rs index d4c5ff6..8a63454 100644 --- a/satrs/src/pus/mod.rs +++ b/satrs/src/pus/mod.rs @@ -1221,9 +1221,10 @@ pub(crate) fn source_buffer_large_enough( #[cfg(any(feature = "test_util", test))] pub mod test_util { - use crate::request::UniqueApidTargetId; use spacepackets::ecss::{tc::PusTcCreator, tm::PusTmReader}; + use crate::request::UniqueApidTargetId; + use super::{ verification::{self, TcStateAccepted, VerificationToken}, DirectPusPacketHandlerResult, PusPacketHandlingError, @@ -1232,6 +1233,7 @@ pub mod test_util { pub const TEST_APID: u16 = 0x101; pub const TEST_UNIQUE_ID_0: u32 = 0x05; pub const TEST_UNIQUE_ID_1: u32 = 0x06; + pub const TEST_COMPONENT_ID_0: UniqueApidTargetId = UniqueApidTargetId::new(TEST_APID, TEST_UNIQUE_ID_0); pub const TEST_COMPONENT_ID_1: UniqueApidTargetId = @@ -1268,14 +1270,13 @@ pub mod tests { use spacepackets::ecss::tm::{GenericPusTmSecondaryHeader, PusTmCreator, PusTmReader}; use spacepackets::ecss::{PusPacket, WritablePusPacket}; use spacepackets::CcsdsPacket; + use test_util::{TEST_APID, TEST_COMPONENT_ID_0}; use crate::pool::{PoolProvider, SharedStaticMemoryPool, StaticMemoryPool, StaticPoolConfig}; use crate::pus::verification::{RequestId, VerificationReporter}; use crate::tmtc::{PacketAsVec, PacketInPool, PacketSenderWithSharedPool, SharedPacketPool}; use crate::ComponentId; - use super::test_util::{TEST_APID, TEST_COMPONENT_ID_0}; - use super::verification::test_util::TestVerificationReporter; use super::verification::{ TcStateAccepted, VerificationReporterCfg, VerificationReportingProvider, VerificationToken, diff --git a/satrs/src/pus/mode.rs b/satrs/src/pus/mode.rs index 5ff78bf..773ad90 100644 --- a/satrs/src/pus/mode.rs +++ b/satrs/src/pus/mode.rs @@ -39,7 +39,7 @@ mod tests { use crate::{ mode::{ ModeAndSubmode, ModeReply, ModeReplySender, ModeRequest, ModeRequestSender, - ModeRequestorAndHandlerMpsc, ModeRequestorMpsc, + ModeRequestorAndHandlerMpsc, ModeRequestorOneChildMpsc, }, request::{GenericMessage, MessageMetadata}, }; @@ -52,7 +52,8 @@ mod tests { fn test_simple_mode_requestor() { let (reply_sender, reply_receiver) = mpsc::channel(); let (request_sender, request_receiver) = mpsc::channel(); - let mut mode_requestor = ModeRequestorMpsc::new(TEST_COMPONENT_ID_0, reply_receiver); + let mut mode_requestor = + ModeRequestorOneChildMpsc::new(TEST_COMPONENT_ID_0, reply_receiver); mode_requestor.add_message_target(TEST_COMPONENT_ID_1, request_sender); // Send a request and verify it arrives at the receiver. diff --git a/satrs/src/queue.rs b/satrs/src/queue.rs index 93c8ec8..c167114 100644 --- a/satrs/src/queue.rs +++ b/satrs/src/queue.rs @@ -1,6 +1,3 @@ -use core::fmt::{Display, Formatter}; -#[cfg(feature = "std")] -use std::error::Error; #[cfg(feature = "std")] use std::sync::mpsc; @@ -10,89 +7,31 @@ use crate::ComponentId; pub type ChannelId = u32; /// Generic error type for sending something via a message queue. -#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[derive(Debug, Copy, Clone, PartialEq, Eq, thiserror::Error)] pub enum GenericSendError { + #[error("rx side has disconnected")] RxDisconnected, + #[error("queue with max capacity of {0:?} is full")] QueueFull(Option), + #[error("target queue with ID {0} does not exist")] TargetDoesNotExist(ComponentId), } -impl Display for GenericSendError { - fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { - match self { - GenericSendError::RxDisconnected => { - write!(f, "rx side has disconnected") - } - GenericSendError::QueueFull(max_cap) => { - write!(f, "queue with max capacity of {max_cap:?} is full") - } - GenericSendError::TargetDoesNotExist(target) => { - write!(f, "target queue with ID {target} does not exist") - } - } - } -} - -#[cfg(feature = "std")] -impl Error for GenericSendError {} - /// Generic error type for sending something via a message queue. -#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[derive(Debug, Copy, Clone, PartialEq, Eq, thiserror::Error)] pub enum GenericReceiveError { + #[error("nothing to receive")] Empty, + #[error("tx side with id {0:?} has disconnected")] TxDisconnected(Option), } -impl Display for GenericReceiveError { - fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { - match self { - Self::TxDisconnected(channel_id) => { - write!(f, "tx side with id {channel_id:?} has disconnected") - } - Self::Empty => { - write!(f, "nothing to receive") - } - } - } -} - -#[cfg(feature = "std")] -impl Error for GenericReceiveError {} - -#[derive(Debug, Clone)] +#[derive(Debug, Clone, thiserror::Error)] pub enum GenericTargetedMessagingError { - Send(GenericSendError), - Receive(GenericReceiveError), -} -impl From for GenericTargetedMessagingError { - fn from(value: GenericSendError) -> Self { - Self::Send(value) - } -} - -impl From for GenericTargetedMessagingError { - fn from(value: GenericReceiveError) -> Self { - Self::Receive(value) - } -} - -impl Display for GenericTargetedMessagingError { - fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { - match self { - Self::Send(err) => write!(f, "generic targeted messaging error: {}", err), - Self::Receive(err) => write!(f, "generic targeted messaging error: {}", err), - } - } -} - -#[cfg(feature = "std")] -impl Error for GenericTargetedMessagingError { - fn source(&self) -> Option<&(dyn Error + 'static)> { - match self { - GenericTargetedMessagingError::Send(send) => Some(send), - GenericTargetedMessagingError::Receive(receive) => Some(receive), - } - } + #[error("generic targeted messaging send error: {0}")] + Send(#[from] GenericSendError), + #[error("generic targeted messaging receive error: {0}")] + Receive(#[from] GenericReceiveError), } #[cfg(feature = "std")] diff --git a/satrs/src/request.rs b/satrs/src/request.rs index f188798..180ce00 100644 --- a/satrs/src/request.rs +++ b/satrs/src/request.rs @@ -13,7 +13,10 @@ use spacepackets::{ ByteConversionError, }; -use crate::{queue::GenericTargetedMessagingError, ComponentId}; +use crate::{ + queue::{GenericReceiveError, GenericSendError}, + ComponentId, +}; /// Generic request ID type. Requests can be associated with an ID to have a unique identifier /// for them. This can be useful for tasks like tracking their progress. @@ -140,37 +143,38 @@ impl GenericMessage { } /// Generic trait for objects which can send targeted messages. -pub trait MessageSender: Send { - fn send(&self, message: GenericMessage) -> Result<(), GenericTargetedMessagingError>; +pub trait MessageSenderProvider: Send { + fn send(&self, message: GenericMessage) -> Result<(), GenericSendError>; } // Generic trait for objects which can receive targeted messages. -pub trait MessageReceiver { - fn try_recv(&self) -> Result>, GenericTargetedMessagingError>; +pub trait MessageReceiverProvider { + fn try_recv(&self) -> Result>, GenericReceiveError>; } -pub struct MessageWithSenderIdReceiver>(pub R, PhantomData); +pub struct MessageWithSenderIdReceiver>( + pub Receiver, + PhantomData, +); -impl> From for MessageWithSenderIdReceiver { +impl> From for MessageWithSenderIdReceiver { fn from(receiver: R) -> Self { MessageWithSenderIdReceiver(receiver, PhantomData) } } -impl> MessageWithSenderIdReceiver { - pub fn try_recv_message( - &self, - ) -> Result>, GenericTargetedMessagingError> { +impl> MessageWithSenderIdReceiver { + pub fn try_recv_message(&self) -> Result>, GenericReceiveError> { self.0.try_recv() } } -pub struct MessageReceiverWithId> { +pub struct MessageReceiverWithId> { local_channel_id: ComponentId, reply_receiver: MessageWithSenderIdReceiver, } -impl> MessageReceiverWithId { +impl> MessageReceiverWithId { pub fn new(local_channel_id: ComponentId, reply_receiver: R) -> Self { Self { local_channel_id, @@ -183,43 +187,129 @@ impl> MessageReceiverWithId { } } -impl> MessageReceiverWithId { - pub fn try_recv_message( - &self, - ) -> Result>, GenericTargetedMessagingError> { +impl> MessageReceiverWithId { + pub fn try_recv_message(&self) -> Result>, GenericReceiveError> { self.reply_receiver.0.try_recv() } } +pub trait MessageSenderStoreProvider: Default { + fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender); + + fn send_message( + &self, + requestor_info: MessageMetadata, + target_channel_id: ComponentId, + message: Message, + ) -> Result<(), GenericSendError>; +} + #[cfg(feature = "alloc")] pub mod alloc_mod { use crate::queue::GenericSendError; + use std::convert::From; use super::*; use hashbrown::HashMap; - pub struct MessageSenderMap>( - pub HashMap, + pub struct OneMessageSender> { + pub id_and_sender: Option<(ComponentId, S)>, + pub(crate) phantom: PhantomData, + } + + impl> Default for OneMessageSender { + fn default() -> Self { + Self { + id_and_sender: Default::default(), + phantom: Default::default(), + } + } + } + + impl> MessageSenderStoreProvider + for OneMessageSender + { + fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) { + if self.id_and_sender.is_some() { + return; + } + self.id_and_sender = Some((target_id, message_sender)); + } + + fn send_message( + &self, + requestor_info: MessageMetadata, + target_channel_id: ComponentId, + message: Msg, + ) -> Result<(), GenericSendError> { + if let Some((current_id, sender)) = &self.id_and_sender { + if *current_id == target_channel_id { + sender.send(GenericMessage::new(requestor_info, message))?; + return Ok(()); + } + } + Err(GenericSendError::TargetDoesNotExist(target_channel_id)) + } + } + + pub struct MessageSenderList>( + pub alloc::vec::Vec<(ComponentId, S)>, pub(crate) PhantomData, ); - impl> Default for MessageSenderMap { + impl> Default for MessageSenderList { fn default() -> Self { Self(Default::default(), PhantomData) } } - impl> MessageSenderMap { - pub fn add_message_target(&mut self, target_id: ComponentId, message_sender: S) { - self.0.insert(target_id, message_sender); + impl> MessageSenderStoreProvider + for MessageSenderList + { + fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) { + self.0.push((target_id, message_sender)); } - pub fn send_message( + fn send_message( &self, requestor_info: MessageMetadata, target_channel_id: ComponentId, - message: MSG, - ) -> Result<(), GenericTargetedMessagingError> { + message: Msg, + ) -> Result<(), GenericSendError> { + for (current_id, sender) in &self.0 { + if *current_id == target_channel_id { + sender.send(GenericMessage::new(requestor_info, message))?; + return Ok(()); + } + } + Err(GenericSendError::TargetDoesNotExist(target_channel_id)) + } + } + + pub struct MessageSenderMap>( + pub HashMap, + pub(crate) PhantomData, + ); + + impl> Default for MessageSenderMap { + fn default() -> Self { + Self(Default::default(), PhantomData) + } + } + + impl> MessageSenderStoreProvider + for MessageSenderMap + { + fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) { + self.0.insert(target_id, message_sender); + } + + fn send_message( + &self, + requestor_info: MessageMetadata, + target_channel_id: ComponentId, + message: Msg, + ) -> Result<(), GenericSendError> { if self.0.contains_key(&target_channel_id) { return self .0 @@ -227,29 +317,42 @@ pub mod alloc_mod { .unwrap() .send(GenericMessage::new(requestor_info, message)); } - Err(GenericSendError::TargetDoesNotExist(target_channel_id).into()) + Err(GenericSendError::TargetDoesNotExist(target_channel_id)) } } - pub struct MessageSenderAndReceiver, R: MessageReceiver> { + pub struct MessageSenderAndReceiver< + To, + From, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > { pub local_channel_id: ComponentId, - pub message_sender_map: MessageSenderMap, - pub message_receiver: MessageWithSenderIdReceiver, + pub message_sender_store: SenderStore, + pub message_receiver: MessageWithSenderIdReceiver, + pub(crate) phantom: PhantomData<(To, Sender)>, } - impl, R: MessageReceiver> - MessageSenderAndReceiver + impl< + To, + From, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > MessageSenderAndReceiver { - pub fn new(local_channel_id: ComponentId, message_receiver: R) -> Self { + pub fn new(local_channel_id: ComponentId, message_receiver: Receiver) -> Self { Self { local_channel_id, - message_sender_map: Default::default(), + message_sender_store: Default::default(), message_receiver: MessageWithSenderIdReceiver::from(message_receiver), + phantom: PhantomData, } } - pub fn add_message_target(&mut self, target_id: ComponentId, message_sender: S) { - self.message_sender_map + pub fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) { + self.message_sender_store .add_message_target(target_id, message_sender) } @@ -262,9 +365,9 @@ pub mod alloc_mod { &self, request_id: RequestId, target_id: ComponentId, - message: TO, - ) -> Result<(), GenericTargetedMessagingError> { - self.message_sender_map.send_message( + message: To, + ) -> Result<(), GenericSendError> { + self.message_sender_store.send_message( MessageMetadata::new(request_id, self.local_channel_id_generic()), target_id, message, @@ -274,48 +377,64 @@ pub mod alloc_mod { /// Try to receive a message, which can be a reply or a request, depending on the generics. pub fn try_recv_message( &self, - ) -> Result>, GenericTargetedMessagingError> { + ) -> Result>, GenericReceiveError> { self.message_receiver.try_recv_message() } } pub struct RequestAndReplySenderAndReceiver< - REQUEST, - REPLY, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, > { pub local_channel_id: ComponentId, // These 2 are a functional group. - pub request_sender_map: MessageSenderMap, - pub reply_receiver: MessageWithSenderIdReceiver, + pub request_sender_store: ReqSenderStore, + pub reply_receiver: MessageWithSenderIdReceiver, // These 2 are a functional group. - pub request_receiver: MessageWithSenderIdReceiver, - pub reply_sender_map: MessageSenderMap, + pub request_receiver: MessageWithSenderIdReceiver, + pub reply_sender_store: ReplySenderStore, + phantom: PhantomData<(ReqSender, ReplySender)>, } impl< - REQUEST, - REPLY, - S0: MessageSender, - R0: MessageReceiver, - S1: MessageSender, - R1: MessageReceiver, - > RequestAndReplySenderAndReceiver + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > + RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > { pub fn new( local_channel_id: ComponentId, - request_receiver: R1, - reply_receiver: R0, + request_receiver: ReqReceiver, + reply_receiver: ReplyReceiver, ) -> Self { Self { local_channel_id, request_receiver: request_receiver.into(), reply_receiver: reply_receiver.into(), - request_sender_map: Default::default(), - reply_sender_map: Default::default(), + request_sender_store: Default::default(), + reply_sender_store: Default::default(), + phantom: PhantomData, } } @@ -333,21 +452,19 @@ pub mod std_mod { use crate::queue::{GenericReceiveError, GenericSendError}; - impl MessageSender for mpsc::Sender> { - fn send(&self, message: GenericMessage) -> Result<(), GenericTargetedMessagingError> { + impl MessageSenderProvider for mpsc::Sender> { + fn send(&self, message: GenericMessage) -> Result<(), GenericSendError> { self.send(message) .map_err(|_| GenericSendError::RxDisconnected)?; Ok(()) } } - impl MessageSender for mpsc::SyncSender> { - fn send(&self, message: GenericMessage) -> Result<(), GenericTargetedMessagingError> { + impl MessageSenderProvider for mpsc::SyncSender> { + fn send(&self, message: GenericMessage) -> Result<(), GenericSendError> { if let Err(e) = self.try_send(message) { return match e { - mpsc::TrySendError::Full(_) => Err(GenericSendError::QueueFull(None).into()), - mpsc::TrySendError::Disconnected(_) => { - Err(GenericSendError::RxDisconnected.into()) - } + mpsc::TrySendError::Full(_) => Err(GenericSendError::QueueFull(None)), + mpsc::TrySendError::Disconnected(_) => Err(GenericSendError::RxDisconnected), }; } Ok(()) @@ -357,14 +474,14 @@ pub mod std_mod { pub type MessageSenderMapMpsc = MessageReceiverWithId>; pub type MessageSenderMapBoundedMpsc = MessageReceiverWithId>; - impl MessageReceiver for mpsc::Receiver> { - fn try_recv(&self) -> Result>, GenericTargetedMessagingError> { + impl MessageReceiverProvider for mpsc::Receiver> { + fn try_recv(&self) -> Result>, GenericReceiveError> { match self.try_recv() { Ok(msg) => Ok(Some(msg)), Err(e) => match e { mpsc::TryRecvError::Empty => Ok(None), mpsc::TryRecvError::Disconnected => { - Err(GenericReceiveError::TxDisconnected(None).into()) + Err(GenericReceiveError::TxDisconnected(None)) } }, } @@ -385,8 +502,8 @@ mod tests { }; use crate::{ - queue::{GenericReceiveError, GenericSendError, GenericTargetedMessagingError}, - request::{MessageMetadata, MessageSenderMap}, + queue::{GenericReceiveError, GenericSendError}, + request::{MessageMetadata, MessageSenderMap, MessageSenderStoreProvider}, }; use super::{GenericMessage, MessageReceiverWithId, UniqueApidTargetId}; @@ -478,9 +595,7 @@ mod tests { let reply = receiver.try_recv_message(); assert!(reply.is_err()); let error = reply.unwrap_err(); - if let GenericTargetedMessagingError::Receive(GenericReceiveError::TxDisconnected(None)) = - error - { + if let GenericReceiveError::TxDisconnected(None) = error { } else { panic!("unexpected error type"); } @@ -529,9 +644,7 @@ mod tests { ); assert!(result.is_err()); let error = result.unwrap_err(); - if let GenericTargetedMessagingError::Send(GenericSendError::TargetDoesNotExist(target)) = - error - { + if let GenericSendError::TargetDoesNotExist(target) = error { assert_eq!(target, TEST_CHANNEL_ID_2); } else { panic!("Unexpected error type"); @@ -556,7 +669,7 @@ mod tests { ); assert!(result.is_err()); let error = result.unwrap_err(); - if let GenericTargetedMessagingError::Send(GenericSendError::QueueFull(capacity)) = error { + if let GenericSendError::QueueFull(capacity) = error { assert!(capacity.is_none()); } else { panic!("Unexpected error type {}", error); @@ -576,7 +689,7 @@ mod tests { ); assert!(result.is_err()); let error = result.unwrap_err(); - if let GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected) = error { + if let GenericSendError::RxDisconnected = error { } else { panic!("Unexpected error type {}", error); } diff --git a/satrs/src/scheduling.rs b/satrs/src/scheduling.rs index 5df7033..ff2c0a1 100644 --- a/satrs/src/scheduling.rs +++ b/satrs/src/scheduling.rs @@ -154,7 +154,7 @@ pub mod std_mod { } /// Can be used to set the start of the slot to the current time. This is useful if a custom - /// runner implementation is used instead of the [Self::start] method. + /// runner implementation is used instead of the [Self::run_one_task_cycle] method. pub fn init_start_of_slot(&mut self) { self.start_of_slot = Instant::now(); } diff --git a/satrs/src/subsystem.rs b/satrs/src/subsystem.rs new file mode 100644 index 0000000..0212f76 --- /dev/null +++ b/satrs/src/subsystem.rs @@ -0,0 +1,1610 @@ +use crate::{ + health::{HealthState, HealthTableProvider}, + mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender, UNKNOWN_MODE_VAL}, + mode_tree::{ + ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableMapTable, + SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError, TargetTablesMapValue, + }, + queue::GenericTargetedMessagingError, + request::{GenericMessage, RequestId}, + ComponentId, +}; + +#[derive(Debug, PartialEq, Eq, Copy, Clone)] +pub enum SequenceExecutionHelperState { + /// The sequence execution is IDLE, no command is loaded or the sequence exection has + /// finished + Idle, + /// The sequence helper is executing a sequence and no replies need to be awaited. + Busy, + /// The sequence helper is still awaiting a reply from a mode children. The reply awaition + /// is a property of a mode commanding sequence + AwaitingSuccessCheck, +} + +#[derive(Debug, PartialEq, Eq)] +pub enum ModeCommandingResult { + /// The commanding of all children is finished + Done, + /// One step of a commanding chain is finished + StepDone, + /// Reply awaition is required for some children + AwaitingSuccessCheck, +} + +#[derive(Debug, thiserror::Error)] +#[error("mode {0} does not exist")] +pub struct ModeDoesNotExistError(Mode); + +#[derive(Debug, thiserror::Error)] +pub enum StartSequenceError { + #[error("mode {0} does not exist")] + ModeDoesNotExist(#[from] ModeDoesNotExistError), + #[error("invalid request ID")] + InvalidRequestId(RequestId), +} + +/// This sequence execution helper includes some boilerplate logic to +/// execute [SequenceModeTables]. +/// +/// It takes care of commanding the [ModeRequest]s specified in those tables and also includes the +/// states required to track the current progress of a sequence execution and take care of +/// reply and success awaition. +#[derive(Debug)] +pub struct SequenceExecutionHelper { + target_mode: Option, + state: SequenceExecutionHelperState, + request_id: Option, + current_sequence_index: Option, + last_sequence_index: Option, +} + +impl Default for SequenceExecutionHelper { + fn default() -> Self { + Self { + target_mode: None, + state: SequenceExecutionHelperState::Idle, + request_id: None, + current_sequence_index: None, + last_sequence_index: None, + } + } +} + +pub trait IsChildCommandable { + fn is_commandable(&self, id: ComponentId) -> bool; +} + +impl IsChildCommandable for T +where + T: HealthTableProvider, +{ + fn is_commandable(&self, id: ComponentId) -> bool { + self.health(id) + .is_none_or(|h| h != HealthState::ExternalControl) + } +} + +impl SequenceExecutionHelper { + pub fn new() -> Self { + Default::default() + } + + /// Load a new mode sequence to be executed + pub fn load( + &mut self, + mode: Mode, + request_id: RequestId, + sequence_tables: &SequenceModeTables, + ) -> Result<(), ModeDoesNotExistError> { + if !sequence_tables.0.contains_key(&mode) { + return Err(ModeDoesNotExistError(mode)); + } + self.target_mode = Some(mode); + self.request_id = Some(request_id); + self.state = SequenceExecutionHelperState::Busy; + self.current_sequence_index = None; + Ok(()) + } + + /// Run the sequence execution helper. + /// + /// This function will execute the sequence in the given [SequenceModeTables] based on the + /// mode loaded in [Self::load]. It calls [Self::execute_sequence_and_map_to_result] and + /// automatically takes care of state management, including increments of the sequence table + /// index. + /// + /// The returnvalues of the helper have the following meaning. + /// + /// * [ModeCommandingResult::AwaitingSuccessCheck] - The sequence is still awaiting a success. + /// The user should check whether all children have reached the commanded target mode, for + /// example by checking [mode replies][ModeReply] received by the children components, and + /// then calling [Self::confirm_sequence_done] to advance to the sequence or complete the + /// sequence. + /// * [ModeCommandingResult::CommandingDone] - The sequence is done. The user can load a new + /// sequence now without overwriting the last one. The sequence executor is in + /// [SequenceExecutionHelperState::Idle] again. + /// * [ModeCommandingResult::CommandingStepDone] - The sequence has advanced one step. The user + /// can now call [Self::run] again to immediately execute the next step in the sequence. + /// + /// Generally, periodic execution of the [Self::run] method should be performed while + /// [Self::state] is not [SequenceExecutionHelperState::Idle]. + /// + /// # Arguments + /// + /// * `table` - This table contains the sequence tables to reach the mode previously loaded + /// with [Self::load] + /// * `sender` - The sender to send mode requests to the components + /// * `children_mode_store` - The mode store vector to keep track of the mode states of + /// children components + pub fn run( + &mut self, + table: &SequenceModeTables, + sender: &impl ModeRequestSender, + children_mode_store: &mut ModeStoreVec, + is_commandable: &impl IsChildCommandable, + ) -> Result { + if self.state == SequenceExecutionHelperState::Idle { + return Ok(ModeCommandingResult::Done); + } + if self.state == SequenceExecutionHelperState::AwaitingSuccessCheck { + return Ok(ModeCommandingResult::AwaitingSuccessCheck); + } + if self.target_mode.is_none() { + return Ok(ModeCommandingResult::Done); + } + match self.current_sequence_index { + Some(idx) => { + // Execute the sequence. + let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); + self.execute_sequence_and_map_to_result( + seq_table_value, + idx, + sender, + children_mode_store, + is_commandable, + ) + } + None => { + // Find the first sequence + let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); + self.last_sequence_index = Some(seq_table_value.entries.len() - 1); + if seq_table_value.entries.is_empty() { + Ok(ModeCommandingResult::Done) + } else { + self.current_sequence_index = Some(0); + self.execute_sequence_and_map_to_result( + seq_table_value, + 0, + sender, + children_mode_store, + is_commandable, + ) + } + } + } + } + + /// Retrieve the currently loaded target mode + pub fn target_mode(&self) -> Option { + self.target_mode + } + + /// Confirm that a sequence which is awaiting a success check is done + pub fn confirm_sequence_done(&mut self) { + if let SequenceExecutionHelperState::AwaitingSuccessCheck = self.state { + self.state = SequenceExecutionHelperState::Busy; + if let (Some(last_sequence_index), Some(current_sequence_index)) = + (self.last_sequence_index, self.current_sequence_index) + { + if current_sequence_index == last_sequence_index { + self.state = SequenceExecutionHelperState::Idle; + } + } + self.current_sequence_index = Some(self.current_sequence_index.unwrap() + 1); + } + } + + /// Internal state of the execution helper. + pub fn state(&self) -> SequenceExecutionHelperState { + self.state + } + + pub fn request_id(&self) -> Option { + self.request_id + } + + pub fn set_request_id(&mut self, request_id: RequestId) { + self.request_id = Some(request_id); + } + + pub fn awaiting_success_check(&self) -> bool { + self.state == SequenceExecutionHelperState::AwaitingSuccessCheck + } + + pub fn current_sequence_index(&self) -> Option { + self.current_sequence_index + } + + /// Execute a sequence at the given sequence index for a given [SequenceTablesMapValue]. + /// + /// This method calls [Self::execute_sequence] and maps the result to a [ModeCommandingResult]. + /// It is also called by the [Self::run] method of this helper. + pub fn execute_sequence_and_map_to_result( + &mut self, + seq_table_value: &SequenceTablesMapValue, + sequence_idx: usize, + sender: &impl ModeRequestSender, + mode_store_vec: &mut ModeStoreVec, + is_commandable: &impl IsChildCommandable, + ) -> Result { + if self.state() == SequenceExecutionHelperState::Idle || self.request_id.is_none() { + return Ok(ModeCommandingResult::Done); + } + if Self::execute_sequence( + self.request_id.unwrap(), + &seq_table_value.entries[sequence_idx], + sender, + mode_store_vec, + is_commandable, + )? { + self.state = SequenceExecutionHelperState::AwaitingSuccessCheck; + Ok(ModeCommandingResult::AwaitingSuccessCheck) + } else if seq_table_value.entries.len() - 1 == sequence_idx { + self.state = SequenceExecutionHelperState::Idle; + return Ok(ModeCommandingResult::Done); + } else { + self.current_sequence_index = Some(sequence_idx + 1); + return Ok(ModeCommandingResult::StepDone); + } + } + + /// Generic stateless execution helper method. + /// + /// The [RequestId] and the [SequenceTableMapTable] to be executed are passed explicitely + /// here. This method is called by [Self::execute_sequence_and_map_to_result]. + /// + /// This method itereates through the entries of the given sequence table and sends out + /// [ModeRequest]s to set the modes of the children according to the table entries. + /// It also sets the reply awaition field in the children mode store where a success + /// check is required to true. + /// + /// It returns whether any commanding success check is required by any entry in the table. + pub fn execute_sequence( + request_id: RequestId, + map_table: &SequenceTableMapTable, + sender: &impl ModeRequestSender, + children_mode_store: &mut ModeStoreVec, + commandable: &impl IsChildCommandable, + ) -> Result { + let mut some_succes_check_required = false; + for entry in &map_table.entries { + if !commandable.is_commandable(entry.common.target_id) { + continue; + } + sender.send_mode_request( + request_id, + entry.common.target_id, + ModeRequest::SetMode { + mode_and_submode: entry.common.mode_submode, + forced: false, + }, + )?; + if entry.check_success { + children_mode_store.0.iter_mut().for_each(|val| { + if val.id() == entry.common.target_id { + val.awaiting_reply = true; + } + }); + some_succes_check_required = true; + } + } + Ok(some_succes_check_required) + } +} + +#[derive(Debug, Default, PartialEq, Eq, Clone, Copy)] +pub enum ModeTreeHelperState { + #[default] + Idle, + /// The helper is currently trying to keep a target mode. + TargetKeeping, + /// The helper is currently busy to command a mode. + ModeCommanding, +} + +#[derive(Debug, Default, PartialEq, Eq)] +pub enum SubsystemHelperResult { + #[default] + Idle, + /// Busy with target keeping. + TargetKeeping, + /// Result of a mode commanding operation + ModeCommanding(ModeCommandingResult), +} + +impl From for SubsystemHelperResult { + fn from(value: ModeCommandingResult) -> Self { + Self::ModeCommanding(value) + } +} + +#[derive(Debug, thiserror::Error)] +pub enum ModeTreeHelperError { + #[error("generic targeted messaging error: {0}")] + Message(#[from] GenericTargetedMessagingError), + #[error("current mode {0} is not contained in target table")] + CurrentModeNotInTargetTable(Mode), + /// Mode command has failed, for example while executing a mode table. + #[error("mode command failed")] + ModeCommmandFailure { + /// Table index of the sequence table entry which failed. + seq_table_index: Option, + }, + /// Target mode keeping violation. + #[error("target keeping violation")] + TargetKeepingViolation { + /// Table index of the sequence table entry which failed. + fallback_mode: Option, + }, +} + +/// This is a helper object which can be used by a subsystem component to execute mode sequences +/// and perform target keeping. +/// +/// This helper object tries to compose as much data and state information as possible which is +/// required for this process. +pub struct SubsystemCommandingHelper { + /// State of the helper. + state: ModeTreeHelperState, + /// Current mode of the owner subsystem. + current_mode: Mode, + /// This data structure is used to track all mode children. + pub children_mode_store: ModeStoreVec, + /// This field is set when a mode sequence is executed. It is used to determine whether mode + /// replies are relevant for reply awaition logic. + active_internal_request_id: Option, + /// The primary data structure to keep the target state information for subsystem + /// [modes][Mode]. it specifies the mode each child should have for a certain subsystem mode + /// and is relevant for target keeping. + pub target_tables: TargetModeTables, + /// The primary data structure to keep the sequence commanding information for commanded + /// subsystem [modes][Mode]. It specifies the actual commands and the order they should be + /// sent in to reach a certain [mode][Mode]. + pub sequence_tables: SequenceModeTables, + /// The sequence execution helper is used to execute sequences in the [Self::sequence_tables]. + pub seq_exec_helper: SequenceExecutionHelper, +} + +impl Default for SubsystemCommandingHelper { + fn default() -> Self { + Self { + current_mode: UNKNOWN_MODE_VAL, + state: Default::default(), + children_mode_store: Default::default(), + active_internal_request_id: None, + target_tables: Default::default(), + sequence_tables: Default::default(), + seq_exec_helper: Default::default(), + } + } +} + +impl SubsystemCommandingHelper { + /// Create a new substem commanding helper with an intial [ModeTreeHelperState::Idle] state, + /// an empty mode children store and empty target and sequence mode tables. + pub fn new( + children_mode_store: ModeStoreVec, + target_tables: TargetModeTables, + sequence_tables: SequenceModeTables, + ) -> Self { + Self { + current_mode: UNKNOWN_MODE_VAL, + state: ModeTreeHelperState::Idle, + children_mode_store, + active_internal_request_id: None, + target_tables, + sequence_tables, + seq_exec_helper: Default::default(), + } + } + + pub fn state(&self) -> ModeTreeHelperState { + self.state + } + + pub fn mode(&self) -> Mode { + self.current_mode + } + + pub fn request_id(&self) -> Option { + self.active_internal_request_id.map(|v| v >> 8) + } + + /// This returns the internal request ID, which is the regular [Self::request_id] specified + /// by the user shifter 8 to the right and then increment with the current sequence commanding + /// step. The value can still be retrieved because it might be required for reply verification. + /// + /// The state machine specifies this request ID for all mode commands related to the + /// current step of sequence commanding. + pub fn internal_request_id(&self) -> Option { + self.active_internal_request_id + } + + /// Retrieve the fallback mode for the current mode of the subsystem by trying to retrieve + /// it from the target table. + /// + /// If the current mode does not have a fallback mode, returns [None]. + /// If the current mode is not inside the target table, returns a [ModeDoesNotExistError]. + /// The fallback mode can and should be commanded when a target keeping violation was detected + /// or after self-commanding to the current mode has failed, which can happen after a failed + /// mode table execution. + pub fn fallback_mode(&self) -> Result, ModeDoesNotExistError> { + self.target_tables + .0 + .get(&self.current_mode) + .ok_or(ModeDoesNotExistError(self.current_mode)) + .map(|v| v.fallback_mode) + } + + /// Add a mode child to the internal [Self::children_mode_store]. + pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeAndSubmode) { + self.children_mode_store.add_component(child, mode); + } + + /// Add a target mode table and an associated sequence mode table. + pub fn add_target_and_sequence_table( + &mut self, + mode: Mode, + target_table_val: TargetTablesMapValue, + sequence_table_val: SequenceTablesMapValue, + ) { + self.target_tables.0.insert(mode, target_table_val); + self.sequence_tables.0.insert(mode, sequence_table_val); + } + + /// Starts a command sequence for a given [mode][Mode]. + /// + /// # Arguments + /// + /// - `mode` - The mode to command + /// - `request_id` - Request ID associated with the command sequence. The value of this value + /// should not be larger than the maximum possible value for 24 bits: (2 ^ 24) - 1 = 16777215 + /// because 8 bits are reserved for internal sequence index tracking. + pub fn start_command_sequence( + &mut self, + mode: Mode, + request_id: RequestId, + ) -> Result<(), StartSequenceError> { + if request_id > 2_u32.pow(24) - 1 { + return Err(StartSequenceError::InvalidRequestId(request_id)); + } + self.active_internal_request_id = Some(request_id << 8); + self.seq_exec_helper.load( + mode, + self.active_internal_request_id.unwrap(), + &self.sequence_tables, + )?; + self.state = ModeTreeHelperState::ModeCommanding; + Ok(()) + } + + pub fn send_announce_mode_cmd_to_children( + &self, + request_id: RequestId, + req_sender: &impl ModeRequestSender, + recursive: bool, + ) -> Result<(), GenericTargetedMessagingError> { + let mut request = ModeRequest::AnnounceMode; + if recursive { + request = ModeRequest::AnnounceModeRecursive; + } + for child in &self.children_mode_store.0 { + req_sender.send_mode_request(request_id, child.id(), request)?; + } + Ok(()) + } + + pub fn state_machine( + &mut self, + opt_reply: Option>, + req_sender: &impl ModeRequestSender, + is_commandable: &impl IsChildCommandable, + ) -> Result { + if let Some(reply) = opt_reply { + if self.handle_mode_reply(&reply)? { + if self.seq_exec_helper.state() == SequenceExecutionHelperState::Idle { + self.transition_to_target_keeping(); + return Ok(SubsystemHelperResult::ModeCommanding( + ModeCommandingResult::Done, + )); + } + return Ok(SubsystemHelperResult::ModeCommanding( + ModeCommandingResult::StepDone, + )); + } + } + match self.state { + ModeTreeHelperState::Idle => Ok(SubsystemHelperResult::Idle), + ModeTreeHelperState::TargetKeeping => { + // We check whether the current mode is modelled by a target table first. + if let Some(target_table) = self.target_tables.0.get(&self.current_mode) { + self.perform_target_keeping(target_table)?; + } + Ok(SubsystemHelperResult::TargetKeeping) + } + ModeTreeHelperState::ModeCommanding => { + let result = self.seq_exec_helper.run( + &self.sequence_tables, + req_sender, + &mut self.children_mode_store, + is_commandable, + )?; + match result { + ModeCommandingResult::Done => { + // By default, the helper will automatically transition into the target keeping + // mode after an executed sequence. + self.transition_to_target_keeping(); + } + ModeCommandingResult::StepDone => { + // Normally, this step is done after all replies were received, but if no + // reply checking is required for a command sequence, the step would never + // be performed, so this function needs to be called here as well. + self.update_internal_req_id(); + } + ModeCommandingResult::AwaitingSuccessCheck => (), + } + Ok(result.into()) + } + } + } + + fn transition_to_target_keeping(&mut self) { + self.state = ModeTreeHelperState::TargetKeeping; + self.current_mode = self.seq_exec_helper.target_mode().unwrap(); + } + + fn perform_target_keeping( + &self, + target_table: &TargetTablesMapValue, + ) -> Result<(), ModeTreeHelperError> { + for entry in &target_table.entries { + if !entry.monitor_state { + continue; + } + let mut target_mode_violated = false; + self.children_mode_store.0.iter().for_each(|val| { + if val.id() == entry.common.target_id { + target_mode_violated = + if let Some(allowed_submode_mask) = entry.allowed_submode_mask() { + let fixed_bits = !allowed_submode_mask; + (val.mode_and_submode().mode() != entry.common.mode_submode.mode()) + && (val.mode_and_submode().submode() & fixed_bits + != entry.common.mode_submode.submode() & fixed_bits) + } else { + val.mode_and_submode() != entry.common.mode_submode + }; + } + }); + if target_mode_violated { + // Target keeping violated. Report violation and fallback mode to user. + return Err(ModeTreeHelperError::TargetKeepingViolation { + fallback_mode: target_table.fallback_mode, + }); + } + } + Ok(()) + } + + fn update_internal_req_id(&mut self) { + let new_internal_req_id = self.request_id().unwrap() << 8 + | self.seq_exec_helper.current_sequence_index().unwrap() as u32; + self.seq_exec_helper.set_request_id(new_internal_req_id); + self.active_internal_request_id = Some(new_internal_req_id); + } + + // Handles a mode reply message and returns whether the reply completes a step of sequence + // commanding. + fn handle_mode_reply( + &mut self, + reply: &GenericMessage, + ) -> Result { + if !self.children_mode_store.has_component(reply.sender_id()) { + return Ok(false); + } + let mut generic_mode_reply_handler = + |sender_id, mode_and_submode: Option, success: bool| { + let mut partial_step_done = false; + // Tying the reply awaition to the request ID ensures that something like replies + // belonging to older requests do not interfere with the completion handling of + // the mode commanding. This is important for forced mode commands. + let mut handle_awaition = false; + if self.state == ModeTreeHelperState::ModeCommanding + && self.active_internal_request_id.is_some() + && reply.request_id() == self.active_internal_request_id.unwrap() + { + handle_awaition = true; + } + let still_awating_replies = self.children_mode_store.mode_reply_handler( + sender_id, + mode_and_submode, + handle_awaition, + ); + if self.state == ModeTreeHelperState::ModeCommanding + && handle_awaition + && !still_awating_replies.unwrap_or(false) + { + self.seq_exec_helper.confirm_sequence_done(); + self.update_internal_req_id(); + partial_step_done = true; + } + if !success && self.state == ModeTreeHelperState::ModeCommanding { + // The user has to decide how to proceed. + self.state = ModeTreeHelperState::Idle; + return Err(ModeTreeHelperError::ModeCommmandFailure { + seq_table_index: self.seq_exec_helper.current_sequence_index(), + }); + } + Ok(partial_step_done) + }; + match reply.message { + ModeReply::ModeInfo(mode_and_submode) => { + generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true) + } + ModeReply::ModeReply(mode_and_submode) => { + generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true) + } + ModeReply::CantReachMode(_) => { + generic_mode_reply_handler(reply.sender_id(), None, false) + } + ModeReply::WrongMode { reached, .. } => { + generic_mode_reply_handler(reply.sender_id(), Some(reached), true) + } + } + } + + pub fn update_child_mode( + &mut self, + child: ComponentId, + mode: ModeAndSubmode, + ) -> Result<(), TargetNotInModeStoreError> { + let val_mut = self + .children_mode_store + .get_mut(child) + .ok_or(TargetNotInModeStoreError(child))?; + val_mut.mode_and_submode = mode; + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + use crate::{ + mode::{ + tests::{ModeReqSenderMock, ModeReqWrapper}, + Mode, ModeAndSubmode, ModeReply, ModeRequest, UNKNOWN_MODE, + }, + mode_tree::{ + ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, + SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables, + }, + queue::GenericTargetedMessagingError, + request::{GenericMessage, MessageMetadata, RequestId}, + subsystem::{ModeCommandingResult, ModeTreeHelperState, SequenceExecutionHelperState}, + ComponentId, + }; + + #[derive(Debug)] + pub enum ExampleTargetId { + Target0 = 1, + Target1 = 2, + Target2 = 3, + } + + #[derive(Debug)] + pub enum ExampleMode { + Mode0 = 1, + Mode1 = 2, + Mode2 = 3, + } + + #[derive(Debug, Default)] + pub struct IsCommandableMock { + pub commandable_map: std::collections::HashMap, + } + + impl IsChildCommandable for IsCommandableMock { + fn is_commandable(&self, id: ComponentId) -> bool { + self.commandable_map.get(&id).copied().unwrap_or(true) + } + } + + pub struct SequenceExecutorTestbench { + pub sender: ModeReqSenderMock, + pub mode_store: ModeStoreVec, + pub seq_tables: SequenceModeTables, + pub execution_helper: SequenceExecutionHelper, + pub is_commandable_mock: IsCommandableMock, + } + + impl SequenceExecutorTestbench { + pub fn new() -> Self { + let mode_store = create_default_mode_store(); + let (seq_tables, _) = create_simple_sample_seq_tables(); + Self { + sender: ModeReqSenderMock::default(), + mode_store, + seq_tables, + execution_helper: SequenceExecutionHelper::new(), + is_commandable_mock: IsCommandableMock::default(), + } + } + + pub fn get_mode_table(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue { + self.seq_tables.0.get_mut(&(mode as Mode)).unwrap() + } + + pub fn run(&mut self) -> Result { + self.execution_helper.run( + &self.seq_tables, + &self.sender, + &mut self.mode_store, + &self.is_commandable_mock, + ) + } + + fn check_run_is_no_op(&mut self) { + // Assure that no unexpected behaviour occurs. + assert_eq!( + self.execution_helper + .run( + &self.seq_tables, + &self.sender, + &mut self.mode_store, + &self.is_commandable_mock + ) + .unwrap(), + ModeCommandingResult::Done + ); + assert_eq!( + self.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + assert!(self.sender.requests.borrow().is_empty()); + } + + fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) { + assert_eq!( + self.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!(self.sender.requests.borrow().len(), 2); + let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(req_0.target_id, ExampleTargetId::Target0 as u64); + assert_eq!(req_0.request_id, expected_req_id); + assert_eq!( + req_0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE, + forced: false + } + ); + let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req_1.target_id, ExampleTargetId::Target1 as u64); + assert_eq!( + req_1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE, + forced: false + } + ); + } + fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) { + assert_eq!( + self.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!(self.sender.requests.borrow().len(), 1); + let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(req_0.target_id, ExampleTargetId::Target2 as u64); + assert_eq!(req_0.request_id, expected_req_id); + assert_eq!( + req_0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + forced: false + } + ); + } + + fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) { + assert_eq!( + self.execution_helper.target_mode().unwrap(), + ExampleMode::Mode0 as Mode + ); + assert_eq!(self.execution_helper.current_sequence_index().unwrap(), 0); + assert_eq!(self.sender.requests.borrow().len(), 2); + let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(req_0.target_id, ExampleTargetId::Target0 as u64); + assert_eq!(req_0.request_id, expected_req_id); + assert_eq!( + req_0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE, + forced: false + } + ); + let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req_1.target_id, ExampleTargetId::Target1 as u64); + assert_eq!( + req_1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE, + forced: false + } + ); + } + } + + fn create_default_mode_store() -> ModeStoreVec { + let mut mode_store = ModeStoreVec::default(); + mode_store.add_component(ExampleTargetId::Target0 as u64, UNKNOWN_MODE); + mode_store.add_component(ExampleTargetId::Target1 as u64, UNKNOWN_MODE); + mode_store.add_component(ExampleTargetId::Target2 as u64, UNKNOWN_MODE); + mode_store + } + + fn create_simple_sample_seq_tables() -> (SequenceModeTables, TargetModeTables) { + let mut seq_tables = SequenceModeTables::default(); + // Mode 0 - One step command + let mut table_val = SequenceTablesMapValue::new("MODE_0"); + let mut table_seq_0 = SequenceTableMapTable::new("MODE_0_SEQ_0"); + table_seq_0.add_entry(SequenceTableEntry::new( + "TARGET_0", + ExampleTargetId::Target0 as u64, + SUBSYSTEM_MD0_TGT0_MODE, + false, + )); + table_seq_0.add_entry(SequenceTableEntry::new( + "TARGET_1", + ExampleTargetId::Target1 as u64, + SUBSYSTEM_MD0_TGT1_MODE, + false, + )); + table_val.add_sequence_table(table_seq_0); + seq_tables.0.insert(ExampleMode::Mode0 as u32, table_val); + + // Mode 1 - Multi Step command + let mut table_val = SequenceTablesMapValue::new("MODE_1"); + let mut table_seq_0 = SequenceTableMapTable::new("MODE_1_SEQ_0"); + table_seq_0.add_entry(SequenceTableEntry::new( + "MD1_SEQ0_TGT0", + ExampleTargetId::Target0 as u64, + SUBSYSTEM_MD1_ST0_TGT0_MODE, + false, + )); + table_seq_0.add_entry(SequenceTableEntry::new( + "MD1_SEQ0_TGT1", + ExampleTargetId::Target1 as u64, + SUBSYSTEM_MD1_ST0_TGT1_MODE, + false, + )); + table_val.add_sequence_table(table_seq_0); + let mut table_seq_1 = SequenceTableMapTable::new("MODE_1_SEQ_1"); + table_seq_1.add_entry(SequenceTableEntry::new( + "MD1_SEQ1_TGT2", + ExampleTargetId::Target2 as u64, + SUBSYSTEM_MD1_ST1_TGT2_MODE, + false, + )); + table_val.add_sequence_table(table_seq_1); + seq_tables.0.insert(ExampleMode::Mode1 as u32, table_val); + + let mode_tables = TargetModeTables::default(); + // TODO: Write mode tables. + (seq_tables, mode_tables) + } + + pub struct SubsystemHelperTestbench { + pub sender: ModeReqSenderMock, + pub helper: SubsystemCommandingHelper, + pub is_commandable_mock: IsCommandableMock, + } + + impl SubsystemHelperTestbench { + pub fn new() -> Self { + let (sequence_tables, target_tables) = create_simple_sample_seq_tables(); + Self { + sender: ModeReqSenderMock::default(), + helper: SubsystemCommandingHelper::new( + create_default_mode_store(), + target_tables, + sequence_tables, + ), + is_commandable_mock: IsCommandableMock::default(), + } + } + + pub fn start_command_sequence( + &mut self, + mode: ExampleMode, + request_id: RequestId, + ) -> Result<(), StartSequenceError> { + self.helper.start_command_sequence(mode as Mode, request_id) + } + + pub fn send_announce_mode_cmd_to_children( + &mut self, + request_id: RequestId, + recursive: bool, + ) -> Result<(), GenericTargetedMessagingError> { + self.helper + .send_announce_mode_cmd_to_children(request_id, &self.sender, recursive) + } + + pub fn get_sequence_tables(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue { + self.helper + .sequence_tables + .0 + .get_mut(&(mode as Mode)) + .unwrap() + } + + pub fn state_machine( + &mut self, + opt_reply: Option>, + ) -> Result { + self.helper + .state_machine(opt_reply, &self.sender, &self.is_commandable_mock) + } + + pub fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) { + assert_eq!(self.sender.requests.borrow().len(), 2); + let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req0.request_id, expected_req_id); + assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId); + assert_eq!( + req0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE, + forced: false + } + ); + + let req1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req1.request_id, expected_req_id); + assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId); + assert_eq!( + req1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE, + forced: false + } + ); + } + + pub fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) { + assert_eq!(self.sender.requests.borrow().len(), 2); + let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req0.request_id, expected_req_id); + assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId); + assert_eq!( + req0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE, + forced: false + } + ); + + let req1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req1.request_id, expected_req_id); + assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId); + assert_eq!( + req1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE, + forced: false + } + ); + } + + pub fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) { + assert_eq!(self.sender.requests.borrow().len(), 1); + let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req0.request_id, expected_req_id); + assert_eq!(req0.target_id, ExampleTargetId::Target2 as ComponentId); + assert_eq!( + req0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + forced: false + } + ); + } + } + + const SUBSYSTEM_MD0_TGT0_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0); + const SUBSYSTEM_MD0_TGT1_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); + + const SUBSYSTEM_MD1_ST0_TGT0_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0); + const SUBSYSTEM_MD1_ST0_TGT1_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0); + const SUBSYSTEM_MD1_ST1_TGT2_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); + + #[test] + fn test_init_state() { + let execution_helper = SequenceExecutionHelper::new(); + assert_eq!(execution_helper.state(), SequenceExecutionHelperState::Idle); + assert!(!execution_helper.awaiting_success_check()); + assert!(execution_helper.target_mode().is_none()); + assert!(execution_helper.current_sequence_index().is_none()); + } + + #[test] + fn test_sequence_execution_helper_no_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode0 as Mode + ); + assert_eq!( + tb.run().expect("sequence exeecution helper run failure"), + ModeCommandingResult::Done + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + assert!(!tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md0(expected_req_id); + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_with_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let mode0_table = tb.get_mode_table(ExampleMode::Mode0); + mode0_table.entries[0].entries[0].check_success = true; + mode0_table.entries[0].entries[1].check_success = true; + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode0 as Mode + ); + assert_eq!( + tb.run().expect("sequence exeecution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + // These are not cleared, even if the execution helper is already IDLE. This is okay for + // now. + assert!(tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md0(expected_req_id); + tb.execution_helper.confirm_sequence_done(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_with_partial_check() { + let mut tb = SequenceExecutorTestbench::new(); + let mode0_table = tb.get_mode_table(ExampleMode::Mode0); + mode0_table.entries[0].entries[0].check_success = true; + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + // These are not cleared, even if the execution helper is already IDLE. This is okay for + // now. + assert!(tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md0(expected_req_id); + tb.execution_helper.confirm_sequence_done(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_multi_step_no_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::StepDone + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md1_step0(expected_req_id); + assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1); + + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::Done + ); + tb.generic_checks_subsystem_md1_step1(expected_req_id); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_multi_step_full_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + let mode1_table = tb.get_mode_table(ExampleMode::Mode1); + mode1_table.entries[0].entries[0].check_success = true; + mode1_table.entries[0].entries[1].check_success = true; + mode1_table.entries[1].entries[0].check_success = true; + + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + assert!(tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md1_step0(expected_req_id); + assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 0); + tb.execution_helper.confirm_sequence_done(); + + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + assert!(tb.execution_helper.awaiting_success_check()); + assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1); + tb.generic_checks_subsystem_md1_step1(expected_req_id); + tb.execution_helper.confirm_sequence_done(); + tb.check_run_is_no_op(); + } + + // TODO: Test subsystem commanding helper + #[test] + fn test_subsystem_helper_basic_state() { + let tb = SubsystemHelperTestbench::new(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::Idle); + assert!(tb.helper.active_internal_request_id.is_none()); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + assert!(tb.helper.request_id().is_none()); + } + + #[test] + fn test_subsystem_helper_announce_recursive() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.send_announce_mode_cmd_to_children(expected_req_id, true) + .unwrap(); + assert_eq!(tb.sender.requests.borrow().len(), 3); + let check_req = |req: ModeReqWrapper, target_id: ComponentId| { + assert_eq!(req.target_id, target_id); + assert_eq!(req.request_id, expected_req_id); + assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); + }; + let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req0, ExampleTargetId::Target0 as u64); + let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req1, ExampleTargetId::Target1 as u64); + let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req2, ExampleTargetId::Target2 as u64); + } + + #[test] + fn test_subsystem_helper_announce() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.send_announce_mode_cmd_to_children(expected_req_id, false) + .unwrap(); + assert_eq!(tb.sender.requests.borrow().len(), 3); + let check_req = |req: ModeReqWrapper, target_id: ComponentId| { + assert_eq!(req.target_id, target_id); + assert_eq!(req.request_id, expected_req_id); + assert_eq!(req.request, ModeRequest::AnnounceMode); + }; + let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req0, ExampleTargetId::Target0 as u64); + let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req1, ExampleTargetId::Target1 as u64); + let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req2, ExampleTargetId::Target2 as u64); + } + + #[test] + fn test_subsystem_helper_cmd_mode0_no_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.start_command_sequence(ExampleMode::Mode0, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.request_id().unwrap(), 1); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap()); + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_no_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + // Need to cache this before it is incremented, because it is incremented + // immediately in the state machine (no reply checking) + let expected_req_id = tb.helper.internal_request_id().unwrap(); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + // Assert that this was already incremented because no reply checking is necessary. + assert_eq!( + tb.helper.internal_request_id().unwrap(), + expected_req_id + 1 + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(expected_req_id); + // Second commanding step. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode0_with_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode0); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = true; + tb.start_command_sequence(ExampleMode::Mode0, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // One success reply still expected. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_with_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = true; + seq_tables.entries[1].entries[0].check_success = true; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // One success reply still expected. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + let mode_reply_ok = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target2 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_0() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = false; + seq_tables.entries[1].entries[0].check_success = false; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // One success reply still expected. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + + // Inserting the reply makes no difference: This call completes the sequence commanding. + let mode_reply_ok = GenericMessage::new( + MessageMetadata::new(expected_req_id, ExampleTargetId::Target2 as ComponentId), + ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + // The internal request ID is still cached. + tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_1() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = false; + seq_tables.entries[1].entries[0].check_success = false; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // This completes the step, so the next FSM call will perform the next step + // in sequence commanding. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + + // Inserting the reply makes no difference: Sequence command is done and target keeping + // is performed. + let mode_reply_ok = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target2 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok)).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + // The internal request ID is still cached. + tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } +} diff --git a/satrs/tests/mode_tree.rs b/satrs/tests/mode_tree.rs index 17f9836..06c1b2f 100644 --- a/satrs/tests/mode_tree.rs +++ b/satrs/tests/mode_tree.rs @@ -1,103 +1,826 @@ use core::cell::Cell; -use std::{println, sync::mpsc}; - -use satrs::mode::{ - ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler, - ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestorAndHandlerMpscBounded, - ModeRequestorBoundedMpsc, +use num_enum::TryFromPrimitive; +use satrs::dev_mgmt::{ + DevManagerCommandingHelper, DevManagerHelperResult, TransparentDevManagerHook, +}; +use satrs::mode::{ + Mode, ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler, + ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestorAndHandlerMpscBounded, + ModeRequestorOneChildBoundedMpsc, INVALID_MODE, UNKNOWN_MODE, +}; +use satrs::mode_tree::{ + connect_mode_nodes, ModeChild, ModeNode, ModeParent, ModeStoreProvider, SequenceTableEntry, + SequenceTableMapTable, TargetTableEntry, +}; +use satrs::mode_tree::{SequenceTablesMapValue, TargetTablesMapValue}; +use satrs::request::{MessageMetadata, RequestId}; +use satrs::res_code::ResultU16; +use satrs::subsystem::{ + IsChildCommandable, ModeCommandingResult, ModeTreeHelperError, ModeTreeHelperState, + StartSequenceError, SubsystemCommandingHelper, SubsystemHelperResult, }; -use satrs::request::MessageMetadata; use satrs::{ mode::{ModeAndSubmode, ModeReply, ModeRequest}, queue::GenericTargetedMessagingError, request::GenericMessage, ComponentId, }; -use std::string::{String, ToString}; +use std::borrow::{Borrow, BorrowMut}; +use std::cell::RefCell; +use std::collections::{HashMap, VecDeque}; +use std::convert::Infallible; +use std::{println, sync::mpsc}; +pub enum DefaultMode { + OFF = 0, + ON = 1, + NORMAL = 2, +} + +#[derive(Debug)] +pub enum AcsMode { + OFF = 0, + SAFE = 1, + IDLE = 2, +} + +#[derive(Debug, TryFromPrimitive)] +#[repr(u64)] pub enum TestComponentId { - Device1 = 1, - Device2 = 2, - Assembly = 3, - PusModeService = 4, + MagnetometerDevice0 = 1, + MagnetometerDevice1 = 2, + MagnetorquerDevice = 5, + ReactionWheelDevice = 6, + StartrackerDevice = 7, + MgtDevManager = 8, + ReactionWheelAssembly = 10, + MagnetometerAssembly = 11, + AcsController = 14, + AcsSubsystem = 15, + PusModeService = 16, +} + +pub type RequestSenderType = mpsc::SyncSender>; +pub type ReplySenderType = mpsc::SyncSender>; + +#[derive(Default, Debug)] +pub struct ModeRequestHandlerMock { + pub id: ComponentId, + get_mode_calls: RefCell, + start_transition_calls: VecDeque<(MessageMetadata, ModeAndSubmode)>, + announce_mode_calls: RefCell>, + handle_mode_info_calls: VecDeque<(MessageMetadata, ModeAndSubmode)>, + handle_mode_reached_calls: RefCell>>, + send_mode_reply_calls: RefCell>, +} + +impl ModeRequestHandlerMock { + pub fn new(id: ComponentId) -> Self { + Self { + id, + ..Default::default() + } + } +} + +impl ModeRequestHandlerMock { + pub fn clear(&mut self) { + self.get_mode_calls.replace(0); + self.start_transition_calls.clear(); + self.announce_mode_calls.borrow_mut().clear(); + self.handle_mode_reached_calls.borrow_mut().clear(); + self.handle_mode_info_calls.clear(); + self.send_mode_reply_calls.borrow_mut().clear(); + } + pub fn mode_messages_received(&self) -> usize { + *self.get_mode_calls.borrow() + + self.start_transition_calls.borrow().len() + + self.announce_mode_calls.borrow().len() + + self.handle_mode_info_calls.borrow().len() + + self.handle_mode_reached_calls.borrow().len() + + self.send_mode_reply_calls.borrow().len() + } +} + +impl ModeProvider for ModeRequestHandlerMock { + fn mode_and_submode(&self) -> ModeAndSubmode { + *self.get_mode_calls.borrow_mut() += 1; + INVALID_MODE + } +} + +impl ModeRequestHandler for ModeRequestHandlerMock { + type Error = Infallible; + + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + _forced: bool, + ) -> Result<(), Self::Error> { + self.start_transition_calls + .push_back((requestor, mode_and_submode)); + Ok(()) + } + + fn announce_mode(&self, requestor_info: Option, recursive: bool) { + self.announce_mode_calls + .borrow_mut() + .push_back(AnnounceModeInfo { + requestor: requestor_info, + recursive, + }); + } + + fn handle_mode_reached( + &mut self, + requestor_info: Option, + ) -> Result<(), Self::Error> { + self.handle_mode_reached_calls + .borrow_mut() + .push_back(requestor_info); + Ok(()) + } + + fn handle_mode_info( + &mut self, + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + self.handle_mode_info_calls + .push_back((requestor_info, info)); + todo!() + } + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + self.send_mode_reply_calls + .borrow_mut() + .push_back((requestor_info, reply)); + Ok(()) + } +} + +#[derive(Debug)] +pub struct ModeReplyHandlerMock { + pub id: ComponentId, + mode_info_messages: VecDeque<(MessageMetadata, ModeAndSubmode)>, + mode_reply_messages: VecDeque<(MessageMetadata, ModeAndSubmode)>, + cant_reach_mode_messages: VecDeque<(MessageMetadata, ResultU16)>, + wrong_mode_messages: VecDeque<(MessageMetadata, ModeAndSubmode, ModeAndSubmode)>, +} + +impl ModeReplyHandlerMock { + pub fn new(id: ComponentId) -> Self { + Self { + id, + mode_info_messages: Default::default(), + mode_reply_messages: Default::default(), + cant_reach_mode_messages: Default::default(), + wrong_mode_messages: Default::default(), + } + } + + pub fn num_of_received_mode_replies(&self) -> usize { + self.mode_info_messages.len() + + self.mode_reply_messages.len() + + self.cant_reach_mode_messages.len() + + self.wrong_mode_messages.len() + } + + pub fn handle_mode_reply(&mut self, request: &GenericMessage) { + match request.message { + ModeReply::ModeInfo(mode_and_submode) => { + self.mode_info_messages + .push_back((request.requestor_info, mode_and_submode)); + } + ModeReply::ModeReply(mode_and_submode) => { + self.mode_reply_messages + .push_back((request.requestor_info, mode_and_submode)); + } + ModeReply::CantReachMode(result_u16) => { + self.cant_reach_mode_messages + .push_back((request.requestor_info, result_u16)); + } + ModeReply::WrongMode { expected, reached } => { + self.wrong_mode_messages + .push_back((request.requestor_info, expected, reached)); + } + } + } } struct PusModeService { pub request_id_counter: Cell, - pub mode_node: ModeRequestorBoundedMpsc, + pub mode_reply_mock: ModeReplyHandlerMock, + mode_node: ModeRequestorOneChildBoundedMpsc, } impl PusModeService { - pub fn send_announce_mode_cmd_to_assy(&self) { + pub fn new(init_req_count: u32, mode_node: ModeRequestorOneChildBoundedMpsc) -> Self { + Self { + request_id_counter: Cell::new(init_req_count), + mode_reply_mock: ModeReplyHandlerMock::new( + TestComponentId::PusModeService as ComponentId, + ), + mode_node, + } + } + pub fn run(&mut self) { + while let Some(reply) = self.mode_node.try_recv_mode_reply().unwrap() { + self.mode_reply_mock.handle_mode_reply(&reply); + } + } + + pub fn announce_modes_recursively(&self) { self.mode_node .send_mode_request( self.request_id_counter.get(), - TestComponentId::Assembly as ComponentId, + TestComponentId::AcsSubsystem as ComponentId, ModeRequest::AnnounceModeRecursive, ) .unwrap(); self.request_id_counter .replace(self.request_id_counter.get() + 1); } + + pub fn send_mode_cmd(&self, mode: ModeAndSubmode) -> RequestId { + let request_id = self.request_id_counter.get(); + self.mode_node + .send_mode_request( + request_id, + TestComponentId::AcsSubsystem as ComponentId, + ModeRequest::SetMode { + mode_and_submode: mode, + forced: false, + }, + ) + .unwrap(); + self.request_id_counter.replace(request_id + 1); + request_id + } } -struct TestDevice { - pub name: String, - pub mode_node: ModeRequestHandlerMpscBounded, - pub mode_and_submode: ModeAndSubmode, +impl ModeNode for PusModeService { + fn id(&self) -> ComponentId { + TestComponentId::PusModeService as ComponentId + } } -impl TestDevice { - pub fn run(&mut self) { - self.check_mode_requests().expect("mode messaging error"); +impl ModeParent for PusModeService { + type Sender = RequestSenderType; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) { + self.mode_node.add_message_target(id, request_sender); + } +} + +#[derive(Debug, Default)] +struct IsCommandableDummy {} + +impl IsChildCommandable for IsCommandableDummy { + fn is_commandable(&self, _id: ComponentId) -> bool { + true + } +} + +struct AcsSubsystem { + pub mode_node: ModeRequestorAndHandlerMpscBounded, + pub mode_requestor_info: Option, + pub target_mode_and_submode: Option, + pub subsystem_helper: SubsystemCommandingHelper, + is_commandable_dummy: IsCommandableDummy, + pub mode_req_mock: ModeRequestHandlerMock, + pub mode_reply_mock: ModeReplyHandlerMock, +} + +impl AcsSubsystem { + pub fn id() -> ComponentId { + TestComponentId::AcsSubsystem as u64 } - pub fn check_mode_requests(&mut self) -> Result<(), ModeError> { - if let Some(request) = self.mode_node.try_recv_mode_request()? { - self.handle_mode_request(request)? + pub fn new(mode_node: ModeRequestorAndHandlerMpscBounded) -> Self { + Self { + mode_node, + mode_requestor_info: None, + target_mode_and_submode: None, + is_commandable_dummy: IsCommandableDummy::default(), + subsystem_helper: SubsystemCommandingHelper::default(), + mode_req_mock: ModeRequestHandlerMock::new(Self::id()), + mode_reply_mock: ModeReplyHandlerMock::new(Self::id()), + } + } + + pub fn get_num_mode_requests(&mut self) -> usize { + self.mode_req_mock.mode_messages_received() + } + + pub fn handle_subsystem_helper_result( + &mut self, + result: Result, + ) { + match result { + Ok(result) => { + if let SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) = result { + self.handle_mode_reached(self.mode_requestor_info) + .expect("mode reply handling failed"); + } + } + Err(error) => match error { + ModeTreeHelperError::Message(_generic_targeted_messaging_error) => { + panic!("messaging error") + } + ModeTreeHelperError::CurrentModeNotInTargetTable(_) => panic!("mode not found"), + ModeTreeHelperError::ModeCommmandFailure { seq_table_index: _ } => { + // TODO: Cache the command failure. + } + ModeTreeHelperError::TargetKeepingViolation { fallback_mode } => { + if let Some(fallback_mode) = fallback_mode { + self.subsystem_helper + .start_command_sequence(fallback_mode, 0) + .unwrap(); + } + } + }, + } + } + + pub fn run(&mut self) { + while let Some(request) = self.mode_node.try_recv_mode_request().unwrap() { + self.handle_mode_request(request) + .expect("mode messaging error"); + } + + let mut received_reply = false; + while let Some(mode_reply) = self.mode_node.try_recv_mode_reply().unwrap() { + received_reply = true; + self.mode_reply_mock.handle_mode_reply(&mode_reply); + let result = self.subsystem_helper.state_machine( + Some(mode_reply), + &self.mode_node, + &self.is_commandable_dummy, + ); + self.handle_subsystem_helper_result(result); + } + if !received_reply { + let result = self.subsystem_helper.state_machine( + None, + &self.mode_node, + &self.is_commandable_dummy, + ); + self.handle_subsystem_helper_result(result); + } + } + + pub fn add_target_and_sequence_table( + &mut self, + mode: Mode, + target_table_val: TargetTablesMapValue, + sequence_table_val: SequenceTablesMapValue, + ) { + self.subsystem_helper.add_target_and_sequence_table( + mode, + target_table_val, + sequence_table_val, + ); + } +} + +impl ModeNode for AcsSubsystem { + fn id(&self) -> ComponentId { + Self::id() + } +} + +impl ModeParent for AcsSubsystem { + type Sender = RequestSenderType; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) { + self.subsystem_helper.add_mode_child(id, UNKNOWN_MODE); + self.mode_node.add_request_target(id, request_sender); + } +} + +impl ModeChild for AcsSubsystem { + type Sender = ReplySenderType; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) { + self.mode_node.add_reply_target(id, reply_sender); + } +} + +impl ModeProvider for AcsSubsystem { + fn mode_and_submode(&self) -> ModeAndSubmode { + ModeAndSubmode::new(self.subsystem_helper.mode(), 0) + } +} + +#[derive(Debug, thiserror::Error)] +pub enum SubsystemModeError { + #[error("messaging error: {0:?}")] + Mode(#[from] ModeError), + #[error("start sequence error: {0}")] + StartError(#[from] StartSequenceError), +} + +impl ModeRequestHandler for AcsSubsystem { + type Error = SubsystemModeError; + + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + forced: bool, + ) -> Result<(), Self::Error> { + if !forced && self.subsystem_helper.state() == ModeTreeHelperState::ModeCommanding { + return Err(ModeError::Busy.into()); + } + self.mode_requestor_info = Some(requestor); + self.target_mode_and_submode = Some(mode_and_submode); + self.mode_req_mock + .start_transition(requestor, mode_and_submode, forced) + .unwrap(); + println!("subsystem: mode req with ID {}", requestor.request_id()); + self.subsystem_helper + .start_command_sequence(mode_and_submode.mode(), requestor.request_id())?; + Ok(()) + } + + fn announce_mode(&self, requestor_info: Option, recursive: bool) { + println!( + "TestAssembly: Announcing mode (recursively: {}): {:?}", + recursive, + self.subsystem_helper.mode() + ); + let request_id = requestor_info.map_or(0, |info| info.request_id()); + self.subsystem_helper + .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive) + .expect("sending mode request failed"); + self.mode_req_mock.announce_mode(requestor_info, recursive); + } + + fn handle_mode_reached( + &mut self, + requestor_info: Option, + ) -> Result<(), Self::Error> { + self.mode_req_mock + .handle_mode_reached(requestor_info) + .unwrap(); + if let Some(requestor) = requestor_info { + self.send_mode_reply( + requestor, + ModeReply::ModeReply(ModeAndSubmode::new(self.subsystem_helper.mode(), 0)), + )?; + } + Ok(()) + } + + fn handle_mode_info( + &mut self, + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + self.mode_req_mock + .handle_mode_info(requestor_info, info) + .unwrap(); + // TODO: Need to check whether mode table execution is finished. + // This works by checking the children modes received through replies against the + // mode table after all transition tables were executed. + Ok(()) + } + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + self.mode_req_mock + .send_mode_reply(requestor_info, reply) + .unwrap(); + self.mode_node + .send_mode_reply(requestor_info, reply) + .map_err(ModeError::Send)?; + Ok(()) + } +} + +struct MgmAssembly { + pub mode_node: ModeRequestorAndHandlerMpscBounded, + pub mode_requestor_info: Option, + pub mode_and_submode: ModeAndSubmode, + pub commanding_helper: DevManagerCommandingHelper, + pub mode_req_mock: ModeRequestHandlerMock, + pub mode_reply_mock: ModeReplyHandlerMock, +} + +impl MgmAssembly { + pub fn id() -> ComponentId { + TestComponentId::MagnetometerAssembly as u64 + } + pub fn new(mode_node: ModeRequestorAndHandlerMpscBounded) -> Self { + Self { + mode_node, + mode_requestor_info: None, + mode_and_submode: UNKNOWN_MODE, + commanding_helper: DevManagerCommandingHelper::new(TransparentDevManagerHook::default()), + mode_req_mock: ModeRequestHandlerMock::new(Self::id()), + mode_reply_mock: ModeReplyHandlerMock::new(Self::id()), + } + } + + pub fn run(&mut self) { + self.check_mode_requests().expect("mode messaging error"); + self.check_mode_replies().expect("mode messaging error"); + } + + pub fn get_num_mode_requests(&mut self) -> usize { + self.mode_req_mock.mode_messages_received() + } + + pub fn check_mode_requests(&mut self) -> Result<(), GenericTargetedMessagingError> { + while let Some(request) = self.mode_node.try_recv_mode_request()? { + self.handle_mode_request(request).unwrap(); + } + Ok(()) + } + + pub fn check_mode_replies(&mut self) -> Result<(), ModeError> { + while let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? { + self.mode_reply_mock.handle_mode_reply(&reply_and_id); + match self.commanding_helper.handle_mode_reply(&reply_and_id) { + Ok(result) => { + if let DevManagerHelperResult::ModeCommandingDone(context) = result { + // Complete the mode command. + self.mode_and_submode = context.target_mode; + self.handle_mode_reached(self.mode_requestor_info)?; + } + } + Err(err) => match err { + satrs::dev_mgmt::DevManagerHelperError::ChildNotInStore => todo!(), + }, + } } Ok(()) } } -impl ModeProvider for TestDevice { +impl ModeNode for MgmAssembly { + fn id(&self) -> ComponentId { + Self::id() + } +} +impl ModeParent for MgmAssembly { + type Sender = RequestSenderType; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) { + self.mode_node.add_request_target(id, request_sender); + self.commanding_helper.add_mode_child(id, UNKNOWN_MODE); + } +} + +impl ModeChild for MgmAssembly { + type Sender = ReplySenderType; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) { + self.mode_node.add_reply_target(id, reply_sender); + } +} + +impl ModeProvider for MgmAssembly { fn mode_and_submode(&self) -> ModeAndSubmode { self.mode_and_submode } } -impl ModeRequestHandler for TestDevice { +impl ModeRequestHandler for MgmAssembly { + type Error = ModeError; + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + forced: bool, + ) -> Result<(), Self::Error> { + // Always accept forced commands and commands to mode OFF. + if self.commanding_helper.target_mode().is_some() + && !forced + && mode_and_submode.mode() != DefaultMode::OFF as u32 + { + return Err(ModeError::Busy); + } + self.mode_requestor_info = Some(requestor); + self.mode_req_mock + .start_transition(requestor, mode_and_submode, forced) + .unwrap(); + self.commanding_helper.send_mode_cmd_to_all_children( + requestor.request_id(), + mode_and_submode, + forced, + &self.mode_node, + )?; + Ok(()) + } + + fn announce_mode(&self, requestor_info: Option, recursive: bool) { + println!( + "TestAssembly: Announcing mode (recursively: {}): {:?}", + recursive, self.mode_and_submode + ); + let request_id = requestor_info.map_or(0, |info| info.request_id()); + self.commanding_helper + .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive) + .expect("sending mode request failed"); + self.mode_req_mock.announce_mode(requestor_info, recursive); + } + + fn handle_mode_reached( + &mut self, + mode_requestor: Option, + ) -> Result<(), Self::Error> { + if let Some(requestor) = mode_requestor { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?; + } + self.mode_req_mock + .handle_mode_reached(mode_requestor) + .unwrap(); + Ok(()) + } + + fn handle_mode_info( + &mut self, + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + self.mode_req_mock + .handle_mode_info(requestor_info, info) + .unwrap(); + // TODO: A proper assembly must reach to mode changes of its children.. + Ok(()) + } + + fn send_mode_reply( + &self, + requestor: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + self.mode_node.send_mode_reply(requestor, reply)?; + self.mode_req_mock + .send_mode_reply(requestor, reply) + .unwrap(); + Ok(()) + } +} + +struct DeviceManager { + name: &'static str, + pub id: ComponentId, + pub commanding_helper: DevManagerCommandingHelper, + pub mode_node: ModeRequestorAndHandlerMpscBounded, + pub mode_requestor_info: Option, + pub mode_and_submode: ModeAndSubmode, + pub mode_req_mock: ModeRequestHandlerMock, + pub mode_reply_mock: ModeReplyHandlerMock, +} + +impl DeviceManager { + pub fn new( + name: &'static str, + id: ComponentId, + mode_node: ModeRequestorAndHandlerMpscBounded, + ) -> Self { + Self { + name, + id, + mode_node, + mode_requestor_info: None, + commanding_helper: DevManagerCommandingHelper::new(TransparentDevManagerHook::default()), + mode_and_submode: UNKNOWN_MODE, + mode_req_mock: ModeRequestHandlerMock::new(id), + mode_reply_mock: ModeReplyHandlerMock::new(id), + } + } + + pub fn get_num_mode_requests(&mut self) -> usize { + self.mode_req_mock.mode_messages_received() + } + + pub fn run(&mut self) { + self.check_mode_requests().expect("mode messaging error"); + self.check_mode_replies().expect("mode reply error"); + } + + pub fn check_mode_requests(&mut self) -> Result<(), ModeError> { + while let Some(request) = self.mode_node.try_recv_mode_request()? { + self.handle_mode_request(request)? + } + Ok(()) + } + + pub fn check_mode_replies(&mut self) -> Result<(), ModeError> { + while let Some(reply) = self.mode_node.try_recv_mode_reply()? { + self.handle_mode_reply(&reply)?; + } + Ok(()) + } + + pub fn handle_mode_reply( + &mut self, + mode_reply: &GenericMessage, + ) -> Result<(), ModeError> { + self.mode_reply_mock.handle_mode_reply(mode_reply); + match self.commanding_helper.handle_mode_reply(mode_reply) { + Ok(result) => { + match result { + DevManagerHelperResult::Idle => todo!(), + DevManagerHelperResult::Busy => todo!(), + DevManagerHelperResult::ModeCommandingDone(context) => { + // Complete the mode command. + self.handle_mode_reached(self.mode_requestor_info)?; + self.mode_and_submode = context.target_mode; + } + } + } + Err(e) => match e { + satrs::dev_mgmt::DevManagerHelperError::ChildNotInStore => todo!(), + }, + } + Ok(()) + } +} + +impl ModeNode for DeviceManager { + fn id(&self) -> ComponentId { + self.id + } +} + +impl ModeChild for DeviceManager { + type Sender = ReplySenderType; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) { + self.mode_node.add_reply_target(id, reply_sender); + } +} + +impl ModeParent for DeviceManager { + type Sender = RequestSenderType; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) { + self.commanding_helper + .children_mode_store + .add_component(id, UNKNOWN_MODE); + self.mode_node.add_request_target(id, request_sender); + } +} + +impl ModeProvider for DeviceManager { + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +impl ModeRequestHandler for DeviceManager { type Error = ModeError; fn start_transition( &mut self, requestor: MessageMetadata, mode_and_submode: ModeAndSubmode, + forced: bool, ) -> Result<(), ModeError> { self.mode_and_submode = mode_and_submode; - self.handle_mode_reached(Some(requestor))?; + self.mode_requestor_info = Some(requestor); + self.mode_req_mock + .start_transition(requestor, mode_and_submode, forced) + .unwrap(); + self.commanding_helper.send_mode_cmd_to_all_children( + requestor.request_id(), + mode_and_submode, + forced, + &self.mode_node, + )?; Ok(()) } - fn announce_mode(&self, _requestor_info: Option, _recursive: bool) { + fn announce_mode(&self, requestor_info: Option, recursive: bool) { println!( "{}: announcing mode: {:?}", self.name, self.mode_and_submode ); + let request_id = requestor_info.map_or(0, |info| info.request_id()); + self.commanding_helper + .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive) + .expect("sending mode announce request failed"); + self.mode_req_mock.announce_mode(requestor_info, recursive); } fn handle_mode_reached(&mut self, requestor: Option) -> Result<(), ModeError> { if let Some(requestor) = requestor { self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?; } - Ok(()) - } - fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - reply: ModeReply, - ) -> Result<(), ModeError> { - self.mode_node.send_mode_reply(requestor_info, reply)?; + self.mode_req_mock.handle_mode_reached(requestor).unwrap(); Ok(()) } @@ -113,246 +836,775 @@ impl ModeRequestHandler for TestDevice { requestor_info.sender_id(), info ); - Ok(()) - } -} - -struct TestAssembly { - pub mode_node: ModeRequestorAndHandlerMpscBounded, - pub mode_requestor_info: Option, - pub mode_and_submode: ModeAndSubmode, - pub target_mode_and_submode: Option, -} - -impl ModeProvider for TestAssembly { - fn mode_and_submode(&self) -> ModeAndSubmode { - self.mode_and_submode - } -} - -impl TestAssembly { - pub fn run(&mut self) { - self.check_mode_requests().expect("mode messaging error"); - self.check_mode_replies().expect("mode messaging error"); - } - - pub fn check_mode_requests(&mut self) -> Result<(), GenericTargetedMessagingError> { - if let Some(request) = self.mode_node.try_recv_mode_request()? { - match request.message { - ModeRequest::SetMode(mode_and_submode) => { - self.start_transition(request.requestor_info, mode_and_submode) - .unwrap(); - } - ModeRequest::ReadMode => self - .mode_node - .send_mode_reply( - request.requestor_info, - ModeReply::ModeReply(self.mode_and_submode), - ) - .unwrap(), - ModeRequest::AnnounceMode => { - self.announce_mode(Some(request.requestor_info), false) - } - ModeRequest::AnnounceModeRecursive => { - self.announce_mode(Some(request.requestor_info), true) - } - ModeRequest::ModeInfo(_) => todo!(), - } - } - Ok(()) - } - - pub fn check_mode_replies(&mut self) -> Result<(), GenericTargetedMessagingError> { - if let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? { - match reply_and_id.message { - ModeReply::ModeReply(reply) => { - println!( - "TestAssembly: Received mode reply from {:?}, reached: {:?}", - reply_and_id.sender_id(), - reply - ); - } - ModeReply::CantReachMode(_) => todo!(), - ModeReply::WrongMode { expected, reached } => { - println!( - "TestAssembly: Wrong mode reply from {:?}, reached {:?}, expected {:?}", - reply_and_id.sender_id(), - reached, - expected - ); - } - } - } - Ok(()) - } -} - -impl ModeRequestHandler for TestAssembly { - type Error = ModeError; - fn start_transition( - &mut self, - requestor: MessageMetadata, - mode_and_submode: ModeAndSubmode, - ) -> Result<(), Self::Error> { - self.mode_requestor_info = Some(requestor); - self.target_mode_and_submode = Some(mode_and_submode); - Ok(()) - } - - fn announce_mode(&self, requestor_info: Option, recursive: bool) { - println!( - "TestAssembly: Announcing mode (recursively: {}): {:?}", - recursive, self.mode_and_submode - ); - // self.mode_requestor_info = Some((request_id, sender_id)); - let mut mode_request = ModeRequest::AnnounceMode; - if recursive { - mode_request = ModeRequest::AnnounceModeRecursive; - } - let request_id = requestor_info.map_or(0, |info| info.request_id()); - self.mode_node - .request_sender_map - .0 - .iter() - .for_each(|(_, sender)| { - sender - .send(GenericMessage::new( - MessageMetadata::new(request_id, self.mode_node.local_channel_id_generic()), - mode_request, - )) - .expect("sending mode request failed"); - }); - } - - fn handle_mode_reached( - &mut self, - mode_requestor: Option, - ) -> Result<(), Self::Error> { - if let Some(requestor) = mode_requestor { - self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?; - } + self.mode_req_mock + .handle_mode_info(requestor_info, info) + .unwrap(); Ok(()) } fn send_mode_reply( &self, - requestor: MessageMetadata, + requestor_info: MessageMetadata, reply: ModeReply, - ) -> Result<(), Self::Error> { - self.mode_node.send_mode_reply(requestor, reply)?; + ) -> Result<(), ModeError> { + self.mode_node.send_mode_reply(requestor_info, reply)?; + self.mode_req_mock + .send_mode_reply(requestor_info, reply) + .unwrap(); + Ok(()) + } +} + +struct CommonDevice { + name: &'static str, + pub id: ComponentId, + pub mode_node: ModeRequestHandlerMpscBounded, + pub mode_and_submode: ModeAndSubmode, + pub mode_req_mock: ModeRequestHandlerMock, +} + +impl CommonDevice { + pub fn new( + name: &'static str, + id: ComponentId, + mode_node: ModeRequestHandlerMpscBounded, + ) -> Self { + Self { + name, + id, + mode_node, + mode_and_submode: UNKNOWN_MODE, + mode_req_mock: ModeRequestHandlerMock::new(id), + } + } + + pub fn run(&mut self) { + self.check_mode_requests().expect("mode messaging error"); + } + + pub fn check_mode_requests(&mut self) -> Result<(), ModeError> { + if let Some(request) = self.mode_node.try_recv_mode_request()? { + self.handle_mode_request(request)? + } + Ok(()) + } + + pub fn get_and_clear_num_mode_msgs(&mut self) -> usize { + self.mode_req_mock.mode_messages_received() + } +} + +impl ModeNode for CommonDevice { + fn id(&self) -> ComponentId { + self.id + } +} + +impl ModeChild for CommonDevice { + type Sender = ReplySenderType; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) { + self.mode_node.add_message_target(id, reply_sender); + } +} + +impl ModeProvider for CommonDevice { + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +impl ModeRequestHandler for CommonDevice { + type Error = ModeError; + + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + forced: bool, + ) -> Result<(), ModeError> { + if self.id() == TestComponentId::MagnetorquerDevice as u64 { + println!("test"); + } + self.mode_and_submode = mode_and_submode; + self.handle_mode_reached(Some(requestor))?; + self.mode_req_mock + .start_transition(requestor, mode_and_submode, forced) + .unwrap(); + Ok(()) + } + + fn announce_mode(&self, requestor_info: Option, recursive: bool) { + println!( + "{}: announcing mode: {:?}", + self.name, self.mode_and_submode + ); + self.mode_req_mock.announce_mode(requestor_info, recursive); + } + + fn handle_mode_reached(&mut self, requestor: Option) -> Result<(), ModeError> { + if let Some(requestor) = requestor { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?; + } + self.mode_req_mock.handle_mode_reached(requestor).unwrap(); Ok(()) } fn handle_mode_info( &mut self, - _requestor_info: MessageMetadata, - _info: ModeAndSubmode, - ) -> Result<(), Self::Error> { - // TODO: A proper assembly must reach to mode changes of its children.. + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), ModeError> { + // A device is a leaf in the tree.. so this really should not happen + println!( + "{}: unexpected mode info from {:?} with mode: {:?}", + self.name, + requestor_info.sender_id(), + info + ); + self.mode_req_mock + .handle_mode_info(requestor_info, info) + .unwrap(); + Ok(()) + } + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), ModeError> { + self.mode_node.send_mode_reply(requestor_info, reply)?; + self.mode_req_mock + .send_mode_reply(requestor_info, reply) + .unwrap(); Ok(()) } } -fn main() { - // All request channel handles. - let (request_sender_to_dev1, request_receiver_dev1) = mpsc::sync_channel(10); - let (request_sender_to_dev2, request_receiver_dev2) = mpsc::sync_channel(10); - let (request_sender_to_assy, request_receiver_assy) = mpsc::sync_channel(10); - - // All reply channel handles. - let (reply_sender_to_assy, reply_receiver_assy) = mpsc::sync_channel(10); - let (reply_sender_to_pus, reply_receiver_pus) = mpsc::sync_channel(10); - - // Mode requestors and handlers. - let mut mode_node_assy = ModeRequestorAndHandlerMpscBounded::new( - TestComponentId::Assembly as ComponentId, - request_receiver_assy, - reply_receiver_assy, - ); - // Mode requestors only. - let mut mode_node_pus = ModeRequestorBoundedMpsc::new( - TestComponentId::PusModeService as ComponentId, - reply_receiver_pus, - ); - - // Request handlers only. - let mut mode_node_dev1 = ModeRequestHandlerMpscBounded::new( - TestComponentId::Device1 as ComponentId, - request_receiver_dev1, - ); - let mut mode_node_dev2 = ModeRequestHandlerMpscBounded::new( - TestComponentId::Device2 as ComponentId, - request_receiver_dev2, - ); - - // Set up mode request senders first. - mode_node_pus.add_message_target( - TestComponentId::Assembly as ComponentId, - request_sender_to_assy, - ); - mode_node_pus.add_message_target( - TestComponentId::Device1 as ComponentId, - request_sender_to_dev1.clone(), - ); - mode_node_pus.add_message_target( - TestComponentId::Device2 as ComponentId, - request_sender_to_dev2.clone(), - ); - mode_node_assy.add_request_target( - TestComponentId::Device1 as ComponentId, - request_sender_to_dev1, - ); - mode_node_assy.add_request_target( - TestComponentId::Device2 as ComponentId, - request_sender_to_dev2, - ); - - // Set up mode reply senders. - mode_node_dev1.add_message_target( - TestComponentId::Assembly as ComponentId, - reply_sender_to_assy.clone(), - ); - mode_node_dev1.add_message_target( - TestComponentId::PusModeService as ComponentId, - reply_sender_to_pus.clone(), - ); - mode_node_dev2.add_message_target( - TestComponentId::Assembly as ComponentId, - reply_sender_to_assy, - ); - mode_node_dev2.add_message_target( - TestComponentId::PusModeService as ComponentId, - reply_sender_to_pus.clone(), - ); - mode_node_assy.add_reply_target( - TestComponentId::PusModeService as ComponentId, - reply_sender_to_pus, - ); - - let mut device1 = TestDevice { - name: "Test Device 1".to_string(), - mode_node: mode_node_dev1, - mode_and_submode: ModeAndSubmode::new(0, 0), - }; - let mut device2 = TestDevice { - name: "Test Device 2".to_string(), - mode_node: mode_node_dev2, - mode_and_submode: ModeAndSubmode::new(0, 0), - }; - let mut assy = TestAssembly { - mode_node: mode_node_assy, - mode_requestor_info: None, - mode_and_submode: ModeAndSubmode::new(0, 0), - target_mode_and_submode: None, - }; - let pus_service = PusModeService { - request_id_counter: Cell::new(0), - mode_node: mode_node_pus, - }; - - pus_service.send_announce_mode_cmd_to_assy(); - assy.run(); - device1.run(); - device2.run(); - assy.run(); +#[derive(Debug, Default)] +pub struct AnnounceModeInfo { + pub requestor: Option, + pub recursive: bool, +} + +pub struct AcsController { + pub mode_node: ModeRequestHandlerMpscBounded, + pub mode_and_submode: ModeAndSubmode, + pub announce_mode_queue: RefCell>, + pub mode_req_mock: ModeRequestHandlerMock, +} + +impl AcsController { + pub fn id() -> ComponentId { + TestComponentId::AcsController as u64 + } + pub fn new(mode_node: ModeRequestHandlerMpscBounded) -> Self { + Self { + mode_node, + mode_and_submode: UNKNOWN_MODE, + announce_mode_queue: Default::default(), + mode_req_mock: ModeRequestHandlerMock::new(Self::id()), + } + } + pub fn run(&mut self) { + self.check_mode_requests().expect("mode messaging error"); + } + + pub fn check_mode_requests(&mut self) -> Result<(), ModeError> { + if let Some(request) = self.mode_node.try_recv_mode_request()? { + self.handle_mode_request(request)? + } + Ok(()) + } +} + +impl ModeNode for AcsController { + fn id(&self) -> ComponentId { + TestComponentId::AcsController as u64 + } +} + +impl ModeChild for AcsController { + type Sender = ReplySenderType; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) { + self.mode_node.add_message_target(id, reply_sender); + } +} + +impl ModeProvider for AcsController { + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +impl ModeRequestHandler for AcsController { + type Error = ModeError; + + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + forced: bool, + ) -> Result<(), Self::Error> { + self.mode_and_submode = mode_and_submode; + self.handle_mode_reached(Some(requestor))?; + self.mode_req_mock + .start_transition(requestor, mode_and_submode, forced) + .unwrap(); + Ok(()) + } + + fn announce_mode(&self, requestor_info: Option, recursive: bool) { + self.announce_mode_queue + .borrow_mut() + .push_back(AnnounceModeInfo { + requestor: requestor_info, + recursive, + }); + self.mode_req_mock.announce_mode(requestor_info, recursive); + } + + fn handle_mode_reached( + &mut self, + requestor_info: Option, + ) -> Result<(), Self::Error> { + if let Some(requestor) = requestor_info { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?; + } + self.mode_req_mock + .handle_mode_reached(requestor_info) + .unwrap(); + Ok(()) + } + + fn handle_mode_info( + &mut self, + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + // The controller is a leaf in the tree.. so this really should not happen + println!( + "ACS Controller: unexpected mode info from {:?} with mode: {:?}", + requestor_info.sender_id(), + info + ); + self.mode_req_mock + .handle_mode_info(requestor_info, info) + .unwrap(); + Ok(()) + } + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + self.mode_node.send_mode_reply(requestor_info, reply)?; + self.mode_req_mock + .send_mode_reply(requestor_info, reply) + .unwrap(); + Ok(()) + } +} + +pub struct TreeTestbench { + pus: PusModeService, + subsystem: AcsSubsystem, + ctrl: AcsController, + mgt_manager: DeviceManager, + mgm_assy: MgmAssembly, + mgm_devs: [CommonDevice; 2], + mgt_dev: CommonDevice, +} + +impl Default for TreeTestbench { + fn default() -> Self { + Self::new() + } +} + +impl TreeTestbench { + pub fn new() -> Self { + // All request channel handles. + let (request_sender_to_mgm_dev_0, request_receiver_mgm_dev_0) = mpsc::sync_channel(10); + let (request_sender_to_mgm_dev_1, request_receiver_mgm_dev_1) = mpsc::sync_channel(10); + let (request_sender_to_mgt_dev, request_receiver_mgt_dev) = mpsc::sync_channel(10); + let (request_sender_to_mgt_man, request_receiver_mgt_man) = mpsc::sync_channel(10); + let (request_sender_to_mgm_assy, request_receiver_mgm_assy) = mpsc::sync_channel(10); + let (request_sender_to_acs_subsystem, request_receiver_acs_subsystem) = + mpsc::sync_channel(10); + let (request_sender_to_acs_ctrl, request_receiver_acs_ctrl) = mpsc::sync_channel(10); + + // All reply channel handles. + let (reply_sender_to_mgm_assy, reply_receiver_mgm_assy) = mpsc::sync_channel(10); + let (reply_sender_to_acs_subsystem, reply_receiver_acs_subsystem) = mpsc::sync_channel(10); + let (reply_sender_to_pus, reply_receiver_pus) = mpsc::sync_channel(10); + let (reply_sender_to_mgt_man, reply_receiver_mgt_man) = mpsc::sync_channel(10); + + // Mode requestors only. + let mode_node_pus = ModeRequestorOneChildBoundedMpsc::new( + TestComponentId::PusModeService as ComponentId, + reply_receiver_pus, + ); + + // Mode requestors and handlers. + let mgm_assy_node = ModeRequestorAndHandlerMpscBounded::new( + TestComponentId::MagnetometerAssembly as ComponentId, + request_receiver_mgm_assy, + reply_receiver_mgm_assy, + ); + let mgt_dev_mgmt_node = ModeRequestorAndHandlerMpscBounded::new( + TestComponentId::MgtDevManager as ComponentId, + request_receiver_mgt_man, + reply_receiver_mgt_man, + ); + let acs_subsystem_node = ModeRequestorAndHandlerMpscBounded::new( + TestComponentId::AcsSubsystem as ComponentId, + request_receiver_acs_subsystem, + reply_receiver_acs_subsystem, + ); + + // Request handlers only. + let mgm_dev_node_0 = ModeRequestHandlerMpscBounded::new( + TestComponentId::MagnetometerDevice0 as ComponentId, + request_receiver_mgm_dev_0, + ); + let mgm_dev_node_1 = ModeRequestHandlerMpscBounded::new( + TestComponentId::MagnetometerDevice1 as ComponentId, + request_receiver_mgm_dev_1, + ); + let mgt_dev_node = ModeRequestHandlerMpscBounded::new( + TestComponentId::MagnetorquerDevice as ComponentId, + request_receiver_mgt_dev, + ); + let acs_ctrl_node = ModeRequestHandlerMpscBounded::new( + TestComponentId::AcsController as ComponentId, + request_receiver_acs_ctrl, + ); + + let mut mgm_dev_0 = CommonDevice::new( + "MGM_0", + TestComponentId::MagnetometerDevice0 as u64, + mgm_dev_node_0, + ); + let mut mgm_dev_1 = CommonDevice::new( + "MGM_1", + TestComponentId::MagnetometerDevice1 as u64, + mgm_dev_node_1, + ); + let mut mgt_dev = CommonDevice::new( + "MGT", + TestComponentId::MagnetorquerDevice as u64, + mgt_dev_node, + ); + let mut mgt_manager = DeviceManager::new( + "MGT_MANAGER", + TestComponentId::MgtDevManager as u64, + mgt_dev_mgmt_node, + ); + let mut mgm_assy = MgmAssembly::new(mgm_assy_node); + let mut acs_subsystem = AcsSubsystem::new(acs_subsystem_node); + let mut acs_ctrl = AcsController::new(acs_ctrl_node); + let mut pus_service = PusModeService::new(1, mode_node_pus); + + // ACS subsystem tables + let mut target_table_safe = TargetTablesMapValue::new("SAFE_TARGET_TBL", None); + target_table_safe.add_entry(TargetTableEntry::new( + "CTRL_SAFE", + TestComponentId::AcsController as u64, + ModeAndSubmode::new(AcsMode::SAFE as u32, 0), + // All submodes allowed. + Some(0xffff), + )); + target_table_safe.add_entry(TargetTableEntry::new_with_precise_submode( + "MGM_A_NML", + TestComponentId::MagnetometerAssembly as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + )); + target_table_safe.add_entry(TargetTableEntry::new_with_precise_submode( + "MGT_MAN_NML", + TestComponentId::MgtDevManager as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + )); + let mut sequence_tbl_safe_0 = SequenceTableMapTable::new("SAFE_SEQ_0_TBL"); + sequence_tbl_safe_0.add_entry(SequenceTableEntry::new( + "SAFE_SEQ_0_MGM_A", + TestComponentId::MagnetometerAssembly as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as Mode, 0), + false, + )); + sequence_tbl_safe_0.add_entry(SequenceTableEntry::new( + "SAFE_SEQ_0_MGT_MAN", + TestComponentId::MgtDevManager as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as Mode, 0), + false, + )); + let mut sequence_tbl_safe_1 = SequenceTableMapTable::new("SAFE_SEQ_1_TBL"); + sequence_tbl_safe_1.add_entry(SequenceTableEntry::new( + "SAFE_SEQ_1_ACS_CTRL", + TestComponentId::AcsController as u64, + ModeAndSubmode::new(AcsMode::SAFE as Mode, 0), + false, + )); + let mut sequence_tbl_safe = SequenceTablesMapValue::new("SAFE_SEQ_TBL"); + sequence_tbl_safe.add_sequence_table(sequence_tbl_safe_0); + sequence_tbl_safe.add_sequence_table(sequence_tbl_safe_1); + acs_subsystem.add_target_and_sequence_table( + AcsMode::SAFE as u32, + target_table_safe, + sequence_tbl_safe, + ); + + // Connect the PUS mode service to all mode objects. + connect_mode_nodes( + &mut pus_service, + request_sender_to_acs_subsystem, + &mut acs_subsystem, + reply_sender_to_pus.clone(), + ); + connect_mode_nodes( + &mut pus_service, + request_sender_to_acs_ctrl.clone(), + &mut acs_ctrl, + reply_sender_to_pus.clone(), + ); + connect_mode_nodes( + &mut pus_service, + request_sender_to_mgm_dev_0.clone(), + &mut mgm_dev_0, + reply_sender_to_pus.clone(), + ); + connect_mode_nodes( + &mut pus_service, + request_sender_to_mgm_dev_1.clone(), + &mut mgm_dev_1, + reply_sender_to_pus.clone(), + ); + connect_mode_nodes( + &mut pus_service, + request_sender_to_mgm_assy.clone(), + &mut mgm_assy, + reply_sender_to_pus.clone(), + ); + connect_mode_nodes( + &mut pus_service, + request_sender_to_mgt_man.clone(), + &mut mgt_manager, + reply_sender_to_pus.clone(), + ); + connect_mode_nodes( + &mut pus_service, + request_sender_to_mgt_dev.clone(), + &mut mgt_dev, + reply_sender_to_pus.clone(), + ); + + // Connect the ACS subsystem to all children. + connect_mode_nodes( + &mut acs_subsystem, + request_sender_to_mgm_assy, + &mut mgm_assy, + reply_sender_to_acs_subsystem.clone(), + ); + connect_mode_nodes( + &mut acs_subsystem, + request_sender_to_acs_ctrl, + &mut acs_ctrl, + reply_sender_to_acs_subsystem.clone(), + ); + connect_mode_nodes( + &mut acs_subsystem, + request_sender_to_mgt_man.clone(), + &mut mgt_manager, + reply_sender_to_acs_subsystem.clone(), + ); + + connect_mode_nodes( + &mut mgm_assy, + request_sender_to_mgm_dev_0, + &mut mgm_dev_0, + reply_sender_to_mgm_assy.clone(), + ); + connect_mode_nodes( + &mut mgm_assy, + request_sender_to_mgm_dev_1, + &mut mgm_dev_1, + reply_sender_to_mgm_assy, + ); + + connect_mode_nodes( + &mut mgt_manager, + request_sender_to_mgt_dev, + &mut mgt_dev, + reply_sender_to_mgt_man, + ); + Self { + pus: pus_service, + subsystem: acs_subsystem, + ctrl: acs_ctrl, + mgm_assy, + mgt_dev, + mgt_manager, + mgm_devs: [mgm_dev_0, mgm_dev_1], + } + } + + pub fn run(&mut self) { + self.pus.run(); + self.subsystem.run(); + self.ctrl.run(); + self.mgt_manager.run(); + self.mgm_assy.run(); + self.mgm_devs[0].run(); + self.mgm_devs[1].run(); + self.mgt_dev.run(); + } +} + +#[test] +fn announce_recursively() { + let mut tb = TreeTestbench::new(); + tb.pus.announce_modes_recursively(); + // Run everything twice so the order does not matter. + tb.run(); + tb.run(); + assert_eq!(tb.subsystem.get_num_mode_requests(), 1); + let mut announces = tb.subsystem.mode_req_mock.announce_mode_calls.borrow_mut(); + assert_eq!(announces.len(), 1); + announces = tb.ctrl.mode_req_mock.announce_mode_calls.borrow_mut(); + assert_eq!(tb.ctrl.mode_req_mock.start_transition_calls.len(), 0); + assert_eq!(tb.ctrl.mode_and_submode(), UNKNOWN_MODE); + assert_eq!(announces.len(), 1); + assert_eq!(tb.mgm_assy.get_num_mode_requests(), 1); + announces = tb.mgm_assy.mode_req_mock.announce_mode_calls.borrow_mut(); + assert_eq!(tb.mgm_assy.mode_req_mock.start_transition_calls.len(), 0); + assert_eq!(tb.mgm_assy.mode_and_submode(), UNKNOWN_MODE); + assert_eq!(announces.len(), 1); + for mgm_dev in &mut tb.mgm_devs { + assert_eq!(mgm_dev.get_and_clear_num_mode_msgs(), 1); + announces = mgm_dev.mode_req_mock.announce_mode_calls.borrow_mut(); + assert_eq!(mgm_dev.mode_req_mock.start_transition_calls.len(), 0); + assert_eq!(mgm_dev.mode_and_submode(), UNKNOWN_MODE); + assert_eq!(announces.len(), 1); + } + assert_eq!(announces.len(), 1); + assert_eq!(tb.mgt_dev.get_and_clear_num_mode_msgs(), 1); + announces = tb.mgt_dev.mode_req_mock.announce_mode_calls.borrow_mut(); + assert_eq!(tb.mgt_dev.mode_req_mock.start_transition_calls.len(), 0); + assert_eq!(tb.mgt_dev.mode_and_submode(), UNKNOWN_MODE); + assert_eq!(announces.len(), 1); + assert_eq!(tb.mgt_manager.get_num_mode_requests(), 1); + announces = tb + .mgt_manager + .mode_req_mock + .announce_mode_calls + .borrow_mut(); + assert_eq!(tb.mgt_manager.mode_req_mock.start_transition_calls.len(), 0); + assert_eq!(tb.mgt_manager.mode_and_submode(), UNKNOWN_MODE); + assert_eq!(announces.len(), 1); +} + +fn generic_mode_reply_checker( + reply_meta: MessageMetadata, + mode_and_submode: ModeAndSubmode, + expected_modes: &mut HashMap, +) { + let id = TestComponentId::try_from(reply_meta.sender_id()).expect("invalid sender id"); + if !expected_modes.contains_key(&reply_meta.sender_id()) { + panic!("received unexpected mode reply from component {:?}", id); + } + let expected_mode = expected_modes.get(&reply_meta.sender_id()).unwrap(); + assert_eq!( + expected_mode, &mode_and_submode, + "mode mismatch for component {:?}, expected {:?}, got {:?}", + id, expected_mode, mode_and_submode + ); + expected_modes.remove(&reply_meta.sender_id()); +} + +#[test] +fn command_safe_mode() { + let mut tb = TreeTestbench::new(); + assert_eq!(tb.ctrl.mode_and_submode(), UNKNOWN_MODE); + assert_eq!(tb.mgm_devs[0].mode_and_submode(), UNKNOWN_MODE); + assert_eq!(tb.mgm_devs[1].mode_and_submode(), UNKNOWN_MODE); + let request_id = tb + .pus + .send_mode_cmd(ModeAndSubmode::new(AcsMode::SAFE as u32, 0)); + tb.run(); + tb.run(); + tb.run(); + let expected_req_id_not_ctrl = request_id << 8; + let expected_req_id_ctrl = (request_id << 8) + 1; + assert_eq!( + tb.ctrl.mode_and_submode(), + ModeAndSubmode::new(AcsMode::SAFE as u32, 0) + ); + assert_eq!( + tb.mgm_devs[0].mode_and_submode(), + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0) + ); + assert_eq!( + tb.mgm_devs[1].mode_and_submode(), + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0) + ); + assert_eq!( + tb.mgm_assy.mode_and_submode(), + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0) + ); + assert_eq!( + tb.mgt_manager.mode_and_submode(), + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0) + ); + assert_eq!( + tb.mgt_dev.mode_and_submode(), + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0) + ); + assert_eq!( + tb.subsystem.mode_and_submode(), + ModeAndSubmode::new(AcsMode::SAFE as u32, 0) + ); + + // Check function calls for subsystem + let generic_mock_check = |ctx: &str, + mock: &mut ModeRequestHandlerMock, + expected_mode_for_transition: ModeAndSubmode, + expected_req_id: RequestId| { + assert_eq!(mock.start_transition_calls.borrow().len(), 1); + let start_transition_call = mock.start_transition_calls.borrow_mut().front().unwrap(); + assert_eq!( + start_transition_call.0.request_id(), + expected_req_id, + "unexpected req ID for component {}", + ctx + ); + assert_eq!(start_transition_call.1, expected_mode_for_transition); + assert_eq!(mock.handle_mode_reached_calls.borrow().len(), 1); + + let handle_mode_reached_ref = mock.handle_mode_reached_calls.borrow(); + let handle_mode_reached_call = handle_mode_reached_ref.front().unwrap(); + assert_eq!( + handle_mode_reached_call.as_ref().unwrap().request_id(), + expected_req_id + ); + drop(handle_mode_reached_ref); + + assert_eq!(mock.send_mode_reply_calls.borrow().len(), 1); + let mode_reply_call = *mock.send_mode_reply_calls.borrow_mut().front().unwrap(); + assert_eq!(mode_reply_call.0.request_id(), expected_req_id); + if let ModeReply::ModeReply(mode_and_submode) = mode_reply_call.1 { + assert_eq!( + mode_and_submode, expected_mode_for_transition, + "unexpected mode for component {}", + mock.id + ); + } else { + panic!("Unexpected mode reply for component {}", mock.id); + } + // TODO: Check all mode replies + assert_eq!(mock.mode_messages_received(), 3); + mock.clear(); + }; + + generic_mock_check( + "subsystem", + &mut tb.subsystem.mode_req_mock, + ModeAndSubmode::new(AcsMode::SAFE as u32, 0), + request_id, + ); + assert_eq!( + tb.subsystem.subsystem_helper.request_id().unwrap(), + request_id + ); + assert_eq!( + tb.subsystem.subsystem_helper.state(), + ModeTreeHelperState::TargetKeeping + ); + assert_eq!(tb.subsystem.subsystem_helper.mode(), AcsMode::SAFE as Mode); + assert_eq!( + tb.subsystem.mode_reply_mock.num_of_received_mode_replies(), + 3 + ); + let mut expected_modes = HashMap::new(); + expected_modes.insert( + TestComponentId::AcsController as u64, + ModeAndSubmode::new(AcsMode::SAFE as u32, 0), + ); + expected_modes.insert( + TestComponentId::MagnetometerAssembly as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + ); + expected_modes.insert( + TestComponentId::MgtDevManager as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + ); + while let Some(reply) = tb.subsystem.mode_reply_mock.mode_reply_messages.pop_front() { + generic_mode_reply_checker(reply.0, reply.1, &mut expected_modes); + } + assert!(expected_modes.is_empty()); + + generic_mock_check( + "ctrl", + &mut tb.ctrl.mode_req_mock, + ModeAndSubmode::new(AcsMode::SAFE as u32, 0), + expected_req_id_ctrl, + ); + generic_mock_check( + "mgm assy", + &mut tb.mgm_assy.mode_req_mock, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + expected_req_id_not_ctrl, + ); + let mut expected_modes = HashMap::new(); + expected_modes.insert( + TestComponentId::MagnetometerDevice0 as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + ); + expected_modes.insert( + TestComponentId::MagnetometerDevice1 as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + ); + while let Some(reply) = tb.mgm_assy.mode_reply_mock.mode_reply_messages.pop_front() { + generic_mode_reply_checker(reply.0, reply.1, &mut expected_modes); + } + assert!(expected_modes.is_empty()); + + generic_mock_check( + "mgt mgmt", + &mut tb.mgt_manager.mode_req_mock, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + expected_req_id_not_ctrl, + ); + let mut expected_modes = HashMap::new(); + expected_modes.insert( + TestComponentId::MagnetorquerDevice as u64, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + ); + let reply = tb + .mgt_manager + .mode_reply_mock + .mode_reply_messages + .pop_front() + .unwrap(); + generic_mode_reply_checker(reply.0, reply.1, &mut expected_modes); + + generic_mock_check( + "mgt dev", + &mut tb.mgt_dev.mode_req_mock, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + expected_req_id_not_ctrl, + ); + generic_mock_check( + "mgm dev 0", + &mut tb.mgm_devs[0].mode_req_mock, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + expected_req_id_not_ctrl, + ); + generic_mock_check( + "mgm dev 1", + &mut tb.mgm_devs[1].mode_req_mock, + ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), + expected_req_id_not_ctrl, + ); } diff --git a/satrs/tests/pus_events.rs b/satrs/tests/pus_events.rs index d9d572e..81c7b70 100644 --- a/satrs/tests/pus_events.rs +++ b/satrs/tests/pus_events.rs @@ -6,7 +6,6 @@ use satrs::events::{EventU32, EventU32TypedSev, Severity, SeverityInfo}; use satrs::params::U32Pair; use satrs::params::{Params, ParamsHeapless, WritableToBeBytes}; use satrs::pus::event_man::{DefaultPusEventReportingMap, EventReporter, PusEventTmCreatorWithMap}; -use satrs::pus::test_util::TEST_COMPONENT_ID_0; use satrs::request::UniqueApidTargetId; use satrs::tmtc::PacketAsVec; use spacepackets::ecss::tm::PusTmReader; @@ -100,10 +99,7 @@ fn test_threaded_usage() { // Event sender and TM checker thread let jh1 = thread::spawn(move || { event_tx - .send(EventMessage::new( - TEST_COMPONENT_ID_0.id(), - INFO_EVENT.into(), - )) + .send(EventMessage::new(TEST_ID.id(), INFO_EVENT.into())) .expect("Sending info event failed"); loop { match event_packet_rx.try_recv() { @@ -130,7 +126,7 @@ fn test_threaded_usage() { } event_tx .send(EventMessage::new_with_params( - TEST_COMPONENT_ID_0.id(), + TEST_ID.id(), LOW_SEV_EVENT, &Params::Heapless((2_u32, 3_u32).into()), ))