introduce forced flag for set mode cmd

satrs-example/src/acs/mgm.rs

@@ -386,6 +386,7 @@ impl<
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        _forced: bool,
     ) -> Result<(), satrs::mode::ModeError> {
         log::info!(
             "{}: transitioning to mode {:?}",
@@ -575,7 +576,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         testbench.handler.periodic_operation();
@@ -633,7 +637,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         testbench.handler.periodic_operation();

satrs-example/src/eps/pcdu.rs

@@ -412,6 +412,7 @@ impl<ComInterface: SerialInterface, TmSender: EcssTmSender> ModeRequestHandler
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        _forced: bool,
     ) -> Result<(), satrs::mode::ModeError> {
         log::info!(
             "{}: transitioning to mode {:?}",
@@ -660,7 +661,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap();
@@ -692,7 +696,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         testbench

satrs-example/src/main.rs

@@ -291,7 +291,10 @@ fn static_tmtc_pool_main() {
     pcdu_handler_mode_tx
         .send(GenericMessage::new(
             MessageMetadata::new(0, NO_SENDER),
-            ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)),
+            ModeRequest::SetMode {
+                mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0),
+                forced: false,
+            },
         ))
         .expect("sending initial mode request failed");
 
@@ -598,7 +601,10 @@ fn dyn_tmtc_pool_main() {
     pcdu_handler_mode_tx
         .send(GenericMessage::new(
             MessageMetadata::new(0, NO_SENDER),
-            ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)),
+            ModeRequest::SetMode {
+                mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0),
+                forced: false,
+            },
         ))
         .expect("sending initial mode request failed");
 

satrs-example/src/pus/mode.rs

@@ -191,7 +191,13 @@ impl PusTcToRequestConverter<ActivePusRequestStd, ModeRequest> for ModeRequestCo
                 }
                 let mode_and_submode = ModeAndSubmode::from_be_bytes(&tc.user_data()[4..])
                     .expect("mode and submode extraction failed");
-                Ok((active_request, ModeRequest::SetMode(mode_and_submode)))
+                Ok((
+                    active_request,
+                    ModeRequest::SetMode {
+                        mode_and_submode,
+                        forced: false,
+                    },
+                ))
             }
             Subservice::TcReadMode => Ok((active_request, ModeRequest::ReadMode)),
             Subservice::TcAnnounceMode => Ok((active_request, ModeRequest::AnnounceMode)),
@@ -347,7 +353,13 @@ mod tests {
         let (_active_req, req) = testbench
             .convert(token, &[], TEST_APID, TEST_UNIQUE_ID_0)
             .expect("conversion has failed");
-        assert_eq!(req, ModeRequest::SetMode(mode_and_submode));
+        assert_eq!(
+            req,
+            ModeRequest::SetMode {
+                mode_and_submode,
+                forced: false
+            }
+        );
     }
 
     #[test]

satrs/src/mode.rs

