Merge pull request 'continue subsystem integration' (#272) from start-adding-acs-subsys into main

Reviewed-on: #272
2026-08-25 16:58:50 +02:00
parent 8195dac587 f6a87cff28
commit c40b8f32c9
7 changed files with 574 additions and 129 deletions
@@ -29,6 +29,7 @@ enum Commands {
    Mgm0(MgmArgs),
    Mgm1(MgmArgs),
    MgmAssy(MgmAssemblyArgs),
+    AcsSubsystem(SubsystemArgs),
 }

 impl Commands {
@@ -38,6 +39,7 @@ impl Commands {
            Commands::Mgm0(_mgm_args) => types::ComponentId::AcsMgm0,
            Commands::Mgm1(_mgm_args) => types::ComponentId::AcsMgm1,
            Commands::MgmAssy(_mgm_assembly_args) => types::ComponentId::AcsMgmAssembly,
+            Commands::AcsSubsystem(_subsystem_args) => types::ComponentId::AcsSubsystem,
        }
    }
 }
@@ -60,6 +62,14 @@ struct MgmAssemblyArgs {
    mode: Option<AssemblyModeSelect>,
 }

+#[derive(Debug, PartialEq, Eq, Clone, Copy, clap::Parser)]
+struct SubsystemArgs {
+    #[arg(short, long)]
+    ping: bool,
+    #[arg(short, long)]
+    mode: Option<SubsystemModeSelect>,
+}
+
 #[derive(Debug, PartialEq, Eq, Clone, Copy, clap::ValueEnum)]
 pub enum DeviceModeSelect {
    Off,
@@ -73,6 +83,12 @@ pub enum AssemblyModeSelect {
    Normal,
 }

+#[derive(Debug, PartialEq, Eq, Clone, Copy, clap::ValueEnum)]
+pub enum SubsystemModeSelect {
+    Off,
+    Safe,
+}
+
 fn setup_logger(level: log::LevelFilter) -> Result<(), fern::InitError> {
    fern::Dispatch::new()
        .format(|out, message, record| {
@@ -234,6 +250,45 @@ fn main() -> anyhow::Result<()> {
                    client.send_to(&request_packet, addr).unwrap();
                }
            }
+            Commands::AcsSubsystem(subsystem_args) => {
+                if subsystem_args.ping {
+                    let request = types::ccsds::CcsdsTcPacketOwned::new_with_request(
+                        SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)),
+                        TcHeader::new(cmd.target_id(), types::MessageType::Ping),
+                        types::acs::subsystem::request::Request::Ping,
+                    );
+                    let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header);
+                    log::info!(
+                        "sending {:?} ping request with TC ID {:#010x}",
+                        target_id,
+                        sent_tc_id.raw()
+                    );
+                    let request_packet = request.to_vec();
+                    client.send_to(&request_packet, addr).unwrap();
+                }
+                if let Some(mode) = subsystem_args.mode {
+                    let subsystem_mode = match mode {
+                        SubsystemModeSelect::Off => types::acs::subsystem::Mode::Off,
+                        SubsystemModeSelect::Safe => types::acs::subsystem::Mode::Safe,
+                    };
+
+                    let request = types::ccsds::CcsdsTcPacketOwned::new_with_request(
+                        SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)),
+                        TcHeader::new(target_id, types::MessageType::Mode),
+                        types::acs::subsystem::request::Request::Mode(
+                            types::acs::subsystem::request::ModeRequest::SetMode(subsystem_mode),
+                        ),
+                    );
+                    let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header);
+                    log::info!(
+                        "sending {:?} mode request with TC ID {:#010x}",
+                        target_id,
+                        sent_tc_id.raw()
+                    );
+                    let request_packet = request.to_vec();
+                    client.send_to(&request_packet, addr).unwrap();
+                }
+            }
        }
    }

@@ -320,7 +375,15 @@ fn handle_raw_tm_packet(data: &[u8]) -> anyhow::Result<()> {
                        postcard::from_bytes::<types::acs::mgm::response::Response>(remainder);
                    log::info!("Received response from MGM1: {:?}", response.unwrap());
                }
