continue mode tree

satrs/src/mode_tree.rs

@@ -6,6 +6,9 @@ use crate::{
     ComponentId,
 };
 
+#[cfg(feature = "alloc")]
+pub use alloc_mod::*;
+
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 pub enum TableEntryType {
     /// Target table containing information of the expected children modes for  given mode.
@@ -79,31 +82,11 @@ impl TargetTableEntry {
     }
 }
 
-#[derive(Debug)]
-pub struct TargetTableMapValue {
-    /// Name for a given mode table entry.
-    pub name: &'static str,
-    /// These are the rows of the a target table.
-    pub entries: Vec<TargetTableEntry>,
-}
-
-impl TargetTableMapValue {
-    pub fn new(name: &'static str) -> Self {
-        Self {
-            name,
-            entries: Default::default(),
-        }
-    }
-
-    pub fn add_entry(&mut self, entry: TargetTableEntry) {
-        self.entries.push(entry);
-    }
-}
-
 /// An entry for the sequence tables.
 ///
 /// The `check_success` field instructs the mode sequence executor to verify that the
 /// target mode was actually reached before executing the next sequence.
+#[derive(Debug)]
 pub struct SequenceTableEntry {
     pub common: ModeTableEntryCommon,
     pub check_success: bool,
@@ -135,48 +118,124 @@ impl SequenceTableEntry {
     }
 }
 
-pub struct SequenceTableMapTable {
+pub trait ModeStoreProvider {
-    /// Name for a given mode sequence.
+    fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode);
-    pub name: &'static str,
+    fn get_mode(&self, target_id: ComponentId) -> Option<ModeAndSubmode>;
-    /// These are the rows of the a sequence table.
+    fn set_mode(&mut self, target_id: ComponentId, mode: ModeAndSubmode);
-    pub entries: Vec<SequenceTableEntry>,
 }
 