@@ -116,7 +116,10 @@ impl TargetedModeCommand {
 pub enum ModeRequest {
     /// Mode information. Can be used to notify other components of changed modes.
     ModeInfo(ModeAndSubmode),
-    SetMode(ModeAndSubmode),
+    SetMode {
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+    },
     ReadMode,
     AnnounceMode,
     AnnounceModeRecursive,
@@ -203,6 +206,7 @@ pub trait ModeRequestHandler: ModeProvider {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), Self::Error>;
 
     fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool);
@@ -229,9 +233,10 @@ pub trait ModeRequestHandler: ModeProvider {
         request: GenericMessage<ModeRequest>,
     ) -> Result<(), Self::Error> {
         match request.message {
-            ModeRequest::SetMode(mode_and_submode) => {
+            ModeRequest::SetMode {
-                self.start_transition(request.requestor_info, mode_and_submode)
+                mode_and_submode,
-            }
+                forced,
+            } => self.start_transition(request.requestor_info, mode_and_submode, forced),
             ModeRequest::ReadMode => self.send_mode_reply(
                 request.requestor_info,
                 ModeReply::ModeReply(self.mode_and_submode()),

satrs/src/pus/mod.rs

@@ -1221,9 +1221,10 @@ pub(crate) fn source_buffer_large_enough(
 
 #[cfg(any(feature = "test_util", test))]
 pub mod test_util {
-    use crate::request::UniqueApidTargetId;
     use spacepackets::ecss::{tc::PusTcCreator, tm::PusTmReader};
 
+    use crate::request::UniqueApidTargetId;
+
     use super::{
         verification::{self, TcStateAccepted, VerificationToken},
         DirectPusPacketHandlerResult, PusPacketHandlingError,
@@ -1232,6 +1233,7 @@ pub mod test_util {
     pub const TEST_APID: u16 = 0x101;
     pub const TEST_UNIQUE_ID_0: u32 = 0x05;
     pub const TEST_UNIQUE_ID_1: u32 = 0x06;
+
     pub const TEST_COMPONENT_ID_0: UniqueApidTargetId =
         UniqueApidTargetId::new(TEST_APID, TEST_UNIQUE_ID_0);
     pub const TEST_COMPONENT_ID_1: UniqueApidTargetId =
@@ -1268,14 +1270,13 @@ pub mod tests {
     use spacepackets::ecss::tm::{GenericPusTmSecondaryHeader, PusTmCreator, PusTmReader};
     use spacepackets::ecss::{PusPacket, WritablePusPacket};
     use spacepackets::CcsdsPacket;
+    use test_util::{TEST_APID, TEST_COMPONENT_ID_0};
 
     use crate::pool::{PoolProvider, SharedStaticMemoryPool, StaticMemoryPool, StaticPoolConfig};
     use crate::pus::verification::{RequestId, VerificationReporter};
     use crate::tmtc::{PacketAsVec, PacketInPool, PacketSenderWithSharedPool, SharedPacketPool};
     use crate::ComponentId;
 
-    use super::test_util::{TEST_APID, TEST_COMPONENT_ID_0};
-
     use super::verification::test_util::TestVerificationReporter;
     use super::verification::{
         TcStateAccepted, VerificationReporterCfg, VerificationReportingProvider, VerificationToken,

satrs/src/subsystem.rs

@@ -166,7 +166,10 @@ impl SequenceExecutionHelper {
             sender.send_mode_request(
                 request_id,
                 entry.common.target_id,
-                ModeRequest::SetMode(entry.common.mode_submode),
+                ModeRequest::SetMode {
+                    mode_and_submode: entry.common.mode_submode,
+                    forced: false,
+                },
             )?;
             mode_store_vec.0.iter_mut().for_each(|val| {
                 if val.id() == entry.common.target_id {

satrs/tests/mode_tree.rs

@@ -13,8 +13,10 @@ use satrs::mode_tree::{
     ModeStoreVec, SequenceModeTables, SequenceTablesMapValue, TargetModeTables,
     TargetTablesMapValue,
 };
-use satrs::request::MessageMetadata;
+use satrs::request::{MessageMetadata, RequestId};
-use satrs::subsystem::{SequenceExecutionHelper, SequenceHandlerResult, TargetKeepingResult};
+use satrs::subsystem::{
+    ModeDoesNotExistError, SequenceExecutionHelper, SequenceHandlerResult, TargetKeepingResult,
+};
 use satrs::{
     mode::{ModeAndSubmode, ModeReply, ModeRequest},
     queue::GenericTargetedMessagingError,
@@ -191,13 +193,23 @@ impl ModeTreeCommandingHelper {
         };
     }
 
+    pub fn start_command_sequence(
+        &mut self,
+        mode: Mode,
+        request_id: RequestId,
+    ) -> Result<(), ModeDoesNotExistError> {
+        self.helper.load(mode, request_id, &self.sequence_tables)?;
+        self.state = ModeTreeHelperState::SequenceCommanding;
+        Ok(())
+    }
+
     pub fn state_machine(
         &mut self,
-        opt_reply: Option<&GenericMessage<ModeReply>>,
+        opt_reply: Option<GenericMessage<ModeReply>>,
         req_sender: &impl ModeRequestSender,
     ) -> Result<ModeTreeHelperResult, ModeTreeHelperError> {
         if let Some(reply) = opt_reply {
-            self.handle_mode_reply(reply);
+            self.handle_mode_reply(&reply);
         }
         match self.state {
             ModeTreeHelperState::Idle => Ok(ModeTreeHelperResult::Idle),
@@ -280,6 +292,7 @@ impl ModeRequestHandler for ModeRequestHandlerMock {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        _forced: bool,
     ) -> Result<(), Self::Error> {
         self.start_transition_calls
             .push_back((requestor, mode_and_submode));
@@ -347,6 +360,21 @@ impl PusModeService {
         self.request_id_counter
             .replace(self.request_id_counter.get() + 1);
     }
+
+    pub fn send_mode_cmd(&self, mode: ModeAndSubmode) {
+        self.mode_node
+            .send_mode_request(
+                self.request_id_counter.get(),
+                TestComponentId::AcsSubsystem as ComponentId,
+                ModeRequest::SetMode {
+                    mode_and_submode: mode,
+                    forced: false,
+                },
+            )
+            .unwrap();
+        self.request_id_counter
+            .replace(self.request_id_counter.get() + 1);
+    }
 }
 
 impl ModeNode for PusModeService {
@@ -398,8 +426,23 @@ impl AcsSubsystem {
             self.handle_mode_request(request)
                 .expect("mode messaging error");
         }
-        if let Some(_reply) = self.mode_node.try_recv_mode_reply().unwrap() {
+        let mut mode_reply = None;
-            // TODO: Implementation.
+        if let Some(reply) = self.mode_node.try_recv_mode_reply().unwrap() {
+            mode_reply = Some(reply);
+        }
+        match self
+            .subsystem_helper
+            .state_machine(mode_reply, &self.mode_node)
+        {
+            Ok(result) => match result {
+                ModeTreeHelperResult::Idle => todo!(),
+                ModeTreeHelperResult::TargetKeeping(target_keeping_result) => todo!(),
+                ModeTreeHelperResult::SequenceCommanding(sequence_handler_result) => todo!(),
+            },
+            Err(error) => match error {
+                ModeTreeHelperError::Message(generic_targeted_messaging_error) => todo!(),
+                ModeTreeHelperError::CurrentModeNotInTargetTable(_) => todo!(),
+            },
         }
     }
     pub fn add_target_and_sequence_table(
@@ -454,13 +497,21 @@ impl ModeRequestHandler for AcsSubsystem {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), Self::Error> {
         self.mode_requestor_info = Some(requestor);
         self.target_mode_and_submode = Some(mode_and_submode);
         self.mode_req_handler_mock
-            .start_transition(requestor, mode_and_submode)
+            .start_transition(requestor, mode_and_submode, forced)
             .unwrap();
+        // TODO: CHeck if a transition is already active. For now, we do not allow a new transition
+        // if one is already active.
         // Execute mode map by executing the transition table(s).
+        // TODO: How to deal with error handling? Add this error to generic ModeError, or create
+        // new error type?
+        self.subsystem_helper
+            .start_command_sequence(mode_and_submode.mode(), requestor.request_id())
+            .unwrap();
         Ok(())
     }
 
@@ -629,11 +680,12 @@ impl ModeRequestHandler for MgmAssembly {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), Self::Error> {
         self.mode_requestor_info = Some(requestor);
         self.target_mode_and_submode = Some(mode_and_submode);
         self.mode_req_mock
-            .start_transition(requestor, mode_and_submode)
+            .start_transition(requestor, mode_and_submode, forced)
             .unwrap();
         Ok(())
     }
@@ -780,11 +832,12 @@ impl ModeRequestHandler for DeviceManager {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), ModeError> {
         self.mode_and_submode = mode_and_submode;
         self.handle_mode_reached(Some(requestor))?;
         self.mode_req_mock
-            .start_transition(requestor, mode_and_submode)
+            .start_transition(requestor, mode_and_submode, forced)
             .unwrap();
         Ok(())
     }
@@ -923,11 +976,12 @@ impl ModeRequestHandler for CommonDevice {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), ModeError> {
         self.mode_and_submode = mode_and_submode;
         self.handle_mode_reached(Some(requestor))?;
         self.mode_req_mock
-            .start_transition(requestor, mode_and_submode)
+            .start_transition(requestor, mode_and_submode, forced)
             .unwrap();
         Ok(())
     }
@@ -1031,11 +1085,12 @@ impl ModeRequestHandler for AcsController {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), Self::Error> {
         self.mode_and_submode = mode_and_submode;
         self.handle_mode_reached(Some(requestor))?;
         self.mode_req_mock
-            .start_transition(requestor, mode_and_submode)
+            .start_transition(requestor, mode_and_submode, forced)
             .unwrap();
         Ok(())
     }
@@ -1407,4 +1462,6 @@ fn command_safe_mode() {
     tb.mgt_dev.run();
     tb.mgm_devs[0].run();
     tb.mgm_devs[1].run();
+    tb.pus
+        .send_mode_cmd(ModeAndSubmode::new(AcsMode::IDLE as u32, 0));
 }

satrs/tests/pus_events.rs

@@ -6,7 +6,6 @@ use satrs::events::{EventU32, EventU32TypedSev, Severity, SeverityInfo};
 use satrs::params::U32Pair;
 use satrs::params::{Params, ParamsHeapless, WritableToBeBytes};
 use satrs::pus::event_man::{DefaultPusEventReportingMap, EventReporter, PusEventTmCreatorWithMap};
-use satrs::pus::test_util::TEST_COMPONENT_ID_0;
 use satrs::request::UniqueApidTargetId;
 use satrs::tmtc::PacketAsVec;
 use spacepackets::ecss::tm::PusTmReader;
@@ -100,10 +99,7 @@ fn test_threaded_usage() {
     // Event sender and TM checker thread
     let jh1 = thread::spawn(move || {
         event_tx
-            .send(EventMessage::new(
+            .send(EventMessage::new(TEST_ID.id(), INFO_EVENT.into()))
-                TEST_COMPONENT_ID_0.id(),
-                INFO_EVENT.into(),
-            ))
             .expect("Sending info event failed");
         loop {
             match event_packet_rx.try_recv() {
@@ -130,7 +126,7 @@ fn test_threaded_usage() {
         }
         event_tx
             .send(EventMessage::new_with_params(
-                TEST_COMPONENT_ID_0.id(),
+                TEST_ID.id(),
                 LOW_SEV_EVENT,
                 &Params::Heapless((2_u32, 3_u32).into()),
             ))