-                types::ComponentId::AcsSubsystem => todo!(),
+                types::ComponentId::AcsSubsystem => {
+                    let response = postcard::from_bytes::<types::acs::subsystem::response::Response>(
+                        remainder,
+                    );
+                    log::info!(
+                        "Received response from ACS subsystem: {:?}",
+                        response.unwrap()
+                    );
+                }
                types::ComponentId::EpsSubsystem => todo!(),
                types::ComponentId::UdpServer => todo!(),
                types::ComponentId::TcpServer => todo!(),
@@ -1 +1,118 @@
-// TODO: Write dummy controller
+use std::sync::mpsc::{Receiver, SyncSender, TryRecvError};
+
+use types::acs::ctrl::{Mode, request::ModeRequest, response::ModeReport};
+
+/// Helper component for communication with a parent component, which is usually an assembly
+/// or a subsystem.
+pub struct ModeLeafHelper {
+    pub request_rx: Receiver<ModeRequest>,
+    pub report_tx: SyncSender<ModeReport>,
+}
+
+/// Dummy ACS controller. It has no actual control law and reaches any commanded mode
+/// immediately, which is sufficient to exercise the mode commanding of its parent subsystem.
+pub struct Controller {
+    mode: Mode,
+    mode_leaf_helper: ModeLeafHelper,
+}
+
+impl Controller {
+    pub fn new(mode_leaf_helper: ModeLeafHelper) -> Self {
+        Self {
+            mode: Mode::Passive,
+            mode_leaf_helper,
+        }
+    }
+
+    #[allow(dead_code)]
+    #[inline]
+    pub fn mode(&self) -> Mode {
+        self.mode
+    }
+
+    pub fn periodic_operation(&mut self) {
+        self.handle_mode_leaf_handling();
+    }
+
+    fn handle_mode_leaf_handling(&mut self) {
+        loop {
+            match self.mode_leaf_helper.request_rx.try_recv() {
+                Ok(request) => match request {
+                    ModeRequest::SetMode(mode) => {
+                        log::info!("ACS controller: transitioning to mode {:?}", mode);
+                        self.mode = mode;
+                        self.report_mode();
+                    }
+                    ModeRequest::ReadMode => self.report_mode(),
+                },
+                Err(e) => match e {
+                    TryRecvError::Empty => break,
+                    TryRecvError::Disconnected => log::warn!("packet sender disconnected"),
+                },
+            }
+        }
+    }
+
+    fn report_mode(&self) {
+        self.mode_leaf_helper
+            .report_tx
+            .send(ModeReport::Mode(self.mode))
+            .expect("failed to send mode report to parent");
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use std::sync::mpsc;
+
+    use super::*;
+
+    struct ControllerTestbench {
+        request_tx: SyncSender<ModeRequest>,
+        report_rx: Receiver<ModeReport>,
+        controller: Controller,
+    }
+
+    impl ControllerTestbench {
+        fn new() -> Self {
+            let (request_tx, request_rx) = mpsc::sync_channel(5);
+            let (report_tx, report_rx) = mpsc::sync_channel(5);
+            Self {
+                request_tx,
+                report_rx,
+                controller: Controller::new(ModeLeafHelper {
+                    request_rx,
+                    report_tx,
+                }),
+            }
+        }
+    }
+
+    #[test]
+    fn test_initial_mode() {
+        let testbench = ControllerTestbench::new();
+        assert_eq!(testbench.controller.mode(), Mode::Passive);
+    }
+
+    #[test]
+    fn test_set_mode() {
+        let mut testbench = ControllerTestbench::new();
+        testbench
+            .request_tx
+            .send(ModeRequest::SetMode(Mode::Safe))
+            .unwrap();
+        testbench.controller.periodic_operation();
+        assert_eq!(testbench.controller.mode(), Mode::Safe);
+        let report = testbench.report_rx.try_recv().expect("no mode report sent");
+        assert_eq!(report, ModeReport::Mode(Mode::Safe));
+    }
+
+    #[test]
+    fn test_read_mode() {
+        let mut testbench = ControllerTestbench::new();
+        testbench.request_tx.send(ModeRequest::ReadMode).unwrap();
+        testbench.controller.periodic_operation();
+        let report = testbench.report_rx.try_recv().expect("no mode report sent");
+        assert_eq!(report, ModeReport::Mode(Mode::Passive));
+    }
+}
@@ -1 +1,119 @@
+use std::sync::mpsc::{Receiver, SyncSender, TryRecvError};

+use types::acs::mgt::{Mode, request::ModeRequest, response::ModeReport};
+
+/// Helper component for communication with a parent component, which is usually an assembly
+/// or a subsystem.
+pub struct ModeLeafHelper {
+    pub request_rx: Receiver<ModeRequest>,
+    pub report_tx: SyncSender<ModeReport>,
+}
+
+/// Dummy magnetorquer (MGT) device handler. It has no actual actuation logic and reaches any
+/// commanded mode immediately, which is sufficient to exercise the mode commanding of its
+/// parent subsystem.
+pub struct Mgt {
+    mode: Mode,
+    mode_leaf_helper: ModeLeafHelper,
+}
+
+impl Mgt {
+    pub fn new(mode_leaf_helper: ModeLeafHelper) -> Self {
+        Self {
+            mode: Mode::Off,
+            mode_leaf_helper,
+        }
+    }
+
+    #[allow(dead_code)]
+    #[inline]
+    pub fn mode(&self) -> Mode {
+        self.mode
+    }
+
+    pub fn periodic_operation(&mut self) {
+        self.handle_mode_leaf_handling();
+    }
+
+    fn handle_mode_leaf_handling(&mut self) {
+        loop {
+            match self.mode_leaf_helper.request_rx.try_recv() {
+                Ok(request) => match request {
+                    ModeRequest::SetMode(mode) => {
+                        log::info!("MGT: transitioning to mode {:?}", mode);
+                        self.mode = mode;
+                        self.report_mode();
+                    }
+                    ModeRequest::ReadMode => self.report_mode(),
+                },
+                Err(e) => match e {
+                    TryRecvError::Empty => break,
+                    TryRecvError::Disconnected => log::warn!("packet sender disconnected"),
+                },
+            }
+        }
+    }
+
+    fn report_mode(&self) {
+        self.mode_leaf_helper
+            .report_tx
+            .send(ModeReport::Mode(self.mode))
+            .expect("failed to send mode report to parent");
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use std::sync::mpsc;
+
+    use super::*;
+
+    struct MgtTestbench {
+        request_tx: SyncSender<ModeRequest>,
+        report_rx: Receiver<ModeReport>,
+        mgt: Mgt,
+    }
+
+    impl MgtTestbench {
+        fn new() -> Self {
+            let (request_tx, request_rx) = mpsc::sync_channel(5);
+            let (report_tx, report_rx) = mpsc::sync_channel(5);
+            Self {
+                request_tx,
+                report_rx,
+                mgt: Mgt::new(ModeLeafHelper {
+                    request_rx,
+                    report_tx,
+                }),
+            }
+        }
+    }
+
+    #[test]
+    fn test_initial_mode() {
+        let testbench = MgtTestbench::new();
+        assert_eq!(testbench.mgt.mode(), Mode::Off);
+    }
+
+    #[test]
+    fn test_set_mode() {
+        let mut testbench = MgtTestbench::new();
+        testbench
+            .request_tx
+            .send(ModeRequest::SetMode(Mode::Normal))
+            .unwrap();
+        testbench.mgt.periodic_operation();
+        assert_eq!(testbench.mgt.mode(), Mode::Normal);
+        let report = testbench.report_rx.try_recv().expect("no mode report sent");
+        assert_eq!(report, ModeReport::Mode(Mode::Normal));
+    }
+
+    #[test]
+    fn test_read_mode() {
+        let mut testbench = MgtTestbench::new();
+        testbench.request_tx.send(ModeRequest::ReadMode).unwrap();
+        testbench.mgt.periodic_operation();
+        let report = testbench.report_rx.try_recv().expect("no mode report sent");
+        assert_eq!(report, ModeReport::Mode(Mode::Off));
+    }
+}
@@ -5,9 +5,15 @@ use std::{
 };

 use satrs::{
-    mode_tree::{ModeStoreProvider, ModeStoreVec, SequenceModeTables, TargetModeTables},
+    mode_tree::{
+        ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry,
+        SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables,
+    },
    spacepackets::CcsdsPacketIdAndPsc,
-    subsystem::SubsystemCommandingHelper,
+    subsystem::{
+        ModeCommandingResult, ModeRaw, ModeResponse, ModeSetRequest, ModeTreeHelperError,
+        SubsystemCommandingHelper, SubsystemHelperResult,
+    },
 };
 use satrs_example::{ModeHelper, TmtcQueues};
 use types::{
@@ -15,11 +21,6 @@ use types::{
    acs::subsystem::{Mode, response},
 };

-#[derive(Debug)]
-pub struct TransitionInfo {
-    check_mode_reached: bool,
-}
-
 #[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
 pub enum TransitionState {
    #[default]
@@ -27,95 +28,139 @@ pub enum TransitionState {
    AwaitingReplies,
 }

-#[derive(Debug)]
-pub struct ChildModes {
-    mgm_assembly_mode: types::acs::mgm_assembly::Mode,
-    mgt_mode: types::acs::mgt::Mode,
-    controller_mode: types::acs::ctrl::Mode,
+fn build_sequence_tables() -> SequenceModeTables {
+    let mut seq_tables = SequenceModeTables::default();
+
+    let mut off_table = SequenceTablesMapValue::new("OFF");
+    let mut off_step_0 = SequenceTableMapTable::new("OFF_STEP_0");
+    off_step_0.add_entry(SequenceTableEntry::new(
+        "OFF_CTRL_PASSIVE",
+        ComponentId::AcsController as satrs::ComponentId,
+        types::acs::ctrl::Mode::Passive.into(),
+        true,
+    ));
+    off_table.add_sequence_table(off_step_0);
+    let mut off_step_1 = SequenceTableMapTable::new("OFF_STEP_1");
+    off_step_1.add_entry(SequenceTableEntry::new(
+        "OFF_MGM_ASSY_OFF",
+        ComponentId::AcsMgmAssembly as satrs::ComponentId,
+        types::acs::mgm_assembly::Mode::Device(types::DeviceMode::Off).into(),
+        false,
+    ));
+    off_step_1.add_entry(SequenceTableEntry::new(
+        "OFF_MGT_OFF",
+        ComponentId::AcsMgt as satrs::ComponentId,
+        types::acs::mgt::Mode::Off.into(),
+        false,
+    ));
+    off_table.add_sequence_table(off_step_1);
+    seq_tables.0.insert(Mode::Off as ModeRaw, off_table);
+
+    let mut safe_table = SequenceTablesMapValue::new("SAFE");
+    let mut safe_step_0 = SequenceTableMapTable::new("SAFE_STEP_0");
+    safe_step_0.add_entry(SequenceTableEntry::new(
+        "SAFE_MGM_ASSY_NORMAL",
+        ComponentId::AcsMgmAssembly as satrs::ComponentId,
+        types::acs::mgm_assembly::Mode::Device(types::DeviceMode::Normal).into(),
+        false,
+    ));
+    safe_step_0.add_entry(SequenceTableEntry::new(
+        "SAFE_MGT_NORMAL",
+        ComponentId::AcsMgt as satrs::ComponentId,
+        types::acs::mgt::Mode::Normal.into(),
+        false,
+    ));
+    safe_table.add_sequence_table(safe_step_0);
+    let mut safe_step_1 = SequenceTableMapTable::new("SAFE_STEP_1");
+    safe_step_1.add_entry(SequenceTableEntry::new(
+        "SAFE_CTRL_SAFE",
+        ComponentId::AcsController as satrs::ComponentId,
+        types::acs::ctrl::Mode::Safe.into(),
+        false,
+    ));
+    safe_table.add_sequence_table(safe_step_1);
+    seq_tables.0.insert(Mode::Safe as ModeRaw, safe_table);
+
+    seq_tables
 }

-#[derive(Debug)]
-pub struct TransitionCommands {
-    mgm_assembly_mode: Option<(types::acs::mgm_assembly::Mode, TransitionInfo)>,
-    mgt_mode: Option<(types::acs::mgt::Mode, TransitionInfo)>,
-    controller_mode: Option<(types::acs::ctrl::Mode, TransitionInfo)>,
+fn ctrl_response_to_mode_response(
+    response: types::acs::ctrl::response::ModeReport,
+) -> ModeResponse {
+    let sender_id = ComponentId::AcsController as satrs::ComponentId;
+    match response {
+        types::acs::ctrl::response::ModeReport::Mode(mode) => ModeResponse {
+            request_id: 0,
+            sender_id,
+            reported_mode: mode.into(),
+            success: true,
+        },
+        types::acs::ctrl::response::ModeReport::WrongMode(_) => ModeResponse {
+            request_id: 0,
+            sender_id,
+            reported_mode: 0,
+            success: false,
+        },
+    }
 }

-const OFF_SEQUENCE: [TransitionCommands; 2] = [
-    TransitionCommands {
-        mgm_assembly_mode: None,
-        mgt_mode: None,
-        controller_mode: Some((
-            types::acs::ctrl::Mode::Passive,
-            TransitionInfo {
-                check_mode_reached: true,
-            },
-        )),
-    },
-    TransitionCommands {
-        mgm_assembly_mode: Some((
-            types::acs::mgm_assembly::Mode::Device(types::DeviceMode::Off),
-            TransitionInfo {
-                check_mode_reached: false,
-            },
-        )),
-        mgt_mode: Some((
-            types::acs::mgt::Mode::Off,
-            TransitionInfo {
-                check_mode_reached: false,
-            },
-        )),
-        controller_mode: None,
-    },
-];
+fn mgm_assy_response_to_mode_response(
+    response: types::acs::mgm_assembly::response::ModeResponse,
+) -> ModeResponse {
+    let sender_id = ComponentId::AcsMgmAssembly as satrs::ComponentId;
+    match response {
+        types::acs::mgm_assembly::response::ModeResponse::Mode(mode) => ModeResponse {
+            request_id: 0,
+            sender_id,
+            reported_mode: mode.into(),
+            success: true,
+        },
+        types::acs::mgm_assembly::response::ModeResponse::SetModeTimeout(_)
+        | types::acs::mgm_assembly::response::ModeResponse::WrongMode(_)
+        | types::acs::mgm_assembly::response::ModeResponse::CanNotKeepMode(_) => ModeResponse {
+            request_id: 0,
+            sender_id,
+            reported_mode: 0,
+            success: false,
+        },
+    }
+}

-const SAFE_SEQUENCE: [TransitionCommands; 2] = [
-    TransitionCommands {
-        mgm_assembly_mode: Some((
-            types::acs::mgm_assembly::Mode::Device(types::DeviceMode::Normal),
-            TransitionInfo {
-                check_mode_reached: false,
-            },
-        )),
-        mgt_mode: Some((
-            types::acs::mgt::Mode::Normal,
-            TransitionInfo {
-                check_mode_reached: false,
-            },
-        )),
-        controller_mode: None,
-    },
-    TransitionCommands {
-        mgm_assembly_mode: None,
-        mgt_mode: None,
-        controller_mode: Some((
-            types::acs::ctrl::Mode::Safe,
-            TransitionInfo {
-                check_mode_reached: false,
-            },
-        )),
-    },
-];
+fn mgt_response_to_mode_response(response: types::acs::mgt::response::ModeReport) -> ModeResponse {
+    let sender_id = ComponentId::AcsMgt as satrs::ComponentId;
+    match response {
+        types::acs::mgt::response::ModeReport::Mode(mode) => ModeResponse {
+            request_id: 0,
+            sender_id,
+            reported_mode: mode.into(),
+            success: true,
+        },
+        types::acs::mgt::response::ModeReport::WrongMode(_) => ModeResponse {
+            request_id: 0,
+            sender_id,
+            reported_mode: 0,
+            success: false,
+        },
+    }
+}

 #[derive(Debug)]
 pub struct ModeRequestSenders {
    pub mode_request_ctrl: SyncSender<types::acs::ctrl::request::ModeRequest>,
-    pub mode_request_assy: SyncSender<types::acs::mgm_assembly::request::ModeRequest>,
+    pub mode_request_mgm_assy: SyncSender<types::acs::mgm_assembly::request::ModeRequest>,
    pub mode_request_mgt: SyncSender<types::acs::mgt::request::ModeRequest>,
 }

 #[derive(Debug)]
 pub struct ModeReportReceivers {
    pub mode_response_ctrl: Receiver<types::acs::ctrl::response::ModeReport>,
-    pub mode_response_assy: Receiver<types::acs::mgm_assembly::response::ModeResponse>,
+    pub mode_response_mgm_assy: Receiver<types::acs::mgm_assembly::response::ModeResponse>,
    pub mode_response_mgt: Receiver<types::acs::mgt::response::ModeReport>,
 }

 #[derive(Debug)]
 pub struct Subsystem {
    mode_helper: ModeHelper<types::acs::subsystem::Mode, TransitionState>,
-    transition_step: usize,
-    current_child_modes: Option<ChildModes>,
    mode_request_senders: ModeRequestSenders,
    mode_report_receivers: ModeReportReceivers,
    tmtc_queues: TmtcQueues,
@@ -152,18 +197,16 @@ impl Subsystem {

        let target_tables = TargetModeTables::default();

-        let sequence_tables = SequenceModeTables::default();
+        let sequence_tables = build_sequence_tables();

        Self {
            mode_helper: ModeHelper::new(
                types::acs::subsystem::Mode::Off,
                Duration::from_millis(2000),
            ),
-            current_child_modes: None,
            mode_request_senders,
            mode_report_receivers,
            tmtc_queues,
-            transition_step: 0,
            subsystem_helper: SubsystemCommandingHelper::new(
                mode_store_vec,
                target_tables,
@@ -174,6 +217,116 @@ impl Subsystem {

    pub fn periodic_operation(&mut self) {
        self.handle_telecommands();
+        let mut mode_requests = Vec::new();
+
+        while let Ok(response) = self.mode_report_receivers.mode_response_ctrl.try_recv() {
+            let result = self
+                .subsystem_helper
+                .state_machine(Some(ctrl_response_to_mode_response(response)), |request| {
+                    mode_requests.push(request)
+                });
+            self.handle_state_machine_result(result);
+        }
+        while let Ok(response) = self.mode_report_receivers.mode_response_mgm_assy.try_recv() {
+            let result = self.subsystem_helper.state_machine(
+                Some(mgm_assy_response_to_mode_response(response)),
+                |request| mode_requests.push(request),
+            );
+            self.handle_state_machine_result(result);
+        }
+        while let Ok(response) = self.mode_report_receivers.mode_response_mgt.try_recv() {
+            let result = self
+                .subsystem_helper
+                .state_machine(Some(mgt_response_to_mode_response(response)), |request| {
+                    mode_requests.push(request)
+                });
+            self.handle_state_machine_result(result);
+        }
+
+        let result = self
+            .subsystem_helper
+            .state_machine(None, |request| mode_requests.push(request));
+        self.handle_state_machine_result(result);
+
+        for request in mode_requests {
+            self.handle_mode_set_request(request);
+        }
+    }
+
+    fn handle_state_machine_result(
+        &mut self,
+        result: Result<SubsystemHelperResult, ModeTreeHelperError>,
+    ) {
+        match result {
+            Ok(result) => match result {
+                SubsystemHelperResult::Idle => (),
+                SubsystemHelperResult::TargetKeeping => (),
+                SubsystemHelperResult::ModeCommanding(mode_commanding_result) => {
+                    match mode_commanding_result {
+                        ModeCommandingResult::Done => {
+                            let target_mode_typed =
+                                self.subsystem_helper.seq_exec_helper.target_mode();
+                            let target_mode = target_mode_typed.map(Mode::try_from);
+                            log::info!(
+                                "ACS SS: mode commanding for target mode {:?} done",
+                                target_mode
+                            );
+                            self.finish_mode_transition();
+                        }
+                        ModeCommandingResult::StepDone => log::info!("mode commanding step done"),
+                        ModeCommandingResult::AwaitingSuccessCheck => {
+                            log::info!("ACS SS: mode commanding awaiting success check")
+                        }
+                    }
+                }
+            },
+            Err(e) => {
+                log::error!("mode tree helper error: {}", e);
+                self.mode_helper.finish(false);
+            }
+        }
+    }
+
+    fn finish_mode_transition(&mut self) {
+        let tc_commander = self.mode_helper.finish(true);
+        if let Some(requestor) = tc_commander {
+            self.send_telemetry(
+                Some(requestor),
+                response::Response::Mode(response::ModeResponse::Mode(self.mode_helper.current)),
+            );
+        }
+    }
+
+    fn handle_mode_set_request(&mut self, request: ModeSetRequest) {
+        let target_id = ComponentId::try_from(request.target_id);
+        if let Ok(target_id) = target_id {
+            match target_id {
+                ComponentId::AcsMgmAssembly => self
+                    .mode_request_senders
+                    .mode_request_mgm_assy
+                    .send(types::acs::mgm_assembly::request::ModeRequest::SetMode(
+                        types::acs::mgm_assembly::Mode::try_from(request.mode).unwrap(),
+                    ))
+                    .unwrap(),
+                ComponentId::AcsController => self
+                    .mode_request_senders
+                    .mode_request_ctrl
+                    .send(types::acs::ctrl::request::ModeRequest::SetMode(
+                        types::acs::ctrl::Mode::try_from(request.mode).unwrap(),
+                    ))
+                    .unwrap(),
+                ComponentId::AcsMgt => self
+                    .mode_request_senders
+                    .mode_request_mgt
+                    .send(types::acs::mgt::request::ModeRequest::SetMode(
+                        types::acs::mgt::Mode::try_from(request.mode).unwrap(),
+                    ))
+                    .unwrap(),
+                _ => {
+                    log::error!("invalid target ID {:?} for mode command", target_id);
+                }
+            }
+        }
    }

    pub fn handle_telecommands(&mut self) {
@@ -189,7 +342,7 @@ impl Subsystem {
                                self.send_telemetry(Some(tc_id), response::Response::Ok)
                            }
                            types::acs::subsystem::request::Request::Mode(mode_request) => {
-                                self.handle_mode_request(mode_request);
+                                self.handle_mode_request(Some(tc_id), mode_request);
                            }
                        },
                        Err(e) => {
@@ -205,50 +358,21 @@ impl Subsystem {
        }
    }

-    pub fn transition_sequence_for_mode(mode: Mode) -> &'static [TransitionCommands] {
-        match mode {
-            Mode::Off => &OFF_SEQUENCE,
-            Mode::Safe => &SAFE_SEQUENCE,
-        }
-    }
-
-    pub fn execute_transition_step(&mut self, step: &TransitionCommands) {
-        if let Some((target_mode, _info)) = &step.mgm_assembly_mode {
-            self.mode_request_senders
-                .mode_request_assy
-                .send(types::acs::mgm_assembly::request::ModeRequest::SetMode(
-                    *target_mode,
-                ))
-                .expect("failed to send mode request to MGM assembly");
-        }
-        if let Some((target_mode, _info)) = &step.mgt_mode {
-            self.mode_request_senders
-                .mode_request_mgt
-                .send(types::acs::mgt::request::ModeRequest::SetMode(*target_mode))
-                .expect("failed to send mode request to MGM assembly");
-        }
-        if let Some((target_mode, _info)) = &step.controller_mode {
-            self.mode_request_senders
-                .mode_request_ctrl
-                .send(types::acs::ctrl::request::ModeRequest::SetMode(
-                    *target_mode,
-                ))
-                .expect("failed to send mode request to MGM assembly");
-        }
-    }
-
    pub fn handle_mode_request(
        &mut self,
+        tc_id: Option<CcsdsPacketIdAndPsc>,
        mode_request: types::acs::subsystem::request::ModeRequest,
    ) {
        match mode_request {
            types::acs::subsystem::request::ModeRequest::SetMode(target_mode) => {
                self.mode_helper.start(target_mode);
-                self.transition_step = 0;
-                let first_step = Self::transition_sequence_for_mode(target_mode)
-                    .get(self.transition_step)
-                    .expect("empty transition table");
-                self.execute_transition_step(first_step);
+                self.mode_helper.tc_commander = tc_id;
+                if let Err(e) = self
+                    .subsystem_helper
+                    .start_command_sequence(target_mode as ModeRaw)
+                {
+                    log::error!("error starting command sequence: {}", e);
+                }
            }
            types::acs::subsystem::request::ModeRequest::ReadMode => {
                self.send_telemetry(
@@ -37,7 +37,7 @@ use tmtc::{tc_source::TcSourceTask, tm_sink::TmSink};
 use types::{ComponentId, DeviceMode};

 use crate::{
-    acs::{mgm, mgm_assembly, subsystem},
+    acs::{ctrl, mgm, mgm_assembly, mgt, subsystem},
    control::Controller,
    eps::pcdu::SwitchSet,
    event_manager::EventManager,
@@ -81,11 +81,11 @@ fn main() {
    let (pcdu_handler_tc_tx, pcdu_handler_tc_rx) = mpsc::sync_channel(30);
    let (controller_tc_tx, controller_tc_rx) = mpsc::sync_channel(10);

-    let (mgt_request_tx, _mgt_request_rx) = mpsc::sync_channel(5);
-    let (_mgt_report_tx, mgt_report_rx) = mpsc::sync_channel(5);
+    let (mgt_request_tx, mgt_request_rx) = mpsc::sync_channel(5);
+    let (mgt_report_tx, mgt_report_rx) = mpsc::sync_channel(5);

-    let (acs_ctrl_request_tx, _acs_ctrl_request_rx) = mpsc::sync_channel(5);
-    let (_acs_ctrl_response_tx, acs_ctrl_response_rx) = mpsc::sync_channel(5);
+    let (acs_ctrl_request_tx, acs_ctrl_request_rx) = mpsc::sync_channel(5);
+    let (acs_ctrl_response_tx, acs_ctrl_response_rx) = mpsc::sync_channel(5);

    // These message handles need to go into the MGM assembly and ACS subsystem.
    let (mgm_assembly_request_tx, mgm_assembly_request_rx) = mpsc::sync_channel(5);
@@ -226,15 +226,25 @@ fn main() {
        Duration::from_millis(2000),
    );

+    let mut acs_controller = ctrl::Controller::new(ctrl::ModeLeafHelper {
+        request_rx: acs_ctrl_request_rx,
+        report_tx: acs_ctrl_response_tx,
+    });
+
+    let mut acs_mgt = mgt::Mgt::new(mgt::ModeLeafHelper {
+        request_rx: mgt_request_rx,
+        report_tx: mgt_report_tx,
+    });
+
    let mut acs_subsystem = subsystem::Subsystem::new(
        subsystem::ModeRequestSenders {
            mode_request_ctrl: acs_ctrl_request_tx,
-            mode_request_assy: mgm_assembly_request_tx,
+            mode_request_mgm_assy: mgm_assembly_request_tx,
            mode_request_mgt: mgt_request_tx,
        },
        subsystem::ModeReportReceivers {
            mode_response_ctrl: acs_ctrl_response_rx,
-            mode_response_assy: mgm_assembly_report_rx,
+            mode_response_mgm_assy: mgm_assembly_report_rx,
            mode_response_mgt: mgt_report_rx,
        },
        TmtcQueues {
@@ -340,6 +350,8 @@ fn main() {
                mgm_0_handler.periodic_operation();
                mgm_1_handler.periodic_operation();
                mgm_assembly.periodic_operation();
+                acs_controller.periodic_operation();
+                acs_mgt.periodic_operation();
                acs_subsystem.periodic_operation();
                thread::sleep(Duration::from_millis(FREQ_MS_AOCS));
            }
@@ -1,7 +1,18 @@
-#[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)]
+#[derive(
+    serde::Serialize,
+    serde::Deserialize,
+    Debug,
+    Clone,
+    Copy,
+    PartialEq,
+    Eq,
+    num_enum::TryFromPrimitive,
+    num_enum::IntoPrimitive,
+)]
+#[repr(u32)]
 pub enum Mode {
-    Off,
-    Safe,
+    Off = 0,
+    Safe = 1,
 }

 #[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)]
@@ -23,7 +23,7 @@ pub mod pcdu;
    num_enum::TryFromPrimitive,
    num_enum::IntoPrimitive,
 )]
-#[repr(u64)]
+#[repr(u32)]
 pub enum ComponentId {
    Controller,