-impl SequenceTableMapTable {
+#[cfg(feature = "alloc")]
-    pub fn new(name: &'static str) -> Self {
+pub mod alloc_mod {
-        Self {
+    use super::*;
-            name,
+
-            entries: Default::default(),
+    #[derive(Debug)]
+    pub struct TargetTableMapValue {
+        /// Name for a given mode table entry.
+        pub name: &'static str,
+        /// These are the rows of the a target table.
+        pub entries: Vec<TargetTableEntry>,
+    }
+
+    impl TargetTableMapValue {
+        pub fn new(name: &'static str) -> Self {
+            Self {
+                name,
+                entries: Default::default(),
+            }
+        }
+
+        pub fn add_entry(&mut self, entry: TargetTableEntry) {
+            self.entries.push(entry);
         }
     }
 
-    pub fn add_entry(&mut self, entry: SequenceTableEntry) {
+    #[derive(Debug)]
-        self.entries.push(entry);
+    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<SequenceTableEntry>,
     }
-}
 
-pub struct SequenceTableMapValue {
+    impl SequenceTableMapTable {
-    /// Name for a given mode sequence.
+        pub fn new(name: &'static str) -> Self {
-    pub name: &'static str,
+            Self {
-    /// Each sequence can consists of multiple sequences that are executed consecutively.
+                name,
-    pub entries: Vec<SequenceTableMapTable>,
+                entries: Default::default(),
-}
+            }
+        }
 
-impl SequenceTableMapValue {
+        pub fn add_entry(&mut self, entry: SequenceTableEntry) {
-    pub fn new(name: &'static str) -> Self {
+            self.entries.push(entry);
-        Self {
-            name,
-            entries: Default::default(),
         }
     }
 
-    pub fn add_sequence_table(&mut self, entry: SequenceTableMapTable) {
+    #[derive(Debug)]
-        self.entries.push(entry);
+    pub struct SequenceTableMapValue {
+        /// Name for a given mode sequence.
+        pub name: &'static str,
+        /// Each sequence can consists of multiple sequences that are executed consecutively.
+        pub entries: Vec<SequenceTableMapTable>,
+    }
+
+    impl SequenceTableMapValue {
+        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<Mode, TargetTableMapValue>);
+    #[derive(Debug, Default)]
+    pub struct SequenceModeTables(pub HashMap<Mode, SequenceTableMapValue>);
+
+    #[derive(Debug, Default)]
+    pub struct ModeStoreVec(pub alloc::vec::Vec<(ComponentId, ModeAndSubmode)>);
+    #[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));
+        }
+
+        fn get_mode(&self, target_id: ComponentId) -> Option<ModeAndSubmode> {
+            self.0.iter().find_map(|(id, mode)| {
+                if *id == target_id {
+                    return Some(*mode);
+                }
+                None
+            })
+        }
+
+        fn set_mode(&mut self, target_id: ComponentId, mode_to_set: ModeAndSubmode) {
+            self.0.iter_mut().for_each(|(id, mode)| {
+                if *id == target_id {
+                    *mode = mode_to_set;
+                }
+            });
+        }
+    }
+
+    impl ModeStoreProvider for ModeStoreMap {
+        fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) {
+            self.0.insert(target_id, mode);
+        }
+        fn get_mode(&self, target_id: ComponentId) -> Option<ModeAndSubmode> {
+            self.0.get(&target_id).copied()
+        }
+        fn set_mode(&mut self, target_id: ComponentId, mode_to_set: ModeAndSubmode) {
+            self.0.insert(target_id, mode_to_set);
+        }
     }
 }
 
-pub struct TargetModeTable(pub HashMap<Mode, TargetTableMapValue>);
-pub struct SequenceModeTable(pub HashMap<Mode, SequenceTableMapValue>);
-
 #[cfg(test)]
 mod tests {}

satrs/src/subsystem.rs

@@ -1,6 +1,6 @@
 use crate::{
     mode::{Mode, ModeAndSubmode, ModeRequest, ModeRequestSender},
-    mode_tree::{SequenceModeTable, SequenceTableMapTable, SequenceTableMapValue},
+    mode_tree::{SequenceModeTables, SequenceTableMapTable, SequenceTableMapValue},
     queue::GenericTargetedMessagingError,
     request::RequestId,
     ComponentId,
@@ -40,9 +40,9 @@ impl SequenceExecutionHelper {
     pub fn new(
         mode: Mode,
         request_id: RequestId,
-        sequence_table: &SequenceModeTable,
+        sequence_tables: &SequenceModeTables,
     ) -> Option<Self> {
-        if !sequence_table.0.contains_key(&mode) {
+        if !sequence_tables.0.contains_key(&mode) {
             return None;
         }
         Some(Self {
@@ -61,7 +61,7 @@ impl SequenceExecutionHelper {
 
     pub fn run(
         &mut self,
-        table: &SequenceModeTable,
+        table: &SequenceModeTables,
         sender: &impl ModeRequestSender,
     ) -> Result<SequenceHandlerResult, GenericTargetedMessagingError> {
         if self.state == SequenceExecutionHelperStates::AwaitingCheckSuccess {
@@ -110,7 +110,6 @@ impl SequenceExecutionHelper {
     pub fn execute_sequence(
         request_id: RequestId,
         map_table: &SequenceTableMapTable,
-        //sequence_idx: usize,
         sender: &impl ModeRequestSender,
     ) -> Result<bool, GenericTargetedMessagingError> {
         let mut some_succes_check_required = false;
@@ -129,6 +128,4 @@ impl SequenceExecutionHelper {
 }
 
 #[cfg(test)]
-mod tests {
+mod tests {}
-    pub struct SatSystem {}
-}

satrs/tests/mode_tree.rs

@@ -2,11 +2,15 @@ use core::cell::Cell;
 use std::{println, sync::mpsc};
 
 use satrs::mode::{
-    ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler,
+    Mode, ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler,
-    ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestorAndHandlerMpscBounded,
+    ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestSender,
-    ModeRequestorBoundedMpsc,
+    ModeRequestorAndHandlerMpscBounded, ModeRequestorBoundedMpsc,
 };
-use satrs::request::MessageMetadata;
+use satrs::mode_tree::alloc_mod::{
+    ModeStoreVec, SequenceModeTables, SequenceTableMapValue, TargetModeTables, TargetTableMapValue,
+};
+use satrs::mode_tree::ModeStoreProvider;
+use satrs::request::{MessageMetadata, MessageSender};
 use satrs::{
     mode::{ModeAndSubmode, ModeReply, ModeRequest},
     queue::GenericTargetedMessagingError,
@@ -15,12 +19,63 @@ use satrs::{
 };
 use std::string::{String, ToString};
 
+const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(0xffffffff, 0);
+const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(0xffffffff - 1, 0);
+
 pub enum TestComponentId {
-    Device1 = 1,
+    MagnetometerDevice = 1,
-    Device2 = 2,
+    MagnetorquerDevice = 2,
-    Assembly = 3,
+    ReactionWheelDevice = 3,
-    Subsystem = 4,
+    StartrackerDevice = 4,
-    PusModeService = 5,
+    ReactionWheelAssembly = 8,
+    MagnetometerAssembly = 9,
+    AcsController = 14,
+    AcsSubsystem = 15,
+    PusModeService = 16,
+}
+
+pub type RequestSenderType = mpsc::SyncSender<GenericMessage<ModeRequest>>;
+pub type ReplySenderType = mpsc::SyncSender<GenericMessage<ModeReply>>;
+
+/// 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 {
+    type Sender: MessageSender<ModeRequest>;
+
+    fn id(&self) -> ComponentId;
+    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 {
+    type Sender: MessageSender<ModeReply>;
+
+    fn id(&self) -> ComponentId;
+    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<ReqSender, ReplySender>(
+    parent: &mut impl ModeParent<Sender=ReqSender>,
+    request_sender: ReqSender,
+    child: &mut impl ModeChild<Sender=ReplySender>,
+    reply_sender: ReplySender,
+) {
+    parent.add_mode_child(child.id(), request_sender);
+    child.add_mode_parent(parent.id(), reply_sender);
 }
 
 struct PusModeService {
@@ -29,11 +84,11 @@ struct PusModeService {
 }
 
 impl PusModeService {
-    pub fn send_announce_mode_cmd_to_assy(&self) {
+    pub fn send_announce_mode_cmd_to_subsystem(&self) {
         self.mode_node
             .send_mode_request(
                 self.request_id_counter.get(),
-                TestComponentId::Assembly as ComponentId,
+                TestComponentId::AcsSubsystem as ComponentId,
                 ModeRequest::AnnounceModeRecursive,
             )
             .unwrap();
@@ -42,21 +97,71 @@ impl PusModeService {
     }
 }
 
-struct TestSubsystem {
+impl ModeParent for PusModeService {
-    pub name: String,
+    type Sender = RequestSenderType;
+
+    fn id(&self) -> ComponentId {
+        TestComponentId::PusModeService as ComponentId
+    }
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) {
+        self.mode_node.add_message_target(id, request_sender);
+    }
+}
+
+struct AcsSubsystem {
     pub mode_node: ModeRequestorAndHandlerMpscBounded,
     pub mode_requestor_info: Option<MessageMetadata>,
     pub mode_and_submode: ModeAndSubmode,
     pub target_mode_and_submode: Option<ModeAndSubmode>,
+    pub children_mode_store: ModeStoreVec,
+    pub target_tables: TargetModeTables,
+    pub sequence_tables: SequenceModeTables,
 }
 
-impl ModeProvider for TestSubsystem {
+impl AcsSubsystem {
+    pub fn add_target_and_sequence_table(
+        &mut self,
+        mode: Mode,
+        target_table_val: TargetTableMapValue,
+        sequence_table_val: SequenceTableMapValue,
+    ) {
+        self.target_tables.0.insert(mode, target_table_val);
+        self.sequence_tables.0.insert(mode, sequence_table_val);
+    }
+}
+
+impl ModeParent for AcsSubsystem {
+    type Sender = RequestSenderType;
+
+    fn id(&self) -> ComponentId {
+        TestComponentId::PusModeService as ComponentId
+    }
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) {
+        self.children_mode_store.add_component(id, UNKNOWN_MODE);
+        self.mode_node.add_request_target(id, request_sender);
+    }
+}
+
+impl ModeChild for AcsSubsystem {
+    type Sender = ReplySenderType;
+
+    fn id(&self) -> ComponentId {
+        TestComponentId::PusModeService as ComponentId
+    }
+    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 {
         self.mode_and_submode
     }
 }
 
-impl ModeRequestHandler for TestSubsystem {
+impl ModeRequestHandler for AcsSubsystem {
     type Error = ModeError;
 
     fn start_transition(
@@ -75,7 +180,6 @@ impl ModeRequestHandler for TestSubsystem {
             "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;
@@ -126,20 +230,14 @@ impl ModeRequestHandler for TestSubsystem {
     }
 }
 
-struct TestAssembly {
+struct MgmAssembly {
     pub mode_node: ModeRequestorAndHandlerMpscBounded,
     pub mode_requestor_info: Option<MessageMetadata>,
     pub mode_and_submode: ModeAndSubmode,
     pub target_mode_and_submode: Option<ModeAndSubmode>,
 }
 
-impl ModeProvider for TestAssembly {
+impl MgmAssembly {
-    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");
@@ -196,7 +294,37 @@ impl TestAssembly {
     }
 }
 
-impl ModeRequestHandler for TestAssembly {
+impl ModeParent for MgmAssembly {
+    type Sender = RequestSenderType;
+
+    fn id(&self) -> ComponentId {
+        TestComponentId::AcsSubsystem as ComponentId
+    }
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) {
+        self.mode_node.add_request_target(id, request_sender);
+    }
+}
+
+impl ModeChild for MgmAssembly {
+    type Sender = ReplySenderType;
+
+    fn id(&self) -> ComponentId {
+        TestComponentId::PusModeService as ComponentId
+    }
+
+    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 MgmAssembly {
     type Error = ModeError;
     fn start_transition(
         &mut self,
@@ -262,13 +390,23 @@ impl ModeRequestHandler for TestAssembly {
     }
 }
 
-struct TestDevice {
+struct CommonDevice {
-    pub name: String,
+    name: String,
+    pub id: ComponentId,
     pub mode_node: ModeRequestHandlerMpscBounded,
     pub mode_and_submode: ModeAndSubmode,
 }
 
-impl TestDevice {
+impl CommonDevice {
+    pub fn new(name: String, id: ComponentId, mode_node: ModeRequestHandlerMpscBounded) -> Self {
+        Self {
+            name,
+            id,
+            mode_node,
+            mode_and_submode: ModeAndSubmode::new(0, 0),
+        }
+    }
+
     pub fn run(&mut self) {
         self.check_mode_requests().expect("mode messaging error");
     }
@@ -281,13 +419,25 @@ impl TestDevice {
     }
 }
 
-impl ModeProvider for TestDevice {
+impl ModeChild for CommonDevice {
+    type Sender = ReplySenderType;
+
+    fn id(&self) -> ComponentId {
+        self.id
+    }
+
+    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 TestDevice {
+impl ModeRequestHandler for CommonDevice {
     type Error = ModeError;
 
     fn start_transition(
@@ -338,22 +488,120 @@ impl ModeRequestHandler for TestDevice {
     }
 }
 
+pub struct AcsController {
+    pub mode_node: ModeRequestHandlerMpscBounded,
+    pub mode_and_submode: ModeAndSubmode,
+}
+
+impl AcsController {
+    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 ModeChild for AcsController {
+    type Sender = ReplySenderType;
+
+    fn id(&self) -> ComponentId {
+        TestComponentId::AcsController as u64
+    }
+
+    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,
+    ) -> Result<(), Self::Error> {
+        self.mode_and_submode = mode_and_submode;
+        self.handle_mode_reached(Some(requestor))?;
+        Ok(())
+    }
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
+        println!(
+            "ACS Controllerj: announcing mode: {:?}",
+            self.mode_and_submode
+        );
+    }
+
+    fn handle_mode_reached(
+        &mut self,
+        requestor_info: Option<MessageMetadata>,
+    ) -> Result<(), Self::Error> {
+        if let Some(requestor) = requestor_info {
+            self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?;
+        }
+        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
+        );
+        Ok(())
+    }
+
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), Self::Error> {
+        self.mode_node.send_mode_reply(requestor_info, reply)?;
+        Ok(())
+    }
+}
+
+#[test]
 fn main() {
     // All request channel handles.
-    let (request_sender_to_dev1, request_receiver_dev1) = mpsc::sync_channel(10);
+    let (request_sender_to_mgm_dev, request_receiver_mgm_dev) = mpsc::sync_channel(10);
-    let (request_sender_to_dev2, request_receiver_dev2) = mpsc::sync_channel(10);
+    let (request_sender_to_mgt_dev, request_receiver_mgt_dev) = mpsc::sync_channel(10);
-    let (request_sender_to_assy, request_receiver_assy) = mpsc::sync_channel(10);
+    let (request_sender_to_mgm_assy, request_receiver_mgm_assy) = mpsc::sync_channel(10);
-    let (request_sender_to_subsystem, request_receiver_subsystem) = 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_assy, reply_receiver_assy) = mpsc::sync_channel(10);
+    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);
 
     // Mode requestors and handlers.
-    let mut mode_node_assy = ModeRequestorAndHandlerMpscBounded::new(
+    let mut mgm_assy_node = ModeRequestorAndHandlerMpscBounded::new(
-        TestComponentId::Assembly as ComponentId,
+        TestComponentId::MagnetometerAssembly as ComponentId,
-        request_receiver_assy,
+        request_receiver_mgm_assy,
-        reply_receiver_assy,
+        reply_receiver_mgm_assy,
+    );
+    let mut acs_subsystem_node = ModeRequestorAndHandlerMpscBounded::new(
+        TestComponentId::AcsSubsystem as ComponentId,
+        request_receiver_acs_subsystem,
+        reply_receiver_acs_subsystem,
     );
     // Mode requestors only.
     let mut mode_node_pus = ModeRequestorBoundedMpsc::new(
@@ -362,88 +610,130 @@ fn main() {
     );
 
     // Request handlers only.
-    let mut mode_node_dev1 = ModeRequestHandlerMpscBounded::new(
+    let mut mgm_dev_node = ModeRequestHandlerMpscBounded::new(
-        TestComponentId::Device1 as ComponentId,
+        TestComponentId::MagnetometerDevice as ComponentId,
-        request_receiver_dev1,
+        request_receiver_mgm_dev,
     );
-    let mut mode_node_dev2 = ModeRequestHandlerMpscBounded::new(
+    let mut mgt_dev_node = ModeRequestHandlerMpscBounded::new(
-        TestComponentId::Device2 as ComponentId,
+        TestComponentId::MagnetorquerDevice as ComponentId,
-        request_receiver_dev2,
+        request_receiver_mgt_dev,
     );
-
+    let mut acs_ctrl_node = ModeRequestHandlerMpscBounded::new(
-    // Set up mode request senders first.
+        TestComponentId::AcsController as ComponentId,
-    mode_node_pus.add_message_target(
+        request_receiver_acs_ctrl,
-        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_pus.add_message_target(
-        TestComponentId::Subsystem as ComponentId,
-        request_sender_to_subsystem.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(
+    mgm_dev_node.add_message_target(
-        TestComponentId::Assembly as ComponentId,
+        TestComponentId::MagnetometerAssembly as ComponentId,
-        reply_sender_to_assy.clone(),
+        reply_sender_to_mgm_assy.clone(),
     );
-    mode_node_dev1.add_message_target(
+    mgt_dev_node.add_message_target(
+        TestComponentId::MagnetometerAssembly as ComponentId,
+        reply_sender_to_mgm_assy.clone(),
+    );
+    mgm_assy_node.add_reply_target(
         TestComponentId::PusModeService as ComponentId,
         reply_sender_to_pus.clone(),
     );
-    mode_node_dev2.add_message_target(
+    mgm_assy_node.add_reply_target(
-        TestComponentId::Assembly as ComponentId,
+        TestComponentId::AcsSubsystem 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 {
+    let mut mode_store_acs_subsystem = ModeStoreVec::default();
-        name: "Test Device 1".to_string(),
+    let mut target_tables_acs_subsystem = TargetModeTables::default();
-        mode_node: mode_node_dev1,
+    let mut sequence_tables_acs_subsystem = SequenceModeTables::default();
-        mode_and_submode: ModeAndSubmode::new(0, 0),
+
-    };
+    let mut mgm_dev = CommonDevice::new(
-    let mut device2 = TestDevice {
+        "MGM".to_string(),
-        name: "Test Device 2".to_string(),
+        TestComponentId::MagnetometerDevice as u64,
-        mode_node: mode_node_dev2,
+        mgm_dev_node,
-        mode_and_submode: ModeAndSubmode::new(0, 0),
+    );
-    };
+    let mut mgt_dev = CommonDevice::new(
-    let mut assy = TestAssembly {
+        "MGT".to_string(),
-        mode_node: mode_node_assy,
+        TestComponentId::MagnetorquerDevice as u64,
+        mgt_dev_node,
+    );
+    let mut mgm_assy = MgmAssembly {
+        mode_node: mgm_assy_node,
         mode_requestor_info: None,
         mode_and_submode: ModeAndSubmode::new(0, 0),
         target_mode_and_submode: None,
     };
-    let pus_service = PusModeService {
+    let mut acs_subsystem = AcsSubsystem {
+        mode_node: acs_subsystem_node,
+        mode_requestor_info: None,
+        mode_and_submode: ModeAndSubmode::new(0, 0),
+        target_mode_and_submode: None,
+        children_mode_store: mode_store_acs_subsystem,
+        target_tables: target_tables_acs_subsystem,
+        sequence_tables: sequence_tables_acs_subsystem,
+    };
+    let mut acs_ctrl = AcsController {
+        mode_node: acs_ctrl_node,
+        mode_and_submode: ModeAndSubmode::new(0, 0),
+    };
+    let mut pus_service = PusModeService {
         request_id_counter: Cell::new(0),
         mode_node: mode_node_pus,
     };
 
-    pus_service.send_announce_mode_cmd_to_assy();
+    // Connect the PUS mode service to all mode objects.
-    assy.run();
+    connect_mode_nodes(
-    device1.run();
+        &mut pus_service,
-    device2.run();
+        request_sender_to_acs_subsystem,
-    assy.run();
+        &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.clone(),
+        &mut mgm_dev,
+        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_dev,
+        &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 mgm_assy,
+        request_sender_to_mgm_dev,
+        &mut mgm_dev,
+        reply_sender_to_mgm_assy,
+    );
+
+    pus_service.send_announce_mode_cmd_to_subsystem();
+    mgm_assy.run();
+    mgm_dev.run();
+    mgt_dev.run();
 }