continue mode tree execution helper

satrs-example/src/pus/mode.rs

@@ -110,6 +110,7 @@ impl PusReplyHandler<ActivePusRequestStd, ModeReply> for ModeReplyHandler {
                     ),
                 )?;
             }
+            ModeReply::ModeInfo(_mode_and_submode) => (),
         };
         Ok(true)
     }

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::<Mode>() + size_of::<Submode>();
 

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<Submode>,
     ) -> 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<Mode>,
         /// These are the rows of the a target table.
         pub entries: Vec<TargetTableEntry>,
     }
 
     impl TargetTablesMapValue {
-        pub fn new(name: &'static str) -> Self {
+        pub fn new(name: &'static str, fallback_mode: Option<Mode>) -> Self {
             Self {
                 name,
+                fallback_mode,
                 entries: Default::default(),
             }
         }
@@ -278,24 +279,49 @@ pub mod alloc_mod {
     #[derive(Debug, Default)]
     pub struct SequenceModeTables(pub HashMap<Mode, SequenceTablesMapValue>);
 
+    #[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<ModeStoreVecValue>);
     #[derive(Debug, Default)]
     pub struct ModeStoreMap(pub hashbrown::HashMap<ComponentId, ModeAndSubmode>);
 
     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<ModeAndSubmode> {
-            self.0.iter().find_map(|(id, mode)| {
+            self.0.iter().find_map(|val| {
-                if *id == target_id {
+                if val.id == target_id {
-                    return Some(*mode);
+                    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)| {
+            self.0.iter_mut().for_each(|val| {
-                if *id == target_id {
+                if val.id == target_id {
-                    *mode = mode_to_set;
+                    val.mode_and_submode = mode_to_set;
                 }
             });
         }

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,

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<Mode> },
+}
+
 #[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<SequenceHandlerResult, GenericTargetedMessagingError> {
         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<SequenceHandlerResult, GenericTargetedMessagingError> {
         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<bool, GenericTargetedMessagingError> {
         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;
             }

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<GenericMessage<ModeRequest>>;
 pub type ReplySenderType = mpsc::SyncSender<GenericMessage<ModeReply>>;
 
-#[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<TargetKeepingResult> for ModeTreeHelperResult {
+    fn from(value: TargetKeepingResult) -> Self {
+        Self::TargetKeeping(value)
+    }
+}
+
 impl From<SequenceHandlerResult> for ModeTreeHelperResult {
     fn from(value: SequenceHandlerResult) -> Self {
         Self::SequenceCommanding(value)
@@ -82,12 +87,16 @@ impl From<SequenceHandlerResult> 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:
+            match self.state {
-            // 1. If we are in IDLE Mode, we are done.
+                ModeTreeHelperState::Idle => (),
-            // 2. If we are in sequencing mode, check whether this completes the sequence. How do
+                ModeTreeHelperState::TargetKeeping => {}
-            //    we best do this? We would have to remember which IDs need a mode confirmation.
+                ModeTreeHelperState::SequenceCommanding => {
-            //    We could extend the mode store for this.
+                    let mut still_awating_replies = false;
-            // 3. If we are in target keeping mode, we have to check whether the target keeping was
+                    self.children_mode_store.0.iter_mut().for_each(|val| {
-            //    violated.
+                        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,24 +200,51 @@ 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.
+                // We check whether the current mode is modelled by a target table first.
-                Ok(ModeTreeHelperResult::TargetKeeping)
+                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;
                         }
-            ModeTreeHelperState::SequenceCommanding => {
+                        let mut target_mode_violated = false;
-                Ok(self.helper.run(&self.sequence_tables, req_sender)?.into())
+                        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)]
 pub struct ModeRequestHandlerMock {
     get_mode_calls: RefCell<u32>,
@@ -266,6 +327,9 @@ impl ModeRequestHandler for ModeRequestHandlerMock {
     }
 }
 
+#[derive(Default, Debug)]
+pub struct ModeReplyHandlerMock {}
+
 struct PusModeService {
     pub request_id_counter: Cell<u32>,
     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(