From f34f8304d4dc4e523c4d370cc25327a5f7f27b41 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 9 Dec 2024 17:28:28 +0100 Subject: [PATCH] continue mode tree execution helper --- satrs-example/src/pus/mode.rs | 1 + satrs/src/mode.rs | 3 + satrs/src/mode_tree.rs | 54 +++++++++++---- satrs/src/pus/mode.rs | 6 +- satrs/src/subsystem.rs | 31 ++++++++- satrs/tests/mode_tree.rs | 125 +++++++++++++++++++++++++--------- 6 files changed, 169 insertions(+), 51 deletions(-) diff --git a/satrs-example/src/pus/mode.rs b/satrs-example/src/pus/mode.rs index 56ae9d4..c0dae29 100644 --- a/satrs-example/src/pus/mode.rs +++ b/satrs-example/src/pus/mode.rs @@ -110,6 +110,7 @@ impl PusReplyHandler for ModeReplyHandler { ), )?; } + ModeReply::ModeInfo(_mode_and_submode) => (), }; Ok(true) } diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs index a18a19d..c8e4ed6 100644 --- a/satrs/src/mode.rs +++ b/satrs/src/mode.rs @@ -28,6 +28,9 @@ pub struct ModeAndSubmode { submode: Submode, } +pub const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(u32::MAX, 0); +pub const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(u32::MAX - 1, 0); + impl ModeAndSubmode { pub const RAW_LEN: usize = size_of::() + size_of::(); diff --git a/satrs/src/mode_tree.rs b/satrs/src/mode_tree.rs index 6156437..86b38c9 100644 --- a/satrs/src/mode_tree.rs +++ b/satrs/src/mode_tree.rs @@ -105,7 +105,6 @@ impl TargetTableEntry { name: &'static str, target_id: ComponentId, mode_submode: ModeAndSubmode, - monitor_state: bool, allowed_submode_mask: Option, ) -> Self { Self { @@ -115,7 +114,7 @@ impl TargetTableEntry { mode_submode, allowed_submode_mask, }, - monitor_state, + monitor_state: true, } } @@ -123,7 +122,6 @@ impl TargetTableEntry { name: &'static str, target_id: ComponentId, mode_submode: ModeAndSubmode, - monitor_state: bool, ) -> Self { Self { common: ModeTableEntryCommon { @@ -132,7 +130,7 @@ impl TargetTableEntry { mode_submode, allowed_submode_mask: None, }, - monitor_state, + monitor_state: true, } } @@ -214,14 +212,17 @@ pub mod alloc_mod { 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) -> Self { + pub fn new(name: &'static str, fallback_mode: Option) -> Self { Self { name, + fallback_mode, entries: Default::default(), } } @@ -278,24 +279,49 @@ pub mod alloc_mod { #[derive(Debug, Default)] pub struct SequenceModeTables(pub HashMap); + #[derive(Debug)] + pub struct ModeStoreVecValue { + id: ComponentId, + mode_and_submode: ModeAndSubmode, + pub awaiting_reply: bool, + } + + impl ModeStoreVecValue { + 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 + } + } + #[derive(Debug, Default)] - pub struct ModeStoreVec(pub alloc::vec::Vec<(ComponentId, ModeAndSubmode)>); + pub struct ModeStoreVec(pub alloc::vec::Vec); #[derive(Debug, Default)] pub struct ModeStoreMap(pub hashbrown::HashMap); impl ModeStoreProvider for ModeStoreVec { fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { - self.0.push((target_id, mode)); + self.0.push(ModeStoreVecValue::new(target_id, mode)); } fn has_component(&self, target_id: ComponentId) -> bool { - self.0.iter().any(|(id, _)| *id == target_id) + self.0.iter().any(|val| val.id == target_id) } fn get_mode(&self, target_id: ComponentId) -> Option { - self.0.iter().find_map(|(id, mode)| { - if *id == target_id { - return Some(*mode); + self.0.iter().find_map(|val| { + if val.id == target_id { + return Some(val.mode_and_submode); } None }) @@ -306,9 +332,9 @@ pub mod alloc_mod { target_id: ComponentId, mode_to_set: ModeAndSubmode, ) { - self.0.iter_mut().for_each(|(id, mode)| { - if *id == target_id { - *mode = mode_to_set; + self.0.iter_mut().for_each(|val| { + if val.id == target_id { + val.mode_and_submode = mode_to_set; } }); } diff --git a/satrs/src/pus/mode.rs b/satrs/src/pus/mode.rs index fea2bff..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, - ModeRequestorAndHandlerOneParentMpsc, ModeRequestorOneChildMpsc, + ModeRequestorAndHandlerMpsc, ModeRequestorOneChildMpsc, }, request::{GenericMessage, MessageMetadata}, }; @@ -90,7 +90,7 @@ mod tests { let (request_sender_to_channel_1, request_receiver_channel_1) = mpsc::channel(); //let (reply_sender_to_channel_2, reply_receiver_channel_2) = mpsc::channel(); - let mut mode_connector = ModeRequestorAndHandlerOneParentMpsc::new( + let mut mode_connector = ModeRequestorAndHandlerMpsc::new( TEST_COMPONENT_ID_0, request_receiver_of_connector, reply_receiver_of_connector, @@ -129,7 +129,7 @@ mod tests { let (_request_sender_to_connector, request_receiver_of_connector) = mpsc::channel(); let (reply_sender_to_channel_2, reply_receiver_channel_2) = mpsc::channel(); - let mut mode_connector = ModeRequestorAndHandlerOneParentMpsc::new( + let mut mode_connector = ModeRequestorAndHandlerMpsc::new( TEST_COMPONENT_ID_0, request_receiver_of_connector, reply_receiver_of_connector, diff --git a/satrs/src/subsystem.rs b/satrs/src/subsystem.rs index 85ded3b..3e2ec04 100644 --- a/satrs/src/subsystem.rs +++ b/satrs/src/subsystem.rs @@ -1,6 +1,6 @@ use crate::{ mode::{Mode, ModeAndSubmode, ModeRequest, ModeRequestSender}, - mode_tree::{SequenceModeTables, SequenceTableMapTable, SequenceTablesMapValue}, + mode_tree::{ModeStoreVec, SequenceModeTables, SequenceTableMapTable, SequenceTablesMapValue}, queue::GenericTargetedMessagingError, request::RequestId, ComponentId, @@ -21,6 +21,12 @@ pub trait CheckSuccessProvider { ); } +#[derive(Debug)] +pub enum TargetKeepingResult { + Ok, + Violated { fallback_mode: Option }, +} + #[derive(Debug)] pub enum SequenceHandlerResult { SequenceDone, @@ -92,6 +98,7 @@ impl SequenceExecutionHelper { &mut self, table: &SequenceModeTables, sender: &impl ModeRequestSender, + mode_store_vec: &mut ModeStoreVec, ) -> Result { if self.state == SequenceExecutionHelperStates::AwaitingCheckSuccess { return Ok(SequenceHandlerResult::AwaitingSuccessCheck); @@ -100,7 +107,12 @@ impl SequenceExecutionHelper { Some(idx) => { // Execute the sequence. let seq_table_value = table.0.get(&self.target_mode).unwrap(); - self.execute_sequence_and_map_to_result(seq_table_value, idx, sender) + self.execute_sequence_and_map_to_result( + seq_table_value, + idx, + sender, + mode_store_vec, + ) } None => { // Find the first sequence @@ -109,7 +121,12 @@ impl SequenceExecutionHelper { Ok(SequenceHandlerResult::SequenceDone) } else { self.current_sequence_index = Some(0); - self.execute_sequence_and_map_to_result(seq_table_value, 0, sender) + self.execute_sequence_and_map_to_result( + seq_table_value, + 0, + sender, + mode_store_vec, + ) } } } @@ -120,11 +137,13 @@ impl SequenceExecutionHelper { seq_table_value: &SequenceTablesMapValue, sequence_idx: usize, sender: &impl ModeRequestSender, + mode_store_vec: &mut ModeStoreVec, ) -> Result { if Self::execute_sequence( self.request_id, &seq_table_value.entries[sequence_idx], sender, + mode_store_vec, )? { self.state = SequenceExecutionHelperStates::AwaitingCheckSuccess; Ok(SequenceHandlerResult::AwaitingSuccessCheck) @@ -140,6 +159,7 @@ impl SequenceExecutionHelper { request_id: RequestId, map_table: &SequenceTableMapTable, sender: &impl ModeRequestSender, + mode_store_vec: &mut ModeStoreVec, ) -> Result { let mut some_succes_check_required = false; for entry in &map_table.entries { @@ -148,6 +168,11 @@ impl SequenceExecutionHelper { entry.common.target_id, ModeRequest::SetMode(entry.common.mode_submode), )?; + mode_store_vec.0.iter_mut().for_each(|val| { + if val.id() == entry.common.target_id { + val.awaiting_reply = true; + } + }); if entry.check_success { some_succes_check_required = true; } diff --git a/satrs/tests/mode_tree.rs b/satrs/tests/mode_tree.rs index 27d87c0..3beaafb 100644 --- a/satrs/tests/mode_tree.rs +++ b/satrs/tests/mode_tree.rs @@ -2,7 +2,8 @@ use core::cell::Cell; use satrs::mode::{ Mode, ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler, ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestSender, - ModeRequestorAndHandlerMpscBounded, ModeRequestorOneChildBoundedMpsc, + ModeRequestorAndHandlerMpscBounded, ModeRequestorOneChildBoundedMpsc, INVALID_MODE, + UNKNOWN_MODE, }; use satrs::mode_tree::{ connect_mode_nodes, ModeChild, ModeNode, ModeParent, ModeStoreProvider, SequenceTableEntry, @@ -13,7 +14,7 @@ use satrs::mode_tree::{ TargetTablesMapValue, }; use satrs::request::MessageMetadata; -use satrs::subsystem::{SequenceExecutionHelper, SequenceHandlerResult}; +use satrs::subsystem::{SequenceExecutionHelper, SequenceHandlerResult, TargetKeepingResult}; use satrs::{ mode::{ModeAndSubmode, ModeReply, ModeRequest}, queue::GenericTargetedMessagingError, @@ -25,9 +26,6 @@ use std::collections::VecDeque; use std::convert::Infallible; use std::{println, sync::mpsc}; -const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(0xffffffff, 0); -const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(0xffffffff - 1, 0); - pub enum DefaultMode { OFF = 0, ON = 1, @@ -58,8 +56,9 @@ pub enum TestComponentId { pub type RequestSenderType = mpsc::SyncSender>; pub type ReplySenderType = mpsc::SyncSender>; -#[derive(Debug)] +#[derive(Debug, Default)] pub enum ModeTreeHelperState { + #[default] Idle, TargetKeeping = 1, SequenceCommanding = 2, @@ -68,10 +67,16 @@ pub enum ModeTreeHelperState { #[derive(Debug)] pub enum ModeTreeHelperResult { Idle, - TargetKeeping, + TargetKeeping(TargetKeepingResult), SequenceCommanding(SequenceHandlerResult), } +impl From for ModeTreeHelperResult { + fn from(value: TargetKeepingResult) -> Self { + Self::TargetKeeping(value) + } +} + impl From for ModeTreeHelperResult { fn from(value: SequenceHandlerResult) -> Self { Self::SequenceCommanding(value) @@ -82,12 +87,16 @@ impl From for ModeTreeHelperResult { pub enum ModeTreeHelperError { #[error("generic targeted messaging error: {0}")] Message(#[from] GenericTargetedMessagingError), + #[error("current mode {0} is not contained in target table")] + CurrentModeNotInTargetTable(Mode), } + // TODO: // // 1. Fallback mode? Needs to be a part of the target mode table.. // 2. State to determine whether we are in sequence execution mode or in target keeping mode. pub struct ModeTreeCommandingHelper { + pub current_mode: ModeAndSubmode, pub state: ModeTreeHelperState, pub children_mode_store: ModeStoreVec, pub target_tables: TargetModeTables, @@ -95,6 +104,19 @@ pub struct ModeTreeCommandingHelper { pub helper: SequenceExecutionHelper, } +impl Default for ModeTreeCommandingHelper { + fn default() -> Self { + Self { + current_mode: UNKNOWN_MODE, + state: Default::default(), + children_mode_store: Default::default(), + target_tables: Default::default(), + sequence_tables: Default::default(), + helper: Default::default(), + } + } +} + impl ModeTreeCommandingHelper { pub fn new( children_mode_store: ModeStoreVec, @@ -102,6 +124,7 @@ impl ModeTreeCommandingHelper { sequence_tables: SequenceModeTables, ) -> Self { Self { + current_mode: UNKNOWN_MODE, state: ModeTreeHelperState::Idle, children_mode_store, target_tables, @@ -135,13 +158,24 @@ impl ModeTreeCommandingHelper { } self.children_mode_store .set_mode_for_contained_component(target_id, mode_and_submode); - // TODO: - // 1. If we are in IDLE Mode, we are done. - // 2. If we are in sequencing mode, check whether this completes the sequence. How do - // we best do this? We would have to remember which IDs need a mode confirmation. - // We could extend the mode store for this. - // 3. If we are in target keeping mode, we have to check whether the target keeping was - // violated. + 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) => { @@ -166,22 +200,49 @@ impl ModeTreeCommandingHelper { self.handle_mode_reply(reply); } match self.state { - ModeTreeHelperState::Idle => todo!(), + ModeTreeHelperState::Idle => Ok(ModeTreeHelperResult::Idle), ModeTreeHelperState::TargetKeeping => { - // TODO: Verify children modes against target table where applicable. - Ok(ModeTreeHelperResult::TargetKeeping) - } - ModeTreeHelperState::SequenceCommanding => { - Ok(self.helper.run(&self.sequence_tables, req_sender)?.into()) + // 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()) { + 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 + }; + } + }) + } + // Target keeping violated. Report violation and fallback mode to user. + return Ok(TargetKeepingResult::Violated { + fallback_mode: None, + } + .into()); + } + Ok(ModeTreeHelperResult::TargetKeeping(TargetKeepingResult::Ok)) } + ModeTreeHelperState::SequenceCommanding => Ok(self + .helper + .run( + &self.sequence_tables, + req_sender, + &mut self.children_mode_store, + )? + .into()), } } - - //pub fn check_current_sequence_against_mode_store(&self) { - // TODO: Check whether current sequence requires success checks and if some are required, - // check mode store content against the sequence target mode. - - //} } #[derive(Default, Debug)] @@ -266,6 +327,9 @@ impl ModeRequestHandler for ModeRequestHandlerMock { } } +#[derive(Default, Debug)] +pub struct ModeReplyHandlerMock {} + struct PusModeService { pub request_id_counter: Cell, pub mode_node: ModeRequestorOneChildBoundedMpsc, @@ -316,7 +380,7 @@ impl AcsSubsystem { mode_requestor_info: None, mode_and_submode: UNKNOWN_MODE, target_mode_and_submode: None, - subsystem_helper: Default::default(), + subsystem_helper: ModeTreeCommandingHelper::default(), mode_req_handler_mock: Default::default(), mode_req_recvd: 0, } @@ -506,6 +570,7 @@ impl MgmAssembly { } pub fn check_mode_replies(&mut self) -> Result<(), GenericTargetedMessagingError> { + // TODO: Call mode reply handler mock. if let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? { match reply_and_id.message { ModeReply::ModeReply(reply) => { @@ -524,6 +589,7 @@ impl MgmAssembly { expected ); } + ModeReply::ModeInfo(_mode_and_submode) => {} } } Ok(()) @@ -1136,12 +1202,11 @@ impl TreeTestbench { }; // ACS subsystem tables - let mut target_table_safe = TargetTablesMapValue::new("SAFE_TARGET_TBL"); + 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), - true, // All submodes allowed. Some(0xffff), )); @@ -1149,13 +1214,11 @@ impl TreeTestbench { "MGM_A_NML", TestComponentId::MagnetometerAssembly as u64, ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), - true, )); target_table_safe.add_entry(TargetTableEntry::new_with_precise_submode( "MGT_MAN_NML", TestComponentId::MgtDevManager as u64, ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0), - true, )); let mut sequence_tbl_safe_0 = SequenceTableMapTable::new("SAFE_SEQ_0_TBL"); sequence_tbl_safe_0.add_entry(SequenceTableEntry::new(