From 91c295d781ddaea54fa6375af1fa9e7f58df222d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 13 Jul 2026 14:36:43 +0200 Subject: [PATCH] try this --- satrs/src/mode_tree.rs | 12 ++ satrs/src/subsystem.rs | 261 ++++++++++++++++++++++++----------------- 2 files changed, 166 insertions(+), 107 deletions(-) diff --git a/satrs/src/mode_tree.rs b/satrs/src/mode_tree.rs index 71a778d..3842a79 100644 --- a/satrs/src/mode_tree.rs +++ b/satrs/src/mode_tree.rs @@ -167,6 +167,10 @@ impl ModeStoreValue { } } +pub trait SequenceTableProvider { + fn sequence_at_index(&self, index: usize) -> Option<&[SequenceTableEntry]>; +} + #[derive(Debug, thiserror::Error)] #[error("mode store is full")] pub struct FullError; @@ -396,6 +400,14 @@ pub mod alloc_mod { } } + impl SequenceTableProvider for SequenceTablesMapValue { + fn sequence_at_index(&self, index: usize) -> Option<&[SequenceTableEntry]> { + self.entries + .get(index) + .map(|table| table.entries.as_slice()) + } + } + #[derive(Debug, Default)] pub struct TargetModeTables(pub HashMap); diff --git a/satrs/src/subsystem.rs b/satrs/src/subsystem.rs index 27cbf28..2d57bbf 100644 --- a/satrs/src/subsystem.rs +++ b/satrs/src/subsystem.rs @@ -2,9 +2,9 @@ use crate::{ ComponentId, mode_tree::{ ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, - SequenceTablesMapValue, + SequenceTableProvider, SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError, + TargetTablesMapValue, }, - queue::GenericTargetedMessagingError, request::RequestId, }; @@ -44,6 +44,10 @@ pub enum StartSequenceError { InvalidRequestId(RequestId), } +#[derive(Debug, thiserror::Error)] +#[error("invalid sequence index")] +pub struct InvalidSequenceIndexError; + /// This sequence execution helper includes some boilerplate logic to /// execute [SequenceModeTables]. /// @@ -126,10 +130,10 @@ impl SequenceExecutionHelper { /// children components pub fn run( &mut self, - table: &SequenceModeTables, - children_mode_store: &mut ModeStoreVec, + sequence_table_provider: &impl SequenceTableProvider, + children_mode_store: &mut impl ModeStoreProvider, mode_request_handler: F, - ) -> Result { + ) -> Result { if self.state == SequenceExecutionHelperState::Idle { return Ok(ModeCommandingResult::Done); } @@ -140,30 +144,31 @@ impl SequenceExecutionHelper { return Ok(ModeCommandingResult::Done); } match self.current_sequence_index { - Some(idx) => { + Some(index) => { // 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, + Ok(self.execute_sequence_and_map_to_result( + index, + sequence_table_provider.sequence_at_index(index).unwrap(), children_mode_store, mode_request_handler, - ) + )) } None => { + let sequence_index = 0; + let sequence = sequence_table_provider + .sequence_at_index(sequence_index) + .unwrap(); // 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() { + if sequence.is_empty() { Ok(ModeCommandingResult::Done) } else { self.current_sequence_index = Some(0); - self.execute_sequence_and_map_to_result( - seq_table_value, - 0, + Ok(self.execute_sequence_and_map_to_result( + sequence_index, + sequence, children_mode_store, mode_request_handler, - ) + )) } } } @@ -210,33 +215,32 @@ impl SequenceExecutionHelper { self.current_sequence_index } - /// Execute a sequence at the given sequence index for a given [SequenceTablesMapValue]. + /// Execute a sequence at the given sequence index for a given sequence. + /// + /// The sequence is identifier by a sequence index. It is represented by a list of + /// [SequenceTableEntry] values. /// /// 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, + sequence_index: usize, + table_entries: &[SequenceTableEntry], children_mode_store: &mut impl ModeStoreProvider, mode_request_handler: impl FnMut(ComponentId, ModeRaw), - ) -> Result { + ) -> ModeCommandingResult { if self.state() == SequenceExecutionHelperState::Idle || self.request_id.is_none() { - return Ok(ModeCommandingResult::Done); + return ModeCommandingResult::Done; } - if Self::execute_sequence( - &seq_table_value.entries[sequence_idx].entries, - children_mode_store, - mode_request_handler, - )? { + if Self::execute_sequence(table_entries, children_mode_store, mode_request_handler) { self.state = SequenceExecutionHelperState::AwaitingSuccessCheck; - Ok(ModeCommandingResult::AwaitingSuccessCheck) - } else if seq_table_value.entries.len() - 1 == sequence_idx { + ModeCommandingResult::AwaitingSuccessCheck + } else if table_entries.len() - 1 == sequence_index { self.state = SequenceExecutionHelperState::Idle; - Ok(ModeCommandingResult::Done) + ModeCommandingResult::Done } else { - self.current_sequence_index = Some(sequence_idx + 1); - Ok(ModeCommandingResult::StepDone) + self.current_sequence_index = Some(sequence_index + 1); + ModeCommandingResult::StepDone } } @@ -255,7 +259,7 @@ impl SequenceExecutionHelper { table_entries: &[SequenceTableEntry], children_mode_store: &mut impl ModeStoreProvider, mut mode_request_handler: impl FnMut(ComponentId, ModeRaw), - ) -> Result { + ) -> bool { let mut some_succes_check_required = false; for entry in table_entries { mode_request_handler(entry.common.target_id, entry.common.mode); @@ -264,7 +268,7 @@ impl SequenceExecutionHelper { some_succes_check_required = true; } } - Ok(some_succes_check_required) + some_succes_check_required } } @@ -296,10 +300,12 @@ impl From for SubsystemHelperResult { #[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(ModeRaw), + #[error("No sequence table found for mode {0:?}")] + NoSequenceTableFound(Option), + #[error("invalid sequence index")] + InvalidSequenceIndex(#[from] InvalidSequenceIndexError), /// Mode command has failed, for example while executing a mode table. #[error("mode command failed")] ModeCommmandFailure { @@ -319,7 +325,6 @@ pub enum ModeTreeHelperError { /// /// 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, @@ -331,12 +336,12 @@ pub struct SubsystemCommandingHelper { /// 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 + /// [modes][ModeRaw]. 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]. + /// subsystem [modes][ModeRaw]. It specifies the actual commands and the order they should be + /// sent in to reach a certain [mode][ModeRaw]. pub sequence_tables: SequenceModeTables, /// The sequence execution helper is used to execute sequences in the [Self::sequence_tables]. pub seq_exec_helper: SequenceExecutionHelper, @@ -356,6 +361,14 @@ impl Default for SubsystemCommandingHelper { } } +#[derive(Debug)] +pub struct ModeResponse { + pub request_id: RequestId, + pub sender_id: ComponentId, + pub reported_mode: ModeRaw, + pub success: bool, +} + 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. @@ -415,7 +428,8 @@ impl SubsystemCommandingHelper { /// Add a mode child to the internal [Self::children_mode_store]. pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeRaw) { - self.children_mode_store.add_component(child, mode); + // Can not fail for regular vector. + self.children_mode_store.add_component(child, mode).unwrap(); } /// Add a target mode table and an associated sequence mode table. @@ -455,14 +469,13 @@ impl SubsystemCommandingHelper { Ok(()) } - pub fn state_machine( + pub fn state_machine( &mut self, - opt_reply: Option>, - req_sender: &impl ModeRequestSender, - is_commandable: &impl IsChildCommandable, + opt_mode_reponse: Option, + mode_request_handler: F, ) -> Result { - if let Some(reply) = opt_reply { - if self.handle_mode_reply(&reply)? { + if let Some(reply) = opt_mode_reponse { + if self.handle_children_mode_changed(reply)? { if self.seq_exec_helper.state() == SequenceExecutionHelperState::Idle { self.transition_to_target_keeping(); return Ok(SubsystemHelperResult::ModeCommanding( @@ -484,9 +497,22 @@ impl SubsystemCommandingHelper { Ok(SubsystemHelperResult::TargetKeeping) } ModeTreeHelperState::ModeCommanding => { + if self.seq_exec_helper.target_mode().is_none() { + return Err(ModeTreeHelperError::NoSequenceTableFound(None)); + } + let seq_table = self + .sequence_tables + .0 + .get(&self.seq_exec_helper.target_mode().unwrap()); + if seq_table.is_none() { + return Err(ModeTreeHelperError::NoSequenceTableFound( + self.seq_exec_helper.target_mode(), + )); + } let result = self.seq_exec_helper.run( - &self.sequence_tables, + seq_table.unwrap(), &mut self.children_mode_store, + mode_request_handler, )?; match result { ModeCommandingResult::Done => { @@ -520,18 +546,21 @@ impl SubsystemCommandingHelper { if !entry.monitor_state { continue; } - let mut target_mode_violated = false; + let 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 - }; + // TODO: Solve this differently + //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 { @@ -553,48 +582,54 @@ impl SubsystemCommandingHelper { // Handles a mode reply message and returns whether the reply completes a step of sequence // commanding. - fn handle_mode_reply( + fn handle_children_mode_changed( &mut self, - reply: &GenericMessage, + mode_response: ModeResponse, ) -> Result { - if !self.children_mode_store.has_component(reply.sender_id()) { + if !self + .children_mode_store + .has_component(mode_response.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) - }; + |sender_id, mode: 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() + && mode_response.request_id == self.active_internal_request_id.unwrap() + { + handle_awaition = true; + } + let still_awating_replies = self.children_mode_store.mode_reply_handler( + mode_response.sender_id, + Some(mode_response.reported_mode), + 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 !mode_response.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) @@ -609,22 +644,22 @@ impl SubsystemCommandingHelper { generic_mode_reply_handler(reply.sender_id(), Some(reached), true) } } + */ } pub fn update_child_mode( &mut self, child: ComponentId, - mode: ModeAndSubmode, + mode: ModeRaw, ) -> Result<(), TargetNotInModeStoreError> { let val_mut = self .children_mode_store .get_mut(child) .ok_or(TargetNotInModeStoreError(child))?; - val_mut.mode_and_submode = mode; + val_mut.mode = mode; Ok(()) } } -*/ #[cfg(test)] mod tests { @@ -633,9 +668,13 @@ mod tests { use super::*; use crate::{ - ComponentId, mode_tree::{ - ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables, - }, queue::GenericTargetedMessagingError, request::RequestId, subsystem::{ModeCommandingResult, SequenceExecutionHelperState}, + ComponentId, + mode_tree::{ + ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, + SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables, + }, + request::RequestId, + subsystem::{ModeCommandingResult, SequenceExecutionHelperState}, }; #[derive(Debug)] @@ -676,9 +715,12 @@ mod tests { pub fn run( &mut self, command_list: &mut Vec<(ComponentId, ModeRaw)>, - ) -> Result { + ) -> Result { self.execution_helper.run( - &self.seq_tables, + self.seq_tables + .0 + .get(&self.execution_helper.target_mode().unwrap()) + .unwrap(), &mut self.mode_store, |component_id, mode_raw| { command_list.push((component_id, mode_raw)); @@ -690,9 +732,14 @@ mod tests { // Assure that no unexpected behaviour occurs. assert_eq!( self.execution_helper - .run(&self.seq_tables, &mut self.mode_store, |_, _| { - panic!("should not have been called") - }) + .run( + self.seq_tables + .0 + .get(&self.execution_helper.target_mode().unwrap()) + .unwrap(), + &mut self.mode_store, + |_, _| { panic!("should not have been called") } + ) .unwrap(), ModeCommandingResult::Done );