diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs index c991b10..e88120e 100644 --- a/satrs/src/mode.rs +++ b/satrs/src/mode.rs @@ -180,6 +180,8 @@ impl> ModeRequestReceiver pub enum ModeError { #[error("Messaging error: {0}")] Messaging(#[from] GenericTargetedMessagingError), + #[error("busy with other mode request")] + Busy, } pub trait ModeProvider { diff --git a/satrs/src/mode_tree.rs b/satrs/src/mode_tree.rs index 86b38c9..6539970 100644 --- a/satrs/src/mode_tree.rs +++ b/satrs/src/mode_tree.rs @@ -282,7 +282,7 @@ pub mod alloc_mod { #[derive(Debug)] pub struct ModeStoreVecValue { id: ComponentId, - mode_and_submode: ModeAndSubmode, + pub mode_and_submode: ModeAndSubmode, pub awaiting_reply: bool, } @@ -309,6 +309,30 @@ pub mod alloc_mod { #[derive(Debug, Default)] pub struct ModeStoreMap(pub hashbrown::HashMap); + impl ModeStoreVec { + /// Generic handler for mode replies received from child components. + /// + /// It returns whether any children are still awating replies. + pub fn generic_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + ) -> bool { + let mut still_awating_replies = 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; + } + val.awaiting_reply = false; + } + if val.awaiting_reply { + still_awating_replies = true; + } + }); + still_awating_replies + } + } impl ModeStoreProvider for ModeStoreVec { fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { self.0.push(ModeStoreVecValue::new(target_id, mode)); diff --git a/satrs/src/subsystem.rs b/satrs/src/subsystem.rs index c816954..8e55608 100644 --- a/satrs/src/subsystem.rs +++ b/satrs/src/subsystem.rs @@ -28,9 +28,9 @@ pub enum TargetKeepingResult { } #[derive(Debug)] -pub enum SequenceHandlerResult { - SequenceDone, - SequenceStepDone, +pub enum ModeCommandingResult { + CommandingDone, + CommandingStepDone, AwaitingSuccessCheck, } @@ -103,12 +103,12 @@ impl SequenceExecutionHelper { table: &SequenceModeTables, sender: &impl ModeRequestSender, mode_store_vec: &mut ModeStoreVec, - ) -> Result { + ) -> Result { if self.state == SequenceExecutionHelperStates::AwaitingCheckSuccess { - return Ok(SequenceHandlerResult::AwaitingSuccessCheck); + return Ok(ModeCommandingResult::AwaitingSuccessCheck); } if self.target_mode.is_none() { - return Ok(SequenceHandlerResult::SequenceDone); + return Ok(ModeCommandingResult::CommandingDone); } match self.current_sequence_index { Some(idx) => { @@ -125,7 +125,7 @@ impl SequenceExecutionHelper { // Find the first sequence let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); if seq_table_value.entries.is_empty() { - Ok(SequenceHandlerResult::SequenceDone) + Ok(ModeCommandingResult::CommandingDone) } else { self.current_sequence_index = Some(0); self.execute_sequence_and_map_to_result( @@ -145,7 +145,7 @@ impl SequenceExecutionHelper { sequence_idx: usize, sender: &impl ModeRequestSender, mode_store_vec: &mut ModeStoreVec, - ) -> Result { + ) -> Result { if Self::execute_sequence( self.request_id, &seq_table_value.entries[sequence_idx], @@ -153,12 +153,12 @@ impl SequenceExecutionHelper { mode_store_vec, )? { self.state = SequenceExecutionHelperStates::AwaitingCheckSuccess; - Ok(SequenceHandlerResult::AwaitingSuccessCheck) + Ok(ModeCommandingResult::AwaitingSuccessCheck) } else if seq_table_value.entries.len() - 1 == sequence_idx { - return Ok(SequenceHandlerResult::SequenceDone); + return Ok(ModeCommandingResult::CommandingDone); } else { self.current_sequence_index = Some(sequence_idx + 1); - return Ok(SequenceHandlerResult::SequenceStepDone); + return Ok(ModeCommandingResult::CommandingStepDone); } } diff --git a/satrs/tests/mode_tree.rs b/satrs/tests/mode_tree.rs index ec08bdd..36b9eb5 100644 --- a/satrs/tests/mode_tree.rs +++ b/satrs/tests/mode_tree.rs @@ -16,7 +16,7 @@ use satrs::mode_tree::{ use satrs::request::{MessageMetadata, RequestId}; use satrs::res_code::ResultU16; use satrs::subsystem::{ - ModeDoesNotExistError, SequenceExecutionHelper, SequenceHandlerResult, TargetKeepingResult, + ModeCommandingResult, ModeDoesNotExistError, SequenceExecutionHelper, TargetKeepingResult, }; use satrs::{ mode::{ModeAndSubmode, ModeReply, ModeRequest}, @@ -65,25 +65,34 @@ pub enum ModeTreeHelperState { #[default] Idle, TargetKeeping = 1, - SequenceCommanding = 2, + ModeCommanding = 2, } -#[derive(Debug)] -pub enum ModeTreeHelperResult { +#[derive(Debug, Default)] +pub enum AssemblyHelperResult { + #[default] + Idle, + TargetKeepingViolation(ComponentId), + ModeCommandingDone, +} + +#[derive(Debug, Default)] +pub enum SubsystemHelperResult { + #[default] Idle, TargetKeeping(TargetKeepingResult), - SequenceCommanding(SequenceHandlerResult), + ModeCommanding(ModeCommandingResult), } -impl From for ModeTreeHelperResult { +impl From for SubsystemHelperResult { fn from(value: TargetKeepingResult) -> Self { Self::TargetKeeping(value) } } -impl From for ModeTreeHelperResult { - fn from(value: SequenceHandlerResult) -> Self { - Self::SequenceCommanding(value) +impl From for SubsystemHelperResult { + fn from(value: ModeCommandingResult) -> Self { + Self::ModeCommanding(value) } } @@ -139,7 +148,7 @@ impl SubsystemCommandingHelper { request_id: RequestId, ) -> Result<(), ModeDoesNotExistError> { self.helper.load(mode, request_id, &self.sequence_tables)?; - self.state = ModeTreeHelperState::SequenceCommanding; + self.state = ModeTreeHelperState::ModeCommanding; Ok(()) } @@ -147,12 +156,12 @@ impl SubsystemCommandingHelper { &mut self, opt_reply: Option>, req_sender: &impl ModeRequestSender, - ) -> Result { + ) -> Result { if let Some(reply) = opt_reply { self.handle_mode_reply(&reply); } match self.state { - ModeTreeHelperState::Idle => Ok(ModeTreeHelperResult::Idle), + 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.mode()) { @@ -183,9 +192,11 @@ impl SubsystemCommandingHelper { } .into()); } - Ok(ModeTreeHelperResult::TargetKeeping(TargetKeepingResult::Ok)) + Ok(SubsystemHelperResult::TargetKeeping( + TargetKeepingResult::Ok, + )) } - ModeTreeHelperState::SequenceCommanding => { + ModeTreeHelperState::ModeCommanding => { let result = self.helper.run( &self.sequence_tables, req_sender, @@ -193,7 +204,7 @@ impl SubsystemCommandingHelper { )?; // By default, the helper will automatically transition into the target keeping // mode after an executed sequence. - if let SequenceHandlerResult::SequenceDone = result { + if let ModeCommandingResult::CommandingDone = result { self.state = ModeTreeHelperState::TargetKeeping; self.current_mode = ModeAndSubmode::new(self.helper.target_mode().unwrap(), 0); } @@ -202,6 +213,35 @@ impl SubsystemCommandingHelper { } } + pub fn handle_mode_reply(&mut self, reply: &GenericMessage) { + if !self.children_mode_store.has_component(reply.sender_id()) { + return; + } + let mut generic_mode_reply_handler = + |sender_id, mode_and_submode: Option| { + let still_awating_replies = self + .children_mode_store + .generic_reply_handler(sender_id, mode_and_submode); + if self.state == ModeTreeHelperState::ModeCommanding && !still_awating_replies { + self.helper.confirm_sequence_done(); + } + }; + match reply.message { + ModeReply::ModeInfo(mode_and_submode) => { + generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode)); + } + ModeReply::ModeReply(mode_and_submode) => { + generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode)); + } + ModeReply::CantReachMode(_) => { + generic_mode_reply_handler(reply.sender_id(), None); + } + ModeReply::WrongMode { reached, .. } => { + generic_mode_reply_handler(reply.sender_id(), Some(reached)); + } + }; + } + pub fn add_target_and_sequence_table( &mut self, mode: Mode, @@ -219,46 +259,6 @@ impl SubsystemCommandingHelper { ) -> Result<(), TargetNotInModeStoreError> { self.children_mode_store.set_mode(child, mode) } - - pub fn handle_mode_reply(&mut self, reply: &GenericMessage) { - let mut update_mode_store = |target_id, mode_and_submode| { - if !self.children_mode_store.has_component(target_id) { - return; - } - self.children_mode_store - .set_mode_for_contained_component(target_id, mode_and_submode); - match self.state { - ModeTreeHelperState::Idle => (), - ModeTreeHelperState::TargetKeeping => {} - ModeTreeHelperState::SequenceCommanding => { - let mut still_awating_replies = false; - self.children_mode_store.0.iter_mut().for_each(|val| { - if val.id() == target_id { - val.awaiting_reply = false; - } - if val.awaiting_reply { - still_awating_replies = true; - } - }); - if !still_awating_replies { - self.helper.confirm_sequence_done(); - } - } - } - }; - match reply.message { - ModeReply::ModeInfo(mode_and_submode) => { - update_mode_store(reply.sender_id(), mode_and_submode); - } - ModeReply::ModeReply(mode_and_submode) => { - update_mode_store(reply.sender_id(), mode_and_submode); - } - ModeReply::CantReachMode(_) => (), - ModeReply::WrongMode { reached, .. } => { - update_mode_store(reply.sender_id(), reached); - } - }; - } } #[derive(Default, Debug)] @@ -473,8 +473,8 @@ impl AcsSubsystem { .state_machine(mode_reply, &self.mode_node) { Ok(result) => match result { - ModeTreeHelperResult::Idle => (), - ModeTreeHelperResult::TargetKeeping(target_keeping_result) => { + SubsystemHelperResult::Idle => (), + SubsystemHelperResult::TargetKeeping(target_keeping_result) => { match target_keeping_result { TargetKeepingResult::Ok => todo!(), TargetKeepingResult::Violated { fallback_mode } => { @@ -486,11 +486,11 @@ impl AcsSubsystem { } } } - ModeTreeHelperResult::SequenceCommanding(sequence_handler_result) => { + SubsystemHelperResult::ModeCommanding(sequence_handler_result) => { match sequence_handler_result { - SequenceHandlerResult::SequenceDone => (), - SequenceHandlerResult::SequenceStepDone => (), - SequenceHandlerResult::AwaitingSuccessCheck => (), + ModeCommandingResult::CommandingDone => (), + ModeCommandingResult::CommandingStepDone => (), + ModeCommandingResult::AwaitingSuccessCheck => (), } } }, @@ -548,17 +548,15 @@ impl ModeProvider for AcsSubsystem { } #[derive(Debug, thiserror::Error)] -pub enum SubsytemModeError { - #[error("mode error: {0:?}")] +pub enum SubsystemModeError { + #[error("messaging error: {0:?}")] Mode(#[from] ModeError), #[error("mode does not exist: {0}")] ModeDoesNotExist(#[from] ModeDoesNotExistError), - #[error("busy with mode transition")] - Busy, } impl ModeRequestHandler for AcsSubsystem { - type Error = SubsytemModeError; + type Error = SubsystemModeError; fn start_transition( &mut self, @@ -566,8 +564,8 @@ impl ModeRequestHandler for AcsSubsystem { mode_and_submode: ModeAndSubmode, forced: bool, ) -> Result<(), Self::Error> { - if !forced && self.subsystem_helper.state == ModeTreeHelperState::SequenceCommanding { - return Err(SubsytemModeError::Busy); + 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); @@ -650,13 +648,98 @@ impl ModeRequestHandler for AcsSubsystem { } } +#[derive(Debug, Default)] +pub struct AssemblyCommandingHelper { + /// The IDs, modes and reply awaition status of all children are tracked in this data + /// structure. + pub children_mode_store: ModeStoreVec, + /// Target mode used for mode commanding. + pub target_mode: Option, + pub state: ModeTreeHelperState, +} + +impl AssemblyCommandingHelper { + pub fn send_mode_cmd_to_all_children_with_reply_awaition( + &mut self, + request_id: RequestId, + mode_and_submode: ModeAndSubmode, + forced: bool, + mode_req_sender: &impl ModeRequestSender, + ) -> Result<(), GenericTargetedMessagingError> { + self.target_mode = Some(mode_and_submode); + for child in self.children_mode_store.0.iter_mut() { + mode_req_sender.send_mode_request( + request_id, + child.id(), + ModeRequest::SetMode { + mode_and_submode, + forced, + }, + )?; + child.awaiting_reply = true; + } + Ok(()) + } + + pub fn count_number_of_children_with_target_mode(&self) -> Option { + self.target_mode?; + let target_mode = self.target_mode.unwrap(); + let mut children_in_target_mode = 0; + for child in self.children_mode_store.0.iter() { + if child.mode_and_submode() == target_mode { + children_in_target_mode += 1; + } + } + Some(children_in_target_mode) + } + + pub fn handle_mode_reply( + &mut self, + mode_reply: &GenericMessage, + ) -> AssemblyHelperResult { + if !self + .children_mode_store + .has_component(mode_reply.sender_id()) + { + return AssemblyHelperResult::Idle; + } + let mut generic_mode_reply_handler = |mode_and_submode: Option| { + let still_awating_replies = self + .children_mode_store + .generic_reply_handler(mode_reply.sender_id(), mode_and_submode); + if self.state == ModeTreeHelperState::TargetKeeping + && mode_and_submode.is_some() + && self.target_mode.is_some() + && mode_and_submode.unwrap() != self.target_mode.unwrap() + { + return AssemblyHelperResult::TargetKeepingViolation(mode_reply.sender_id()); + } + if self.state == ModeTreeHelperState::ModeCommanding && !still_awating_replies { + self.state = ModeTreeHelperState::TargetKeeping; + return AssemblyHelperResult::ModeCommandingDone; + } + AssemblyHelperResult::Idle + }; + 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)), + } + } +} + // TODO: This assembly requires some helper component to process commands.. Maybe implement it // manually first? struct MgmAssembly { pub mode_node: ModeRequestorAndHandlerMpscBounded, pub mode_requestor_info: Option, pub mode_and_submode: ModeAndSubmode, - pub target_mode_and_submode: Option, + pub assembly_helper: AssemblyCommandingHelper, pub mode_req_mock: ModeRequestHandlerMock, pub mode_reply_mock: ModeReplyHandlerMock, } @@ -667,7 +750,7 @@ impl MgmAssembly { mode_node, mode_requestor_info: None, mode_and_submode: UNKNOWN_MODE, - target_mode_and_submode: None, + assembly_helper: Default::default(), mode_req_mock: Default::default(), mode_reply_mock: Default::default(), } @@ -688,30 +771,22 @@ impl MgmAssembly { Ok(()) } - pub fn check_mode_replies(&mut self) -> Result<(), GenericTargetedMessagingError> { - // TODO: If a transition is active, we need to check whether all children have replied - // and have the correct mode. So we probably need some children list / map similarly to the - // subsystem, which also tracks where a reply is still awaited. + pub fn check_mode_replies(&mut self) -> Result<(), ModeError> { if let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? { self.mode_reply_mock.handle_mode_reply(&reply_and_id); - match reply_and_id.message { - ModeReply::ModeReply(reply) => { - println!( - "TestAssembly: Received mode reply from {:?}, reached: {:?}", - reply_and_id.sender_id(), - reply - ); + match self.assembly_helper.handle_mode_reply(&reply_and_id) { + AssemblyHelperResult::Idle => (), + AssemblyHelperResult::TargetKeepingViolation(_id) => { + // TODO: Check whether enough children are available to keep the mode. + // Otherwise, we command everything OFF, because we can not keep the mode. } - ModeReply::CantReachMode(_) => todo!(), - ModeReply::WrongMode { expected, reached } => { - println!( - "TestAssembly: Wrong mode reply from {:?}, reached {:?}, expected {:?}", - reply_and_id.sender_id(), - reached, - expected - ); + AssemblyHelperResult::ModeCommandingDone => { + if self.assembly_helper.target_mode.is_some() { + // Complete the mode command. + self.handle_mode_reached(self.mode_requestor_info)?; + self.mode_and_submode = self.assembly_helper.target_mode.take().unwrap(); + } } - ModeReply::ModeInfo(_mode_and_submode) => {} } } Ok(()) @@ -753,30 +828,24 @@ impl ModeRequestHandler for MgmAssembly { mode_and_submode: ModeAndSubmode, forced: bool, ) -> Result<(), Self::Error> { + // Always accept forced commands and commands to mode OFF. + if self.assembly_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.target_mode_and_submode = Some(mode_and_submode); self.mode_req_mock .start_transition(requestor, mode_and_submode, forced) .unwrap(); - // TODO: Is it correct to simply forward the mode? - self.mode_node - .request_sender_store - .0 - .iter() - .for_each(|(_, sender)| { - sender - .send(GenericMessage::new( - MessageMetadata::new( - requestor.request_id(), - self.mode_node.local_channel_id_generic(), - ), - ModeRequest::SetMode { - mode_and_submode, - forced: false, - }, - )) - .expect("sending mode request failed"); - }); + self.assembly_helper + .send_mode_cmd_to_all_children_with_reply_awaition( + requestor.request_id(), + mode_and_submode, + forced, + &self.mode_node, + )?; Ok(()) }