From 3f0937de835e5512004a17f7cd319c33ce9c6c1e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 11 May 2026 16:50:59 +0200 Subject: [PATCH] start adding ACS subsystem --- embedded-examples/embedded-client/Cargo.toml | 3 +- .../embedded-client/src/bin/stm32f3-client.rs | 2 +- .../embedded-client/src/bin/stm32h7-client.rs | 2 +- embedded-examples/models/Cargo.toml | 2 +- .../stm32f3-disco-rtic/Cargo.lock | 21 +- .../stm32f3-disco-rtic/Cargo.toml | 2 +- .../stm32h7-nucleo-rtic/Cargo.lock | 21 +- .../stm32h7-nucleo-rtic/Cargo.toml | 2 +- satrs-example/client/Cargo.toml | 2 +- satrs-example/client/src/main.rs | 37 +- satrs-example/models/Cargo.toml | 2 +- satrs-example/models/src/acs/ctrl.rs | 39 + satrs-example/models/src/{ => acs}/mgm.rs | 6 +- .../models/src/{ => acs}/mgm_assembly.rs | 51 +- satrs-example/models/src/acs/mgt.rs | 38 + satrs-example/models/src/acs/mod.rs | 5 + satrs-example/models/src/acs/subsystem.rs | 88 + satrs-example/models/src/lib.rs | 18 +- satrs-example/src/acs/mgm.rs | 45 +- satrs-example/src/acs/mgm_assembly.rs | 146 +- satrs-example/src/acs/mgt.rs | 1 + satrs-example/src/acs/mod.rs | 3 +- satrs-example/src/acs/subsystem.rs | 283 ++- satrs-example/src/eps/pcdu.rs | 5 +- satrs-example/src/lib.rs | 1 + satrs-example/src/main.rs | 43 +- satrs-shared/Cargo.toml | 3 +- satrs/Cargo.toml | 6 +- satrs/src/legacy/mode.rs | 763 ++++++++ satrs/src/legacy/mode_tree.rs | 614 +++++++ satrs/src/legacy/subsystem.rs | 1610 +++++++++++++++++ satrs/src/mode.rs | 761 +------- satrs/src/mode_tree.rs | 427 +++-- satrs/src/pus/mode.rs | 124 +- satrs/src/request.rs | 5 +- satrs/src/subsystem.rs | 1121 +++++------- 36 files changed, 4422 insertions(+), 1880 deletions(-) create mode 100644 satrs-example/models/src/acs/ctrl.rs rename satrs-example/models/src/{ => acs}/mgm.rs (95%) rename satrs-example/models/src/{ => acs}/mgm_assembly.rs (70%) create mode 100644 satrs-example/models/src/acs/mgt.rs create mode 100644 satrs-example/models/src/acs/mod.rs create mode 100644 satrs-example/models/src/acs/subsystem.rs create mode 100644 satrs-example/src/acs/mgt.rs create mode 100644 satrs/src/legacy/mode.rs create mode 100644 satrs/src/legacy/mode_tree.rs create mode 100644 satrs/src/legacy/subsystem.rs diff --git a/embedded-examples/embedded-client/Cargo.toml b/embedded-examples/embedded-client/Cargo.toml index c85ab41..bc4946b 100644 --- a/embedded-examples/embedded-client/Cargo.toml +++ b/embedded-examples/embedded-client/Cargo.toml @@ -8,8 +8,7 @@ clap = { version = "4", features = ["derive"] } serialport = "4" toml = "0.9" serde = { version = "1", features = ["derive"] } -satrs-stm32f3-disco-rtic = { path = "../stm32f3-disco-rtic" } -spacepackets = { version = "0.17" } +spacepackets = { version = "0.18" } embedded-models = { path = "../models" } tmtc-utils = { git = "https://egit.irs.uni-stuttgart.de/rust/tmtc-utils.git", version = "0.1", rev = "d8015379a45e77400dd9c71afbf22ca415ed67d1" } postcard = { version = "1", features = ["alloc"] } diff --git a/embedded-examples/embedded-client/src/bin/stm32f3-client.rs b/embedded-examples/embedded-client/src/bin/stm32f3-client.rs index 9e4232c..2d6d93c 100644 --- a/embedded-examples/embedded-client/src/bin/stm32f3-client.rs +++ b/embedded-examples/embedded-client/src/bin/stm32f3-client.rs @@ -66,6 +66,6 @@ fn main() -> anyhow::Result<()> { fn create_stm32f3_tc(request: &stm32f3::Request) -> CcsdsPacketCreatorOwned { let req_raw = postcard::to_allocvec(&request).unwrap(); - let sp_header = SpHeader::new_from_apid(satrs_stm32f3_disco_rtic::APID); + let sp_header = SpHeader::new_from_apid(embedded_models::stm32f3::PUS_APID); CcsdsPacketCreatorOwned::new_tc_with_checksum(sp_header, &req_raw).unwrap() } diff --git a/embedded-examples/embedded-client/src/bin/stm32h7-client.rs b/embedded-examples/embedded-client/src/bin/stm32h7-client.rs index 05626bc..93f4f5a 100644 --- a/embedded-examples/embedded-client/src/bin/stm32h7-client.rs +++ b/embedded-examples/embedded-client/src/bin/stm32h7-client.rs @@ -90,6 +90,6 @@ fn main() -> anyhow::Result<()> { fn create_stm32h7_tc(request: &stm32h7::Request) -> CcsdsPacketCreatorOwned { let req_raw = postcard::to_allocvec(&request).unwrap(); - let sp_header = SpHeader::new_from_apid(satrs_stm32f3_disco_rtic::APID); + let sp_header = SpHeader::new_from_apid(embedded_models::stm32h7::PUS_APID); CcsdsPacketCreatorOwned::new_tc_with_checksum(sp_header, &req_raw).unwrap() } diff --git a/embedded-examples/models/Cargo.toml b/embedded-examples/models/Cargo.toml index 2ec7843..57b8164 100644 --- a/embedded-examples/models/Cargo.toml +++ b/embedded-examples/models/Cargo.toml @@ -6,6 +6,6 @@ edition = "2024" [dependencies] serde = { version = "1", default-features = false } defmt = { version = "1", optional = true } -spacepackets = { version = "0.17", default-features = false, features = ["defmt", "serde"] } +spacepackets = { version = "0.18", default-features = false, features = ["defmt", "serde"] } postcard = { version = "1", features = ["defmt"] } arbitrary-int = "2" diff --git a/embedded-examples/stm32f3-disco-rtic/Cargo.lock b/embedded-examples/stm32f3-disco-rtic/Cargo.lock index 0a79331..0cd6c1d 100644 --- a/embedded-examples/stm32f3-disco-rtic/Cargo.lock +++ b/embedded-examples/stm32f3-disco-rtic/Cargo.lock @@ -20,12 +20,6 @@ dependencies = [ "as-slice", ] -[[package]] -name = "arbitrary-int" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "825297538d77367557b912770ca3083f778a196054b3ee63b22673c4a3cae0a5" - [[package]] name = "arbitrary-int" version = "2.1.1" @@ -83,11 +77,10 @@ checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitbybit" -version = "1.4.0" +version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec187a89ab07e209270175faf9e07ceb2755d984954e58a2296e325ddece2762" +checksum = "71d2a3353d70ac1091a33cbf31fc7e77b19091538a7e306e3740712af19807ca" dependencies = [ - "arbitrary-int 1.3.0", "proc-macro2", "quote", "syn 2.0.118", @@ -644,7 +637,7 @@ dependencies = [ name = "embedded-models" version = "0.1.0" dependencies = [ - "arbitrary-int 2.1.1", + "arbitrary-int", "defmt 1.1.1", "postcard", "serde", @@ -1159,7 +1152,7 @@ checksum = "cf54715a573b99ac80df0bc206da022bcd442c974952c7b9720069370852e21f" name = "satrs-stm32f3-disco-rtic" version = "0.1.0" dependencies = [ - "arbitrary-int 2.1.1", + "arbitrary-int", "cobs 0.5.1", "cortex-m", "cortex-m-rt", @@ -1282,11 +1275,11 @@ checksum = "8ed6a63f02c8539c91a8685a86f4099661ba3da017932f6ebbea6de3f0fa7c90" [[package]] name = "spacepackets" -version = "0.17.0" +version = "0.18.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94979a990b4333f43667cba9fe9e72b9c4e9ada82e09fbe8fb250844358590cc" +checksum = "93b9cf1b8372119e7c79383f1578f3e845b2ca2e6d07073e96f475ba354f44e3" dependencies = [ - "arbitrary-int 2.1.1", + "arbitrary-int", "bitbybit", "chrono", "crc", diff --git a/embedded-examples/stm32f3-disco-rtic/Cargo.toml b/embedded-examples/stm32f3-disco-rtic/Cargo.toml index 8f50218..9c33521 100644 --- a/embedded-examples/stm32f3-disco-rtic/Cargo.toml +++ b/embedded-examples/stm32f3-disco-rtic/Cargo.toml @@ -20,7 +20,7 @@ embassy-time = { version = "0.5", features = ["defmt", "generic-queue-16", "defm enumset = "1" heapless = "0.9" embassy-sync = "0.8" -spacepackets = { version = "0.17", default-features = false, features = ["defmt", "serde"] } +spacepackets = { version = "0.18", default-features = false, features = ["defmt", "serde"] } static_cell = "2" cobs = { version = "0.5", default-features = false, features = ["defmt"] } postcard = { version = "1" } diff --git a/embedded-examples/stm32h7-nucleo-rtic/Cargo.lock b/embedded-examples/stm32h7-nucleo-rtic/Cargo.lock index 65f18f6..153f813 100644 --- a/embedded-examples/stm32h7-nucleo-rtic/Cargo.lock +++ b/embedded-examples/stm32h7-nucleo-rtic/Cargo.lock @@ -20,12 +20,6 @@ dependencies = [ "as-slice", ] -[[package]] -name = "arbitrary-int" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "825297538d77367557b912770ca3083f778a196054b3ee63b22673c4a3cae0a5" - [[package]] name = "arbitrary-int" version = "2.1.1" @@ -89,11 +83,10 @@ checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitbybit" -version = "1.4.0" +version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec187a89ab07e209270175faf9e07ceb2755d984954e58a2296e325ddece2762" +checksum = "71d2a3353d70ac1091a33cbf31fc7e77b19091538a7e306e3740712af19807ca" dependencies = [ - "arbitrary-int 1.3.0", "proc-macro2", "quote", "syn 2.0.117", @@ -586,7 +579,7 @@ dependencies = [ name = "embedded-models" version = "0.1.0" dependencies = [ - "arbitrary-int 2.1.1", + "arbitrary-int", "defmt 1.1.0", "postcard", "serde", @@ -1028,7 +1021,7 @@ checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" name = "satrs-stm32h7-nucleo-rtic" version = "0.1.0" dependencies = [ - "arbitrary-int 2.1.1", + "arbitrary-int", "cortex-m", "cortex-m-rt", "defmt 1.1.0", @@ -1132,11 +1125,11 @@ dependencies = [ [[package]] name = "spacepackets" -version = "0.17.0" +version = "0.18.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94979a990b4333f43667cba9fe9e72b9c4e9ada82e09fbe8fb250844358590cc" +checksum = "93b9cf1b8372119e7c79383f1578f3e845b2ca2e6d07073e96f475ba354f44e3" dependencies = [ - "arbitrary-int 2.1.1", + "arbitrary-int", "bitbybit", "chrono", "crc", diff --git a/embedded-examples/stm32h7-nucleo-rtic/Cargo.toml b/embedded-examples/stm32h7-nucleo-rtic/Cargo.toml index 7e0788b..ff80346 100644 --- a/embedded-examples/stm32h7-nucleo-rtic/Cargo.toml +++ b/embedded-examples/stm32h7-nucleo-rtic/Cargo.toml @@ -25,7 +25,7 @@ panic-probe = { version = "1", features = ["print-defmt"] } embedded-alloc = "0.7" static_cell = "2" rtic = { version = "2", features = ["thumbv7-backend"] } -spacepackets = { version = "0.17", default-features = false, features = ["defmt"] } +spacepackets = { version = "0.18", default-features = false, features = ["defmt"] } postcard = "1" embassy-stm32 = { git = "https://github.com/embassy-rs/embassy.git", rev = "dd8e4c14e53f088bae27c5d841ab7a4fa338a52c", version = "0.6", features = ["stm32h743zi", "memory-x", "defmt", "time-driver-any"]} diff --git a/satrs-example/client/Cargo.toml b/satrs-example/client/Cargo.toml index 692333d..97ceb43 100644 --- a/satrs-example/client/Cargo.toml +++ b/satrs-example/client/Cargo.toml @@ -11,7 +11,7 @@ humantime = "2" serde = { version = "1" } satrs-example = { path = ".." } models = { path = "../models" } -spacepackets = { version = "0.17", git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git", default-features = false } +spacepackets = { version = "0.18", default-features = false } bitbybit = "2" arbitrary-int = "2" ctrlc = { version = "3.5" } diff --git a/satrs-example/client/src/main.rs b/satrs-example/client/src/main.rs index f6af5fa..d390467 100644 --- a/satrs-example/client/src/main.rs +++ b/satrs-example/client/src/main.rs @@ -1,7 +1,7 @@ use anyhow::bail; use arbitrary_int::u11; use clap::Parser as _; -use models::{Apid, MessageType, TcHeader, mgm::request::HkRequest}; +use models::{Apid, MessageType, TcHeader, acs::mgm::request::HkRequest}; use satrs_example::config::{OBSW_SERVER_ADDR, SERVER_PORT}; use spacepackets::{CcsdsPacketIdAndPsc, SpacePacketHeader}; use std::{ @@ -136,7 +136,7 @@ fn main() -> anyhow::Result<()> { let request = models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), TcHeader::new(cmd.target_id(), models::MessageType::Ping), - models::mgm::request::Request::Ping, + models::acs::mgm::request::Request::Ping, ); let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); log::info!( @@ -151,8 +151,8 @@ fn main() -> anyhow::Result<()> { let request = models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), TcHeader::new(target_id, models::MessageType::Hk), - models::mgm::request::Request::Hk(HkRequest { - id: models::mgm::request::HkId::Sensor, + models::acs::mgm::request::Request::Hk(HkRequest { + id: models::acs::mgm::request::HkId::Sensor, req_type: models::HkRequestType::OneShot, }), ); @@ -174,8 +174,8 @@ fn main() -> anyhow::Result<()> { let request = models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), TcHeader::new(target_id, models::MessageType::Mode), - models::mgm::request::Request::Mode( - models::mgm::request::ModeRequest::SetMode(dev_mode), + models::acs::mgm::request::Request::Mode( + models::acs::mgm::request::ModeRequest::SetMode(dev_mode), ), ); let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); @@ -193,7 +193,7 @@ fn main() -> anyhow::Result<()> { let request = models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), TcHeader::new(cmd.target_id(), models::MessageType::Ping), - models::mgm::request::Request::Ping, + models::acs::mgm::request::Request::Ping, ); let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); log::info!( @@ -207,21 +207,21 @@ fn main() -> anyhow::Result<()> { if let Some(mode) = mgm_assembly_args.mode { let assembly_mode = match mode { AssemblyModeSelect::NoModeKeeping => { - models::mgm_assembly::AssemblyMode::NoModeKeeping + models::acs::mgm_assembly::Mode::NoModeKeeping } AssemblyModeSelect::Off => { - models::mgm_assembly::AssemblyMode::Device(models::DeviceMode::Off) + models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Off) } AssemblyModeSelect::Normal => { - models::mgm_assembly::AssemblyMode::Device(models::DeviceMode::Normal) + models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Normal) } }; let request = models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), TcHeader::new(target_id, models::MessageType::Mode), - models::mgm_assembly::request::Request::Mode( - models::mgm_assembly::request::ModeRequest::SetMode(assembly_mode), + models::acs::mgm_assembly::request::Request::Mode( + models::acs::mgm_assembly::request::ModeRequest::SetMode(assembly_mode), ), ); let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); @@ -302,10 +302,10 @@ fn handle_raw_tm_packet(data: &[u8]) -> anyhow::Result<()> { postcard::from_bytes::(remainder); log::info!("Received response from controller: {:?}", response.unwrap()); } - models::ComponentId::AcsSubsystem => todo!(), models::ComponentId::AcsMgmAssembly => { - let response = - postcard::from_bytes::(remainder); + let response = postcard::from_bytes::< + models::acs::mgm_assembly::response::Response, + >(remainder); log::info!( "Received response from MGM Assembly: {:?}", response.unwrap() @@ -313,19 +313,22 @@ fn handle_raw_tm_packet(data: &[u8]) -> anyhow::Result<()> { } models::ComponentId::AcsMgm0 => { let response = - postcard::from_bytes::(remainder); + postcard::from_bytes::(remainder); log::info!("Received response from MGM0: {:?}", response.unwrap()); } models::ComponentId::AcsMgm1 => { let response = - postcard::from_bytes::(remainder); + postcard::from_bytes::(remainder); log::info!("Received response from MGM1: {:?}", response.unwrap()); } + models::ComponentId::AcsSubsystem => todo!(), models::ComponentId::EpsSubsystem => todo!(), models::ComponentId::UdpServer => todo!(), models::ComponentId::TcpServer => todo!(), models::ComponentId::Ground => todo!(), models::ComponentId::EventManager => {} + models::ComponentId::AcsController => todo!(), + models::ComponentId::AcsMgt => todo!(), } } Err(_) => todo!(), diff --git a/satrs-example/models/Cargo.toml b/satrs-example/models/Cargo.toml index e145bc4..df3fb6e 100644 --- a/satrs-example/models/Cargo.toml +++ b/satrs-example/models/Cargo.toml @@ -5,7 +5,7 @@ edition = "2024" [dependencies] serde = { version = "1", features = ["derive"] } -spacepackets = { version = "0.17", git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git", default-features = false } +spacepackets = { version = "0.18", default-features = false } satrs = { path = "../../satrs" } num_enum = { version = "0.7" } strum = { version = "0.28", features = ["derive"] } diff --git a/satrs-example/models/src/acs/ctrl.rs b/satrs-example/models/src/acs/ctrl.rs new file mode 100644 index 0000000..35fcb74 --- /dev/null +++ b/satrs-example/models/src/acs/ctrl.rs @@ -0,0 +1,39 @@ +#[derive( + serde::Serialize, + serde::Deserialize, + Debug, + Clone, + Copy, + PartialEq, + Eq, + num_enum::IntoPrimitive, + num_enum::TryFromPrimitive, +)] +#[repr(u32)] +pub enum Mode { + Passive, + Safe, + Idle, +} + +pub mod request { + use super::Mode; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeRequest { + SetMode(Mode), + ReadMode, + } +} + +pub mod response { + use super::Mode; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeReport { + /// Mode of the assembly. + Mode(super::Mode), + /// Children are in wrong mode after commanding. + WrongMode([Option; 2]), + } +} diff --git a/satrs-example/models/src/mgm.rs b/satrs-example/models/src/acs/mgm.rs similarity index 95% rename from satrs-example/models/src/mgm.rs rename to satrs-example/models/src/acs/mgm.rs index 0584542..8868f30 100644 --- a/satrs-example/models/src/mgm.rs +++ b/satrs-example/models/src/acs/mgm.rs @@ -42,7 +42,7 @@ pub mod request { } #[derive(Default, Debug, Copy, Clone, serde::Serialize, serde::Deserialize)] -pub struct MgmData { +pub struct SensorData { pub valid: bool, pub x: f32, pub y: f32, @@ -50,11 +50,11 @@ pub struct MgmData { } pub mod response { - use crate::{DeviceMode, Message, mgm::MgmData}; + use crate::{DeviceMode, Message, acs::mgm::SensorData}; #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] pub enum HkResponse { - MgmData(MgmData), + MgmData(SensorData), } #[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy)] diff --git a/satrs-example/models/src/mgm_assembly.rs b/satrs-example/models/src/acs/mgm_assembly.rs similarity index 70% rename from satrs-example/models/src/mgm_assembly.rs rename to satrs-example/models/src/acs/mgm_assembly.rs index f0f7417..eb1f1fa 100644 --- a/satrs-example/models/src/mgm_assembly.rs +++ b/satrs-example/models/src/acs/mgm_assembly.rs @@ -1,9 +1,12 @@ use core::str::FromStr; +use num_enum::TryFromPrimitive as _; +use satrs::mode::ModeRaw; + use crate::DeviceMode; #[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)] -pub enum AssemblyMode { +pub enum Mode { /// The assembly mode ressembles the modes of the devices it controls. It also tries to keep /// the children in the correct mode by re-commanding them into the correct mode. Device(DeviceMode), @@ -11,22 +14,48 @@ pub enum AssemblyMode { NoModeKeeping, } -impl FromStr for AssemblyMode { +impl From for ModeRaw { + fn from(value: Mode) -> Self { + match value { + Mode::Device(device_mode) => device_mode.into(), + Mode::NoModeKeeping => 5, + } + } +} + +impl TryFrom for Mode { + type Error = (); + + fn try_from(value: ModeRaw) -> Result { + match DeviceMode::try_from_primitive(value) { + Ok(val) => Ok(Mode::Device(val)), + Err(_) => { + if value == 5 { + Ok(Mode::NoModeKeeping) + } else { + Err(()) + } + } + } + } +} + +impl FromStr for Mode { type Err = (); fn from_str(s: &str) -> Result { match s.to_lowercase().as_str() { - "off" => Ok(AssemblyMode::Device(DeviceMode::Off)), - "on" => Ok(AssemblyMode::Device(DeviceMode::On)), - "normal" => Ok(AssemblyMode::Device(DeviceMode::Normal)), - "no_mode_keeping" => Ok(AssemblyMode::NoModeKeeping), + "off" => Ok(Mode::Device(DeviceMode::Off)), + "on" => Ok(Mode::Device(DeviceMode::On)), + "normal" => Ok(Mode::Device(DeviceMode::Normal)), + "no_mode_keeping" => Ok(Mode::NoModeKeeping), _ => Err(()), } } } pub mod request { - use crate::{HkRequestType, Message, mgm_assembly::AssemblyMode}; + use crate::{HkRequestType, Message, acs::mgm_assembly::Mode}; #[derive(Debug, PartialEq, Eq, Clone, Copy, serde::Serialize, serde::Deserialize)] pub enum HkId { @@ -35,7 +64,7 @@ pub mod request { #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] pub enum ModeRequest { - SetMode(AssemblyMode), + SetMode(Mode), ReadMode, } @@ -75,9 +104,9 @@ pub mod response { } #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] - pub enum ModeReport { + pub enum ModeResponse { /// Mode of the assembly. - Mode(super::AssemblyMode), + Mode(super::Mode), /// Timeout failure setting the children modes. SetModeTimeout([Option; 2]), /// Children are in wrong mode after commanding. @@ -89,7 +118,7 @@ pub mod response { #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug, PartialEq, Eq)] pub enum Response { Ok, - Mode(ModeReport), + Mode(ModeResponse), } impl Response { diff --git a/satrs-example/models/src/acs/mgt.rs b/satrs-example/models/src/acs/mgt.rs new file mode 100644 index 0000000..90122a0 --- /dev/null +++ b/satrs-example/models/src/acs/mgt.rs @@ -0,0 +1,38 @@ +#[derive( + serde::Serialize, + serde::Deserialize, + Debug, + Clone, + Copy, + PartialEq, + Eq, + num_enum::IntoPrimitive, + num_enum::TryFromPrimitive, +)] +#[repr(u32)] +pub enum Mode { + Off, + Normal, +} + +pub mod request { + use super::*; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeRequest { + SetMode(Mode), + ReadMode, + } +} + +pub mod response { + use super::*; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeReport { + /// Mode of the assembly. + Mode(super::Mode), + /// Children are in wrong mode after commanding. + WrongMode([Option; 2]), + } +} diff --git a/satrs-example/models/src/acs/mod.rs b/satrs-example/models/src/acs/mod.rs new file mode 100644 index 0000000..c7cb38d --- /dev/null +++ b/satrs-example/models/src/acs/mod.rs @@ -0,0 +1,5 @@ +pub mod ctrl; +pub mod mgm; +pub mod mgm_assembly; +pub mod mgt; +pub mod subsystem; diff --git a/satrs-example/models/src/acs/subsystem.rs b/satrs-example/models/src/acs/subsystem.rs new file mode 100644 index 0000000..cbcb52e --- /dev/null +++ b/satrs-example/models/src/acs/subsystem.rs @@ -0,0 +1,88 @@ +#[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)] +pub enum Mode { + Off, + Safe, +} + +#[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)] +pub enum ModeChild { + MgmAssembly, + Mgt, + Controller, +} + +impl Mode { + /// Each subsystem mode has a fallback mode to allow clean transitions. + pub const fn fallback_mode(&self) -> Self { + match self { + Mode::Off => Mode::Safe, + Mode::Safe => Mode::Safe, + } + } +} + +pub mod request { + use crate::Message; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeRequest { + SetMode(super::Mode), + ReadMode, + } + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] + pub enum Request { + Ping, + Mode(ModeRequest), + } + + impl Request { + fn message_type(&self) -> crate::MessageType { + match self { + Request::Ping => crate::MessageType::Verification, + Request::Mode(_mode) => crate::MessageType::Mode, + } + } + } + + impl Message for Request { + fn message_type(&self) -> crate::MessageType { + self.message_type() + } + } +} + +pub mod response { + use crate::Message; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeResponse { + /// Mode of the subsystem. + Mode(super::Mode), + /// Command timeout when commanding a child. + CommandTimeout(super::ModeChild), + /// Can not keep mode because a child changed mode unexpectedly. + CanNotKeepMode(super::ModeChild), + } + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] + pub enum Response { + Ok, + Mode(ModeResponse), + } + + impl Response { + fn message_type(&self) -> crate::MessageType { + match self { + Response::Ok => crate::MessageType::Verification, + Response::Mode(_mode_report) => crate::MessageType::Mode, + } + } + } + + impl Message for Response { + fn message_type(&self) -> crate::MessageType { + self.message_type() + } + } +} diff --git a/satrs-example/models/src/lib.rs b/satrs-example/models/src/lib.rs index f7351c4..bedbb04 100644 --- a/satrs-example/models/src/lib.rs +++ b/satrs-example/models/src/lib.rs @@ -6,10 +6,9 @@ use spacepackets::{ time::cds::{CdsTime, MIN_CDS_FIELD_LEN}, }; +pub mod acs; pub mod ccsds; pub mod control; -pub mod mgm; -pub mod mgm_assembly; pub mod pcdu; #[derive( @@ -30,8 +29,10 @@ pub enum ComponentId { AcsSubsystem, AcsMgmAssembly, + AcsController, AcsMgm0, AcsMgm1, + AcsMgt, EpsSubsystem, EpsPcdu, @@ -160,7 +161,18 @@ pub trait Message { /// The states are related both to the physical and the logical state of the device. Some /// device handlers control the power supply of their own device and an off state might also /// mean that the device is physically off. -#[derive(serde::Serialize, serde::Deserialize, Debug, PartialEq, Eq, Copy, Clone)] +#[derive( + serde::Serialize, + serde::Deserialize, + Debug, + PartialEq, + Eq, + Copy, + Clone, + num_enum::IntoPrimitive, + num_enum::TryFromPrimitive, +)] +#[repr(u32)] pub enum DeviceMode { Off = 0, On = 1, diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 94b5229..af98218 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -1,8 +1,8 @@ -use models::mgm::MgmData; -use models::mgm::request::ModeRequest; -use models::mgm::response::ModeResponse; +use models::acs::mgm::SensorData; +use models::acs::mgm::request::ModeRequest; +use models::acs::mgm::response::ModeResponse; use models::pcdu::SwitchId; -use models::{ComponentId, DeviceMode, HkRequestType, mgm}; +use models::{ComponentId, DeviceMode, HkRequestType, acs::mgm}; use satrs::spacepackets::CcsdsPacketIdAndPsc; use satrs_example::{HkHelperSingleSet, ModeHelper, TimestampHelper, TmtcQueues}; use satrs_minisim::acs::MgmRequestLis3Mdl; @@ -153,7 +153,7 @@ pub struct MgmHandlerLis3Mdl { switch_helper: PowerSwitchHelper, tmtc_queues: TmtcQueues, pub spi_com: SpiCommunication, - shared_mgm_set: Arc>, + shared_mgm_set: Arc>, buffers: BufWrapper, stamp_helper: TimestampHelper, hk_helper: HkHelperSingleSet, @@ -167,7 +167,7 @@ impl MgmHandlerLis3Mdl { tmtc_queues: TmtcQueues, switch_helper: PowerSwitchHelper, spi_com: SpiCommunication, - shared_mgm_set: Arc>, + shared_mgm_set: Arc>, mode_leaf_helper: ModeLeafHelper, mode_timeout: Duration, ) -> Self { @@ -308,7 +308,7 @@ impl MgmHandlerLis3Mdl { pub fn handle_hk_request( &mut self, tc_id: Option, - hk_request: &models::mgm::request::HkRequest, + hk_request: &models::acs::mgm::request::HkRequest, ) { match hk_request.req_type { HkRequestType::OneShot => { @@ -462,8 +462,8 @@ mod tests { use arbitrary_int::u11; use models::{ Apid, ComponentId, TcHeader, + acs::mgm::request::HkRequest, ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}, - mgm::request::HkRequest, pcdu::{SwitchRequest, SwitchState, SwitchStateBinary}, }; use satrs::{request::GenericMessage, spacepackets::SpacePacketHeader}; @@ -490,7 +490,7 @@ mod tests { pub fn create_request_tc( select: MgmSelect, - request: models::mgm::request::Request, + request: models::acs::mgm::request::Request, ) -> models::ccsds::CcsdsTcPacketOwned { models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), @@ -533,7 +533,7 @@ mod tests { SpiCommunication::Test(TestSpiInterface::default()), shared_mgm_set, mode_leaf_helper, - Duration::from_millis(200), + Duration::from_millis(100), ); Self { assembly_mode_request_tx, @@ -601,9 +601,10 @@ mod tests { assert_eq!(tm_packet.tm_header.sender_id, ComponentId::AcsMgm0); - let response = postcard::from_bytes::(&tm_packet.payload) - .expect("failed to deserialize mode reply"); - matches!(response, models::mgm::response::Response::Ok); + let response = + postcard::from_bytes::(&tm_packet.payload) + .expect("failed to deserialize mode reply"); + matches!(response, models::acs::mgm::response::Response::Ok); // The device should have been polled once. assert_eq!(testbench.test_spi_interface().call_count, 1); let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap(); @@ -689,9 +690,10 @@ mod tests { assert_eq!(tm_packet.tm_header.sender_id, ComponentId::AcsMgm0); - let response = postcard::from_bytes::(&tm_packet.payload) - .expect("failed to deserialize mode reply"); - if let models::mgm::response::Response::Hk(mgm::response::HkResponse::MgmData(data)) = + let response = + postcard::from_bytes::(&tm_packet.payload) + .expect("failed to deserialize mode reply"); + if let models::acs::mgm::response::Response::Hk(mgm::response::HkResponse::MgmData(data)) = response { assert_eq!(data.valid, false); @@ -740,17 +742,18 @@ mod tests { assert_eq!(mode_tm.tm_header.sender_id, ComponentId::AcsMgm0); - let response = postcard::from_bytes::(&mode_tm.payload) - .expect("failed to deserialize mode reply"); - matches!(response, models::mgm::response::Response::Ok); + let response = + postcard::from_bytes::(&mode_tm.payload) + .expect("failed to deserialize mode reply"); + matches!(response, models::acs::mgm::response::Response::Ok); let hk_tm = testbench.tm_rx.try_recv().expect("no hk reply generated"); assert_eq!(hk_tm.tm_header.sender_id, ComponentId::AcsMgm0); - let response = postcard::from_bytes::(&hk_tm.payload) + let response = postcard::from_bytes::(&hk_tm.payload) .expect("failed to deserialize mode reply"); - if let models::mgm::response::Response::Hk(mgm::response::HkResponse::MgmData(data)) = + if let models::acs::mgm::response::Response::Hk(mgm::response::HkResponse::MgmData(data)) = response { // Set is now valid. diff --git a/satrs-example/src/acs/mgm_assembly.rs b/satrs-example/src/acs/mgm_assembly.rs index 7d1683f..0d1b204 100644 --- a/satrs-example/src/acs/mgm_assembly.rs +++ b/satrs-example/src/acs/mgm_assembly.rs @@ -2,7 +2,7 @@ use std::{sync::mpsc, time::Duration}; use models::{ ComponentId, DeviceMode, - mgm_assembly::{AssemblyMode, request, response}, + acs::mgm_assembly::{Mode, request, response}, }; use satrs::spacepackets::CcsdsPacketIdAndPsc; use satrs_example::{ModeHelper, TmtcQueues}; @@ -11,13 +11,13 @@ use crate::ccsds::pack_ccsds_tm_packet_for_now; pub struct ParentQueueHelper { pub request_rx: mpsc::Receiver, - pub report_tx: mpsc::SyncSender, + pub report_tx: mpsc::SyncSender, } /// Helper component for communication with a parent component, which is usually as assembly. pub struct ChildrenQueueHelper { - pub request_tx_queues: [mpsc::SyncSender; 2], - pub report_rx_queues: [mpsc::Receiver; 2], + pub request_tx_queues: [mpsc::SyncSender; 2], + pub report_rx_queues: [mpsc::Receiver; 2], } #[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] @@ -35,7 +35,7 @@ pub struct MgmInfo { /// MGM assembly component. pub struct Assembly { - mode_helper: ModeHelper, + mode_helper: ModeHelper, /// This boolean is used for the distinction between transitions commanded by the parent /// or by ground, and transitions which were commanded autonomously as part of children /// mode keeping. @@ -56,7 +56,7 @@ impl Assembly { mode_timeout: Duration, ) -> Self { Self { - mode_helper: ModeHelper::new(AssemblyMode::NoModeKeeping, mode_timeout), + mode_helper: ModeHelper::new(Mode::NoModeKeeping, mode_timeout), mode_keeping_transition: false, tmtc_queues, mgm_modes: [MgmInfo::default(); 2], @@ -80,21 +80,21 @@ impl Assembly { match self.tmtc_queues.tc_rx.try_recv() { Ok(packet) => { let tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&packet.sp_header); - match postcard::from_bytes::( + match postcard::from_bytes::( &packet.payload, ) { Ok(request) => match request { - models::mgm_assembly::request::Request::Ping => { + models::acs::mgm_assembly::request::Request::Ping => { self.send_telemetry(Some(tc_id), response::Response::Ok) } - models::mgm_assembly::request::Request::Mode(request) => { + models::acs::mgm_assembly::request::Request::Mode(request) => { match request { request::ModeRequest::SetMode(assembly_mode) => { self.start_transition(false, assembly_mode, Some(tc_id)) } request::ModeRequest::ReadMode => self.send_telemetry( Some(tc_id), - response::Response::Mode(response::ModeReport::Mode( + response::Response::Mode(response::ModeResponse::Mode( self.mode(), )), ), @@ -117,7 +117,7 @@ impl Assembly { pub fn send_telemetry( &self, tc_id: Option, - response: models::mgm_assembly::response::Response, + response: models::acs::mgm_assembly::response::Response, ) { match pack_ccsds_tm_packet_for_now(Self::ID, tc_id, &response) { Ok(packet) => { @@ -136,17 +136,17 @@ impl Assembly { match self.parent_queues.request_rx.try_recv() { Ok(request) => match request { request::ModeRequest::SetMode(assembly_mode) => match assembly_mode { - AssemblyMode::Device(_device_mode) => { + Mode::Device(_device_mode) => { self.start_transition(false, assembly_mode, None); } - AssemblyMode::NoModeKeeping => { - self.mode_helper.current = AssemblyMode::NoModeKeeping; + Mode::NoModeKeeping => { + self.mode_helper.current = Mode::NoModeKeeping; } }, request::ModeRequest::ReadMode => self .parent_queues .report_tx - .send(response::ModeReport::Mode(self.mode_helper.current)) + .send(response::ModeResponse::Mode(self.mode_helper.current)) .unwrap(), }, Err(e) => match e { @@ -165,12 +165,12 @@ impl Assembly { loop { match rx.try_recv() { Ok(report) => match report { - models::mgm::response::ModeResponse::Mode(device_mode) => { + models::acs::mgm::response::ModeResponse::Mode(device_mode) => { self.mgm_modes[idx].mode = Some(device_mode); self.mgm_modes[idx].reply_received = true; mode_report_received = true; } - models::mgm::response::ModeResponse::SetModeTimeout => { + models::acs::mgm::response::ModeResponse::SetModeTimeout => { // Ignore, handle this with our own timeout. log::warn!("MGM {} mode timeout", idx); } @@ -191,23 +191,23 @@ impl Assembly { // Transition is active, check for completion. if self.mode_helper.transition_active() && self.mgm_modes.iter().all(|i| i.reply_received) - && let AssemblyMode::Device(device_mode) = self.mode_helper.target.unwrap() + && let Mode::Device(device_mode) = self.mode_helper.target.unwrap() { // If at least one child reached the correct mode, we are done. if self.mgm_modes.iter().any(|i| i.mode == Some(device_mode)) { self.handle_mode_reached(true); } else { let report = if self.mode_keeping_transition { - response::ModeReport::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) } else { - response::ModeReport::WrongMode(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::WrongMode(self.mgm_modes.map(|info| info.mode)) }; self.handle_mode_transition_failure(report); } } // Mode keeping active: Check children modes. - if let AssemblyMode::Device(device_mode) = self.mode_helper.current + if let Mode::Device(device_mode) = self.mode_helper.current && self .mgm_modes .iter() @@ -226,8 +226,8 @@ impl Assembly { } let target = self.mode_helper.target.unwrap(); let device_mode = match target { - AssemblyMode::Device(device_mode) => device_mode, - AssemblyMode::NoModeKeeping => { + Mode::Device(device_mode) => device_mode, + Mode::NoModeKeeping => { self.handle_mode_reached(true); return; } @@ -240,9 +240,9 @@ impl Assembly { && self.mode_helper.timed_out() { let report = if self.mode_keeping_transition { - response::ModeReport::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) } else { - response::ModeReport::SetModeTimeout(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::SetModeTimeout(self.mgm_modes.map(|info| info.mode)) }; self.handle_mode_transition_failure(report); } @@ -256,15 +256,15 @@ impl Assembly { } self.parent_queues .report_tx - .send(response::ModeReport::Mode(self.mode_helper.current)) + .send(response::ModeResponse::Mode(self.mode_helper.current)) .unwrap(); } - pub fn handle_mode_transition_failure(&mut self, report: response::ModeReport) { + pub fn handle_mode_transition_failure(&mut self, report: response::ModeResponse) { if self.mode_helper.tc_commander.is_some() { self.send_telemetry( self.mode_helper.tc_commander, - response::Response::Mode(response::ModeReport::SetModeTimeout( + response::Response::Mode(response::ModeResponse::SetModeTimeout( self.mgm_modes.map(|info| info.mode), )), ); @@ -275,7 +275,7 @@ impl Assembly { pub fn command_children(&self, mode: DeviceMode) { for tx in &self.children_queues.request_tx_queues { - tx.send(models::mgm::request::ModeRequest::SetMode(mode)) + tx.send(models::acs::mgm::request::ModeRequest::SetMode(mode)) .unwrap(); } } @@ -283,7 +283,7 @@ impl Assembly { pub fn start_transition( &mut self, mode_keeping: bool, - target: AssemblyMode, + target: Mode, tc_id: Option, ) { self.mode_keeping_transition = mode_keeping; @@ -304,7 +304,7 @@ impl Assembly { } #[inline] - pub fn mode(&self) -> AssemblyMode { + pub fn mode(&self) -> Mode { self.mode_helper.current } @@ -322,8 +322,8 @@ mod tests { use arbitrary_int::u11; use models::{ Apid, Message, MessageType, TcHeader, + acs::mgm_assembly, ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}, - mgm_assembly, }; use satrs::spacepackets::SpacePacketHeader; @@ -331,9 +331,9 @@ mod tests { pub struct Testbench { subsystem_req_tx: mpsc::SyncSender, - subsystem_report_rx: mpsc::Receiver, - mgm_request_rx: [mpsc::Receiver; 2], - mgm_report_tx: [mpsc::SyncSender; 2], + subsystem_report_rx: mpsc::Receiver, + mgm_request_rx: [mpsc::Receiver; 2], + mgm_report_tx: [mpsc::SyncSender; 2], tc_tx: mpsc::SyncSender, tm_rx: mpsc::Receiver, assembly: Assembly, @@ -396,7 +396,7 @@ mod tests { } pub fn create_request_tc( - request: models::mgm_assembly::request::Request, + request: models::acs::mgm_assembly::request::Request, ) -> models::ccsds::CcsdsTcPacketOwned { models::ccsds::CcsdsTcPacketOwned::new_with_request( SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), @@ -411,7 +411,7 @@ mod tests { tb.assert_all_queues_empty(); tb.assembly.periodic_operation(); tb.assert_all_queues_empty(); - assert_eq!(tb.assembly.mode(), AssemblyMode::NoModeKeeping); + assert_eq!(tb.assembly.mode(), Mode::NoModeKeeping); } #[test] @@ -419,7 +419,7 @@ mod tests { let mut tb = Testbench::new(); tb.tc_tx .send(create_request_tc(mgm_assembly::request::Request::Mode( - request::ModeRequest::SetMode(AssemblyMode::Device(DeviceMode::Normal)), + request::ModeRequest::SetMode(Mode::Device(DeviceMode::Normal)), ))) .unwrap(); tb.assembly.periodic_operation(); @@ -429,13 +429,13 @@ mod tests { let request = rx.try_recv().unwrap(); assert_eq!( request, - models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + models::acs::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) ); } // Confirm the mode is set. for tx in tb.mgm_report_tx.iter() { - tx.send(models::mgm::response::ModeResponse::Mode( + tx.send(models::acs::mgm::response::ModeResponse::Mode( DeviceMode::Normal, )) .unwrap(); @@ -443,7 +443,7 @@ mod tests { tb.assembly.periodic_operation(); assert!(!tb.assembly.mode_transition_active()); - assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + assert_eq!(tb.assembly.mode(), Mode::Device(DeviceMode::Normal)); let response = tb.tm_rx.try_recv().unwrap(); assert_eq!(response.tm_header.sender_id, Assembly::ID); @@ -456,7 +456,7 @@ mod tests { fn test_parent_commanded_transition() { let mut tb = Testbench::new(); tb.subsystem_req_tx - .send(request::ModeRequest::SetMode(AssemblyMode::Device( + .send(request::ModeRequest::SetMode(Mode::Device( DeviceMode::Normal, ))) .unwrap(); @@ -467,13 +467,13 @@ mod tests { let request = rx.try_recv().unwrap(); assert_eq!( request, - models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + models::acs::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) ); } // Confirm the mode is set. for tx in tb.mgm_report_tx.iter() { - tx.send(models::mgm::response::ModeResponse::Mode( + tx.send(models::acs::mgm::response::ModeResponse::Mode( DeviceMode::Normal, )) .unwrap(); @@ -481,12 +481,12 @@ mod tests { tb.assembly.periodic_operation(); assert!(!tb.assembly.mode_transition_active()); - assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + assert_eq!(tb.assembly.mode(), Mode::Device(DeviceMode::Normal)); let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::Mode(AssemblyMode::Device(DeviceMode::Normal)) + response::ModeResponse::Mode(Mode::Device(DeviceMode::Normal)) ); } @@ -494,7 +494,7 @@ mod tests { fn test_one_mgm_is_sufficient() { let mut tb = Testbench::new(); tb.subsystem_req_tx - .send(request::ModeRequest::SetMode(AssemblyMode::Device( + .send(request::ModeRequest::SetMode(Mode::Device( DeviceMode::Normal, ))) .unwrap(); @@ -505,28 +505,30 @@ mod tests { let request = rx.try_recv().unwrap(); assert_eq!( request, - models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + models::acs::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) ); } // One device is sufficient. tb.mgm_report_tx[0] - .send(models::mgm::response::ModeResponse::Mode( + .send(models::acs::mgm::response::ModeResponse::Mode( DeviceMode::Normal, )) .unwrap(); tb.mgm_report_tx[1] - .send(models::mgm::response::ModeResponse::Mode(DeviceMode::Off)) + .send(models::acs::mgm::response::ModeResponse::Mode( + DeviceMode::Off, + )) .unwrap(); tb.assembly.periodic_operation(); assert!(!tb.assembly.mode_transition_active()); - assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + assert_eq!(tb.assembly.mode(), Mode::Device(DeviceMode::Normal)); let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::Mode(AssemblyMode::Device(DeviceMode::Normal)) + response::ModeResponse::Mode(Mode::Device(DeviceMode::Normal)) ); } @@ -534,7 +536,7 @@ mod tests { fn test_mode_commanding_fails() { let mut tb = Testbench::new(); tb.subsystem_req_tx - .send(request::ModeRequest::SetMode(AssemblyMode::Device( + .send(request::ModeRequest::SetMode(Mode::Device( DeviceMode::Normal, ))) .unwrap(); @@ -545,24 +547,26 @@ mod tests { let request = rx.try_recv().unwrap(); assert_eq!( request, - models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + models::acs::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) ); } // Confirm the mode is set. for tx in tb.mgm_report_tx.iter() { - tx.send(models::mgm::response::ModeResponse::Mode(DeviceMode::Off)) - .unwrap(); + tx.send(models::acs::mgm::response::ModeResponse::Mode( + DeviceMode::Off, + )) + .unwrap(); } tb.assembly.periodic_operation(); assert!(!tb.assembly.mode_transition_active()); - assert_eq!(tb.assembly.mode(), AssemblyMode::NoModeKeeping); + assert_eq!(tb.assembly.mode(), Mode::NoModeKeeping); let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::WrongMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) + response::ModeResponse::WrongMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) ); } @@ -570,7 +574,7 @@ mod tests { fn test_mode_keeping_fails() { let mut tb = Testbench::new(); tb.subsystem_req_tx - .send(request::ModeRequest::SetMode(AssemblyMode::Device( + .send(request::ModeRequest::SetMode(Mode::Device( DeviceMode::Normal, ))) .unwrap(); @@ -581,13 +585,13 @@ mod tests { let request = rx.try_recv().unwrap(); assert_eq!( request, - models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + models::acs::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) ); } // Confirm the mode is set. for tx in tb.mgm_report_tx.iter() { - tx.send(models::mgm::response::ModeResponse::Mode( + tx.send(models::acs::mgm::response::ModeResponse::Mode( DeviceMode::Normal, )) .unwrap(); @@ -595,17 +599,19 @@ mod tests { tb.assembly.periodic_operation(); assert!(!tb.assembly.mode_transition_active()); - assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + assert_eq!(tb.assembly.mode(), Mode::Device(DeviceMode::Normal)); let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::Mode(AssemblyMode::Device(DeviceMode::Normal)) + response::ModeResponse::Mode(Mode::Device(DeviceMode::Normal)) ); for tx in tb.mgm_report_tx.iter() { - tx.send(models::mgm::response::ModeResponse::Mode(DeviceMode::Off)) - .unwrap(); + tx.send(models::acs::mgm::response::ModeResponse::Mode( + DeviceMode::Off, + )) + .unwrap(); } // This should start mode keeping. tb.assembly.periodic_operation(); @@ -615,20 +621,22 @@ mod tests { let request = rx.try_recv().unwrap(); assert_eq!( request, - models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + models::acs::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) ); } // Let the mode keeping fail. for tx in tb.mgm_report_tx.iter() { - tx.send(models::mgm::response::ModeResponse::Mode(DeviceMode::Off)) - .unwrap(); + tx.send(models::acs::mgm::response::ModeResponse::Mode( + DeviceMode::Off, + )) + .unwrap(); } tb.assembly.periodic_operation(); let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::CanNotKeepMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) + response::ModeResponse::CanNotKeepMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) ); } } diff --git a/satrs-example/src/acs/mgt.rs b/satrs-example/src/acs/mgt.rs new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/satrs-example/src/acs/mgt.rs @@ -0,0 +1 @@ + diff --git a/satrs-example/src/acs/mod.rs b/satrs-example/src/acs/mod.rs index 60382dd..c7cb38d 100644 --- a/satrs-example/src/acs/mod.rs +++ b/satrs-example/src/acs/mod.rs @@ -1,6 +1,5 @@ pub mod ctrl; - pub mod mgm; pub mod mgm_assembly; - +pub mod mgt; pub mod subsystem; diff --git a/satrs-example/src/acs/subsystem.rs b/satrs-example/src/acs/subsystem.rs index ec11077..c36a806 100644 --- a/satrs-example/src/acs/subsystem.rs +++ b/satrs-example/src/acs/subsystem.rs @@ -1 +1,282 @@ -// TODO: Write subsystem +#![allow(dead_code)] +use std::{ + sync::mpsc::{self, Receiver, SyncSender}, + time::Duration, +}; + +use models::{ + ComponentId, + acs::subsystem::{Mode, response}, +}; +use satrs::{ + mode_tree::{ModeStoreProvider, ModeStoreVec, SequenceModeTables, TargetModeTables}, + spacepackets::CcsdsPacketIdAndPsc, + subsystem::SubsystemCommandingHelper, +}; +use satrs_example::{ModeHelper, TmtcQueues}; + +#[derive(Debug)] +pub struct TransitionInfo { + check_mode_reached: bool, +} + +#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] +pub enum TransitionState { + #[default] + Idle, + AwaitingReplies, +} + +#[derive(Debug)] +pub struct ChildModes { + mgm_assembly_mode: models::acs::mgm_assembly::Mode, + mgt_mode: models::acs::mgt::Mode, + controller_mode: models::acs::ctrl::Mode, +} + +#[derive(Debug)] +pub struct TransitionCommands { + mgm_assembly_mode: Option<(models::acs::mgm_assembly::Mode, TransitionInfo)>, + mgt_mode: Option<(models::acs::mgt::Mode, TransitionInfo)>, + controller_mode: Option<(models::acs::ctrl::Mode, TransitionInfo)>, +} + +const OFF_SEQUENCE: [TransitionCommands; 2] = [ + TransitionCommands { + mgm_assembly_mode: None, + mgt_mode: None, + controller_mode: Some(( + models::acs::ctrl::Mode::Passive, + TransitionInfo { + check_mode_reached: true, + }, + )), + }, + TransitionCommands { + mgm_assembly_mode: Some(( + models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Off), + TransitionInfo { + check_mode_reached: false, + }, + )), + mgt_mode: Some(( + models::acs::mgt::Mode::Off, + TransitionInfo { + check_mode_reached: false, + }, + )), + controller_mode: None, + }, +]; + +const SAFE_SEQUENCE: [TransitionCommands; 2] = [ + TransitionCommands { + mgm_assembly_mode: Some(( + models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Normal), + TransitionInfo { + check_mode_reached: false, + }, + )), + mgt_mode: Some(( + models::acs::mgt::Mode::Normal, + TransitionInfo { + check_mode_reached: false, + }, + )), + controller_mode: None, + }, + TransitionCommands { + mgm_assembly_mode: None, + mgt_mode: None, + controller_mode: Some(( + models::acs::ctrl::Mode::Safe, + TransitionInfo { + check_mode_reached: false, + }, + )), + }, +]; + +#[derive(Debug)] +pub struct ModeRequestSenders { + pub mode_request_ctrl: SyncSender, + pub mode_request_assy: SyncSender, + pub mode_request_mgt: SyncSender, +} + +#[derive(Debug)] +pub struct ModeReportReceivers { + pub mode_response_ctrl: Receiver, + pub mode_response_assy: Receiver, + pub mode_response_mgt: Receiver, +} + +#[derive(Debug)] +pub struct Subsystem { + mode_helper: ModeHelper, + transition_step: usize, + current_child_modes: Option, + mode_request_senders: ModeRequestSenders, + mode_report_receivers: ModeReportReceivers, + tmtc_queues: TmtcQueues, + subsystem_helper: SubsystemCommandingHelper, +} + +impl Subsystem { + pub const ID: ComponentId = ComponentId::AcsSubsystem; + + pub fn new( + mode_request_senders: ModeRequestSenders, + mode_report_receivers: ModeReportReceivers, + tmtc_queues: TmtcQueues, + ) -> Self { + let mut mode_store_vec = ModeStoreVec::default(); + mode_store_vec + .add_component( + ComponentId::AcsMgmAssembly as satrs::ComponentId, + models::acs::mgm_assembly::Mode::NoModeKeeping.into(), + ) + .unwrap(); + mode_store_vec + .add_component( + ComponentId::AcsController as satrs::ComponentId, + models::acs::ctrl::Mode::Passive.into(), + ) + .unwrap(); + mode_store_vec + .add_component( + ComponentId::AcsMgt as satrs::ComponentId, + models::acs::mgt::Mode::Off.into(), + ) + .unwrap(); + + let target_tables = TargetModeTables::default(); + + let sequence_tables = SequenceModeTables::default(); + + Self { + mode_helper: ModeHelper::new( + models::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, + sequence_tables, + ), + } + } + + pub fn periodic_operation(&mut self) { + self.handle_telecommands(); + } + + pub fn handle_telecommands(&mut self) { + loop { + match self.tmtc_queues.tc_rx.try_recv() { + Ok(packet) => { + let tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&packet.sp_header); + match postcard::from_bytes::( + &packet.payload, + ) { + Ok(request) => match request { + models::acs::subsystem::request::Request::Ping => { + self.send_telemetry(Some(tc_id), response::Response::Ok) + } + models::acs::subsystem::request::Request::Mode(mode_request) => { + self.handle_mode_request(mode_request); + } + }, + Err(e) => { + log::warn!("failed to deserialize request: {}", e); + } + } + } + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => log::warn!("packet sender disconnected"), + }, + } + } + } + + 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(models::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(models::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(models::acs::ctrl::request::ModeRequest::SetMode( + *target_mode, + )) + .expect("failed to send mode request to MGM assembly"); + } + } + + pub fn handle_mode_request( + &mut self, + mode_request: models::acs::subsystem::request::ModeRequest, + ) { + match mode_request { + models::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); + } + models::acs::subsystem::request::ModeRequest::ReadMode => { + self.send_telemetry( + None, + response::Response::Mode(response::ModeResponse::Mode( + self.mode_helper.current, + )), + ); + } + } + } + + pub fn send_telemetry( + &self, + tc_id: Option, + response: models::acs::subsystem::response::Response, + ) { + match crate::ccsds::pack_ccsds_tm_packet_for_now(Self::ID, tc_id, &response) { + Ok(packet) => { + if let Err(e) = self.tmtc_queues.tm_tx.send(packet) { + log::warn!("failed to send TM packet: {}", e); + } + } + Err(e) => { + log::warn!("failed to pack TM packet: {}", e); + } + } + } +} diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 9144490..22cf638 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -536,7 +536,6 @@ mod tests { pcdu::{SwitchMapBinary, SwitchStateBinary}, }; use satrs::{ - mode::{ModeReply, ModeRequest}, request::{GenericMessage, MessageMetadata}, spacepackets::SpacePacketHeader, }; @@ -590,8 +589,8 @@ mod tests { #[allow(dead_code)] pub struct PcduTestbench { - pub mode_request_tx: mpsc::SyncSender>, - pub mode_reply_rx_to_parent: mpsc::Receiver>, + pub mode_request_tx: mpsc::SyncSender, + pub mode_reply_rx_to_parent: mpsc::Receiver, pub tc_tx: mpsc::SyncSender, pub tm_rx: mpsc::Receiver, pub switch_request_tx: mpsc::Sender>, diff --git a/satrs-example/src/lib.rs b/satrs-example/src/lib.rs index ee16549..00f376e 100644 --- a/satrs-example/src/lib.rs +++ b/satrs-example/src/lib.rs @@ -95,6 +95,7 @@ impl HkHelperSingleSet { } } +#[derive(Debug)] pub struct TmtcQueues { pub tc_rx: mpsc::Receiver, pub tm_tx: mpsc::SyncSender, diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index bdef7f4..6d2bf0f 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -23,16 +23,13 @@ use logger::setup_logger; use models::{ComponentId, DeviceMode}; use satrs::{ hal::std::{tcp_server::ServerConfig, udp_server::UdpTcServer}, - mode::{Mode, ModeAndSubmode, ModeRequest}, pus::HandlingStatus, - request::{GenericMessage, MessageMetadata}, spacepackets::time::cds::CdsTime, }; use satrs_example::{ TmtcQueues, config::{ OBSW_SERVER_ADDR, PACKET_ID_VALIDATOR, SERVER_PORT, - components::NO_SENDER, tasks::{FREQ_MS_AOCS, FREQ_MS_CONTROLLER, FREQ_MS_UDP_TMTC, SIM_CLIENT_IDLE_DELAY_MS}, }, }; @@ -40,7 +37,7 @@ use tmtc::sender::TmTcSender; use tmtc::{tc_source::TcSourceTask, tm_sink::TmSink}; use crate::{ - acs::{mgm, mgm_assembly}, + acs::{mgm, mgm_assembly, subsystem}, control::Controller, eps::pcdu::SwitchSet, event_manager::EventManager, @@ -80,12 +77,19 @@ fn main() { let (mgm_0_handler_tc_tx, mgm_0_handler_tc_rx) = mpsc::sync_channel(10); let (mgm_1_handler_tc_tx, mgm_1_handler_tc_rx) = mpsc::sync_channel(10); let (mgm_assembly_tc_tx, mgm_assembly_tc_rx) = mpsc::sync_channel(10); + let (acs_subsystem_tc_tx, acs_subsystem_tc_rx) = mpsc::sync_channel(10); 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 (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); - let (mgm_assembly_report_tx, _mgm_assembly_report_rx) = mpsc::sync_channel(5); + let (mgm_assembly_request_tx, mgm_assembly_request_rx) = mpsc::sync_channel(5); + let (mgm_assembly_report_tx, mgm_assembly_report_rx) = mpsc::sync_channel(5); // These message handles need to go into the MGM assembly and MGM devices. let (mgm_0_mode_request_tx, mgm_0_mode_request_rx) = mpsc::sync_channel(5); @@ -110,6 +114,7 @@ fn main() { tc_source.add_target(ComponentId::AcsMgm0, mgm_0_handler_tc_tx); tc_source.add_target(ComponentId::AcsMgm1, mgm_1_handler_tc_tx); tc_source.add_target(ComponentId::AcsMgmAssembly, mgm_assembly_tc_tx); + tc_source.add_target(ComponentId::AcsSubsystem, acs_subsystem_tc_tx); let tc_sender = TmTcSender::Normal(tc_source_tx.clone()); let udp_tm_handler = UdpTmHandlerWithChannel { @@ -221,6 +226,23 @@ fn main() { Duration::from_millis(2000), ); + 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_mgt: mgt_request_tx, + }, + subsystem::ModeReportReceivers { + mode_response_ctrl: acs_ctrl_response_rx, + mode_response_assy: mgm_assembly_report_rx, + mode_response_mgt: mgt_report_rx, + }, + TmtcQueues { + tc_rx: acs_subsystem_tc_rx, + tm_tx: tm_sink_tx.clone(), + }, + ); + let pcdu_serial_interface = if let Some(sim_client) = opt_sim_client.as_mut() { sim_client.add_reply_recipient(satrs_minisim::SimComponent::Pcdu, pcdu_sim_reply_tx); SerialSimInterfaceWrapper::Sim(SerialInterfaceToSim::new( @@ -241,13 +263,7 @@ fn main() { // The PCDU is a critical component which should be in normal mode immediately. pcdu_handler_mode_tx - .send(GenericMessage::new( - MessageMetadata::new(0, NO_SENDER), - ModeRequest::SetMode { - mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0), - forced: false, - }, - )) + .send(models::pcdu::request::Request::Mode(DeviceMode::Normal)) .expect("sending initial mode request failed"); info!("Starting TMTC and UDP task"); @@ -324,6 +340,7 @@ fn main() { mgm_0_handler.periodic_operation(); mgm_1_handler.periodic_operation(); mgm_assembly.periodic_operation(); + acs_subsystem.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_AOCS)); } }) diff --git a/satrs-shared/Cargo.toml b/satrs-shared/Cargo.toml index 5a2c5a0..4b172cd 100644 --- a/satrs-shared/Cargo.toml +++ b/satrs-shared/Cargo.toml @@ -11,8 +11,7 @@ license = "Apache-2.0" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -# spacepackets = { version = "0.17", default-features = false } -spacepackets = { version = "0.17", git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git", default-features = false } +spacepackets = { version = "0.18", default-features = false } serde = { version = "1", default-features = false, optional = true } defmt = {version = "1", optional = true } diff --git a/satrs/Cargo.toml b/satrs/Cargo.toml index df9bcda..a104eea 100644 --- a/satrs/Cargo.toml +++ b/satrs/Cargo.toml @@ -14,7 +14,7 @@ categories = ["aerospace", "aerospace::space-protocols", "no-std", "hardware-sup [dependencies] satrs-shared = { version = "0.2", path = "../satrs-shared" } -spacepackets = { version = "0.17", git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git", default-features = false } +spacepackets = { version = "0.18", default-features = false } delegate = "0.13" paste = "1" @@ -23,7 +23,7 @@ num_enum = { version = "0.7", default-features = false } cobs = { version = "0.5", default-features = false } thiserror = { version = "2", default-features = false } -hashbrown = { version = "0.16", optional = true } +hashbrown = { version = "0.17", optional = true } static_cell = { version = "2" } heapless = { version = "0.9", optional = true } dyn-clone = { version = "1", optional = true } @@ -42,7 +42,7 @@ serde = "1" zerocopy = "0.8" once_cell = "1" serde_json = "1" -rand = "0.9" +rand = "0.10" tempfile = "3" [dev-dependencies.postcard] diff --git a/satrs/src/legacy/mode.rs b/satrs/src/legacy/mode.rs new file mode 100644 index 0000000..18845d3 --- /dev/null +++ b/satrs/src/legacy/mode.rs @@ -0,0 +1,763 @@ +use core::mem::size_of; +use satrs_shared::res_code::ResultU16; +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; +use spacepackets::ByteConversionError; + +#[cfg(feature = "alloc")] +pub use alloc_mod::*; + +#[cfg(feature = "std")] +pub use std_mod::*; + +use crate::{ + ComponentId, + queue::{GenericReceiveError, GenericSendError}, + request::{ + GenericMessage, MessageMetadata, MessageReceiverProvider, MessageReceiverWithId, RequestId, + }, +}; + +pub type ModeRaw = u32; + +pub type Mode = u32; +pub type Submode = u16; + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct ModeAndSubmode { + mode: Mode, + submode: Submode, +} + +pub const INVALID_MODE_VAL: Mode = Mode::MAX; +pub const UNKNOWN_MODE_VAL: Mode = Mode::MAX - 1; +pub const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(INVALID_MODE_VAL, 0); +pub const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(UNKNOWN_MODE_VAL, 0); + +impl ModeAndSubmode { + pub const RAW_LEN: usize = size_of::() + size_of::(); + + pub const fn new_mode_only(mode: Mode) -> Self { + Self { mode, submode: 0 } + } + + pub const fn new(mode: Mode, submode: Submode) -> Self { + Self { mode, submode } + } + + pub fn from_be_bytes(buf: &[u8]) -> Result { + if buf.len() < 6 { + return Err(ByteConversionError::FromSliceTooSmall { + expected: Self::RAW_LEN, + found: buf.len(), + }); + } + Ok(Self { + mode: Mode::from_be_bytes(buf[0..size_of::()].try_into().unwrap()), + submode: Submode::from_be_bytes( + buf[size_of::()..size_of::() + size_of::()] + .try_into() + .unwrap(), + ), + }) + } + + pub fn write_to_be_bytes(&self, buf: &mut [u8]) -> Result { + if buf.len() < Self::RAW_LEN { + return Err(ByteConversionError::ToSliceTooSmall { + expected: Self::RAW_LEN, + found: buf.len(), + }); + } + buf[0..size_of::()].copy_from_slice(&self.mode.to_be_bytes()); + buf[size_of::()..Self::RAW_LEN].copy_from_slice(&self.submode.to_be_bytes()); + Ok(Self::RAW_LEN) + } + + pub fn mode(&self) -> Mode { + self.mode + } + + pub fn submode(&self) -> Submode { + self.submode + } +} +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct TargetedModeCommand { + pub address: ComponentId, + pub mode_submode: ModeAndSubmode, +} + +impl TargetedModeCommand { + pub const fn new(address: ComponentId, mode_submode: ModeAndSubmode) -> Self { + Self { + address, + mode_submode, + } + } + + pub fn address(&self) -> ComponentId { + self.address + } + + pub fn mode_submode(&self) -> ModeAndSubmode { + self.mode_submode + } + + pub fn mode(&self) -> u32 { + self.mode_submode.mode + } + + pub fn submode(&self) -> u16 { + self.mode_submode.submode + } +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub enum ModeRequest { + /// Mode information. Can be used to notify other components of changed modes. + ModeInfo(ModeAndSubmode), + SetMode { + mode_and_submode: ModeAndSubmode, + forced: bool, + }, + ReadMode, + AnnounceMode, + AnnounceModeRecursive, +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct TargetedModeRequest { + target_id: ComponentId, + mode_request: ModeRequest, +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub enum ModeReply { + /// Mode information. Can be used to notify other components of changed modes. + ModeInfo(ModeAndSubmode), + /// Reply to a mode request to confirm the commanded mode was reached. + ModeReply(ModeAndSubmode), + // Can not reach the commanded mode. Contains a reason as a [ResultU16]. + CantReachMode(ResultU16), + /// We are in the wrong mode for unknown reasons. Contains the expected and reached mode. + WrongMode { + expected: ModeAndSubmode, + reached: ModeAndSubmode, + }, +} + +pub type GenericModeReply = GenericMessage; + +pub trait ModeRequestSender { + fn local_channel_id(&self) -> ComponentId; + fn send_mode_request( + &self, + request_id: RequestId, + target_id: ComponentId, + request: ModeRequest, + ) -> Result<(), GenericSendError>; +} + +pub trait ModeRequestReceiver { + fn try_recv_mode_request( + &self, + ) -> Result>, GenericReceiveError>; +} + +impl> ModeRequestReceiver + for MessageReceiverWithId +{ + fn try_recv_mode_request( + &self, + ) -> Result>, GenericReceiveError> { + self.try_recv_message() + } +} + +#[derive(Debug, Clone, thiserror::Error)] +pub enum ModeError { + #[error("Messaging send error: {0}")] + Send(#[from] GenericSendError), + #[error("Messaging receive error: {0}")] + Receive(#[from] GenericReceiveError), + #[error("busy with other mode request")] + Busy, +} + +pub trait ModeProvider { + fn mode_and_submode(&self) -> ModeAndSubmode; + + fn mode(&self) -> Mode { + self.mode_and_submode().mode() + } + + fn submode(&self) -> Submode { + self.mode_and_submode().submode() + } +} + +pub trait ModeRequestHandler: ModeProvider { + type Error; + + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + forced: bool, + ) -> Result<(), Self::Error>; + + fn announce_mode(&self, requestor_info: Option, recursive: bool); + + fn handle_mode_reached( + &mut self, + requestor_info: Option, + ) -> Result<(), Self::Error>; + + fn handle_mode_info( + &mut self, + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), Self::Error>; + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error>; + + fn handle_mode_request( + &mut self, + request: GenericMessage, + ) -> Result<(), Self::Error> { + match request.message { + ModeRequest::SetMode { + 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()), + ), + ModeRequest::AnnounceMode => { + self.announce_mode(Some(request.requestor_info), false); + Ok(()) + } + ModeRequest::AnnounceModeRecursive => { + self.announce_mode(Some(request.requestor_info), true); + Ok(()) + } + ModeRequest::ModeInfo(info) => self.handle_mode_info(request.requestor_info, info), + } + } +} + +pub trait ModeReplyReceiver { + fn try_recv_mode_reply(&self) + -> Result>, GenericReceiveError>; +} + +impl> ModeReplyReceiver + for MessageReceiverWithId +{ + fn try_recv_mode_reply( + &self, + ) -> Result>, GenericReceiveError> { + self.try_recv_message() + } +} + +pub trait ModeReplySender { + fn local_channel_id(&self) -> ComponentId; + + /// The requestor is assumed to be the target of the reply. + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), GenericSendError>; +} + +#[cfg(feature = "alloc")] +pub mod alloc_mod { + use crate::{ + queue::{GenericReceiveError, GenericSendError}, + request::{ + MessageSenderAndReceiver, MessageSenderMap, MessageSenderProvider, + MessageSenderStoreProvider, RequestAndReplySenderAndReceiver, + }, + }; + + use super::*; + + impl> MessageSenderMap { + pub fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + target_id: ComponentId, + request: ModeReply, + ) -> Result<(), GenericSendError> { + self.send_message(requestor_info, target_id, request) + } + + pub fn add_reply_target(&mut self, target_id: ComponentId, request_sender: S) { + self.add_message_target(target_id, request_sender) + } + } + + impl< + From, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeReplySender for MessageSenderAndReceiver + { + fn local_channel_id(&self) -> ComponentId { + self.local_channel_id_generic() + } + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + request: ModeReply, + ) -> Result<(), GenericSendError> { + self.message_sender_store.send_message( + MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()), + requestor_info.sender_id(), + request, + ) + } + } + + impl< + To, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeReplyReceiver for MessageSenderAndReceiver + { + fn try_recv_mode_reply( + &self, + ) -> Result>, GenericReceiveError> { + self.message_receiver.try_recv_message() + } + } + + impl< + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > + RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > + { + pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: ReplySender) { + self.reply_sender_store + .add_message_target(target_id, reply_sender) + } + } + + impl< + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeReplySender + for RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + ModeReply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > + { + fn local_channel_id(&self) -> ComponentId { + self.local_channel_id_generic() + } + + fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), GenericSendError> { + self.reply_sender_store.send_message( + MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()), + requestor_info.sender_id(), + reply, + ) + } + } + + impl< + Request, + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeReplyReceiver + for RequestAndReplySenderAndReceiver< + Request, + ReqSender, + ReqReceiver, + ReqSenderStore, + ModeReply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > + { + fn try_recv_mode_reply( + &self, + ) -> Result>, GenericReceiveError> { + self.reply_receiver.try_recv_message() + } + } + + /// Helper type definition for a mode handler which can handle mode requests. + pub type ModeRequestHandlerInterface = + MessageSenderAndReceiver; + + impl< + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeRequestHandlerInterface + { + pub fn try_recv_mode_request( + &self, + ) -> Result>, GenericReceiveError> { + self.try_recv_message() + } + + pub fn send_mode_reply( + &self, + requestor_info: MessageMetadata, + reply: ModeReply, + ) -> Result<(), GenericSendError> { + self.send_message( + requestor_info.request_id(), + requestor_info.sender_id(), + reply, + ) + } + } + + /// Helper type defintion for a mode handler object which can send mode requests and receive + /// mode replies. + pub type ModeRequestorInterface = + MessageSenderAndReceiver; + + impl< + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + RequestSenderStore: MessageSenderStoreProvider, + > ModeRequestorInterface + { + pub fn try_recv_mode_reply( + &self, + ) -> Result>, GenericReceiveError> { + self.try_recv_message() + } + + pub fn send_mode_request( + &self, + request_id: RequestId, + target_id: ComponentId, + reply: ModeRequest, + ) -> Result<(), GenericSendError> { + self.send_message(request_id, target_id, reply) + } + } + + /// Helper type defintion for a mode handler object which can both send mode requests and + /// process mode requests. + pub type ModeInterface< + ReqSender, + ReqReceiver, + ReqSenderStore, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > = RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + ModeReply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + >; + + impl> MessageSenderMap { + pub fn send_mode_request( + &self, + requestor_info: MessageMetadata, + target_id: ComponentId, + request: ModeRequest, + ) -> Result<(), GenericSendError> { + self.send_message(requestor_info, target_id, request) + } + + pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: S) { + self.add_message_target(target_id, request_sender) + } + } + + impl< + To, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeRequestReceiver + for MessageSenderAndReceiver + { + fn try_recv_mode_request( + &self, + ) -> Result>, GenericReceiveError> { + self.message_receiver.try_recv_message() + } + } + + impl< + From, + Sender: MessageSenderProvider, + Receiver: MessageReceiverProvider, + SenderStore: MessageSenderStoreProvider, + > ModeRequestSender + for MessageSenderAndReceiver + { + fn local_channel_id(&self) -> ComponentId { + self.local_channel_id_generic() + } + + fn send_mode_request( + &self, + request_id: RequestId, + target_id: ComponentId, + request: ModeRequest, + ) -> Result<(), GenericSendError> { + self.message_sender_store.send_message( + MessageMetadata::new(request_id, self.local_channel_id()), + target_id, + request, + ) + } + } + + impl< + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > + RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > + { + pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: ReqSender) { + self.request_sender_store + .add_message_target(target_id, request_sender) + } + } + + impl< + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeRequestSender + for RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > + { + fn local_channel_id(&self) -> ComponentId { + self.local_channel_id_generic() + } + + fn send_mode_request( + &self, + request_id: RequestId, + target_id: ComponentId, + request: ModeRequest, + ) -> Result<(), GenericSendError> { + self.request_sender_store.send_message( + MessageMetadata::new(request_id, self.local_channel_id()), + target_id, + request, + ) + } + } + + impl< + ReqSender: MessageSenderProvider, + ReqReceiver: MessageReceiverProvider, + ReqSenderStore: MessageSenderStoreProvider, + Reply, + ReplySender: MessageSenderProvider, + ReplyReceiver: MessageReceiverProvider, + ReplySenderStore: MessageSenderStoreProvider, + > ModeRequestReceiver + for RequestAndReplySenderAndReceiver< + ModeRequest, + ReqSender, + ReqReceiver, + ReqSenderStore, + Reply, + ReplySender, + ReplyReceiver, + ReplySenderStore, + > + { + fn try_recv_mode_request( + &self, + ) -> Result>, GenericReceiveError> { + self.request_receiver.try_recv_message() + } + } +} + +#[cfg(feature = "std")] +pub mod std_mod { + use std::sync::mpsc; + + use crate::request::{MessageSenderList, OneMessageSender}; + + use super::*; + + pub type ModeRequestHandlerMpsc = ModeRequestHandlerInterface< + mpsc::Sender>, + mpsc::Receiver>, + MessageSenderList>>, + >; + pub type ModeRequestHandlerMpscBounded = ModeRequestHandlerInterface< + mpsc::SyncSender>, + mpsc::Receiver>, + MessageSenderList>>, + >; + + pub type ModeRequestorOneChildMpsc = ModeRequestorInterface< + mpsc::Sender>, + mpsc::Receiver>, + OneMessageSender>>, + >; + pub type ModeRequestorOneChildBoundedMpsc = ModeRequestorInterface< + mpsc::SyncSender>, + mpsc::Receiver>, + OneMessageSender>>, + >; + pub type ModeRequestorChildListMpsc = ModeRequestorInterface< + mpsc::Sender>, + mpsc::Receiver>, + MessageSenderList>>, + >; + pub type ModeRequestorChildListBoundedMpsc = ModeRequestorInterface< + mpsc::SyncSender>, + mpsc::Receiver>, + MessageSenderList>>, + >; + + pub type ModeRequestorAndHandlerMpsc = ModeInterface< + mpsc::Sender>, + mpsc::Receiver>, + MessageSenderList>>, + mpsc::Sender>, + mpsc::Receiver>, + MessageSenderList>>, + >; + pub type ModeRequestorAndHandlerMpscBounded = ModeInterface< + mpsc::SyncSender>, + mpsc::Receiver>, + MessageSenderList>>, + mpsc::SyncSender>, + mpsc::Receiver>, + MessageSenderList>>, + >; +} + +#[cfg(test)] +pub(crate) mod tests { + use core::cell::RefCell; + use std::collections::VecDeque; + + use crate::{ComponentId, request::RequestId}; + + use super::*; + + pub struct ModeReqWrapper { + pub request_id: RequestId, + pub target_id: ComponentId, + pub request: ModeRequest, + } + + #[derive(Default)] + pub struct ModeReqSenderMock { + pub requests: RefCell>, + } + + impl ModeRequestSender for ModeReqSenderMock { + fn local_channel_id(&self) -> crate::ComponentId { + 0 + } + + fn send_mode_request( + &self, + request_id: RequestId, + target_id: ComponentId, + request: ModeRequest, + ) -> Result<(), GenericSendError> { + self.requests.borrow_mut().push_back(ModeReqWrapper { + request_id, + target_id, + request, + }); + Ok(()) + } + } +} diff --git a/satrs/src/legacy/mode_tree.rs b/satrs/src/legacy/mode_tree.rs new file mode 100644 index 0000000..46d171c --- /dev/null +++ b/satrs/src/legacy/mode_tree.rs @@ -0,0 +1,614 @@ +use alloc::vec::Vec; +use hashbrown::HashMap; + +use crate::{ + ComponentId, mode::{Mode, ModeAndSubmode, ModeRaw, ModeReply, ModeRequest, Submode}, request::MessageSenderProvider, +}; + +#[cfg(feature = "alloc")] +pub use alloc_mod::*; + +/// Common trait for node modes which can have mode parents or mode children. +pub trait ModeNode { + fn id(&self) -> ComponentId; +} +/// 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: ModeNode { + type Sender: MessageSenderProvider; + + 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: ModeNode { + type Sender: MessageSenderProvider; + + 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( + parent: &mut impl ModeParent, + request_sender: ReqSender, + child: &mut impl ModeChild, + reply_sender: ReplySender, +) { + parent.add_mode_child(child.id(), request_sender); + child.add_mode_parent(parent.id(), reply_sender); +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum TableEntryType { + /// Target table containing information of the expected children modes for given mode. + Target, + /// Sequence table which contains information about how to reach a target table, including + /// the order of the sequences. + Sequence, +} + +/// Common fields required for both target and sequence table entries. +/// +/// The most important parameters here are the target ID which this entry belongs to, and the mode +/// and submode the entry either will be commanded to for sequence table entries or which will be +/// monitored for target table entries. +#[derive(Debug, Copy, Clone)] +pub struct ModeTableEntryCommon { + /// Name of respective table entry. + pub name: &'static str, + /// Target component ID. + pub target_id: ComponentId, + /// Has a different meaning depending on whether this is a sequence table or a target table. + /// + /// - For sequence tables, this denotes the mode which will be commanded + /// - For target tables, this is the mode which the target children should have and which + /// might be monitored depending on configuration. + pub mode: ModeRaw, + /// This mask allows to specify multiple allowed submodes for a given mode. + pub allowed_submode_mask: Option, +} + +impl ModeTableEntryCommon { + pub fn set_allowed_submode_mask(&mut self, mask: Submode) { + self.allowed_submode_mask = Some(mask); + } + + pub fn allowed_submode_mask(&self) -> Option { + self.allowed_submode_mask + } +} + +/// An entry for the target tables. +#[derive(Debug)] +pub struct TargetTableEntry { + pub common: ModeTableEntryCommon, + pub monitor_state: bool, +} + +impl TargetTableEntry { + pub fn new( + name: &'static str, + target_id: ComponentId, + mode_submode: ModeAndSubmode, + allowed_submode_mask: Option, + ) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode_submode, + allowed_submode_mask, + }, + monitor_state: true, + } + } + + pub fn new_with_precise_submode( + name: &'static str, + target_id: ComponentId, + mode_submode: ModeAndSubmode, + ) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode_submode, + allowed_submode_mask: None, + }, + monitor_state: true, + } + } + + delegate::delegate! { + to self.common { + pub fn set_allowed_submode_mask(&mut self, mask: Submode); + pub fn allowed_submode_mask(&self) -> Option; + } + } +} + +/// An entry for the sequence tables. +/// +/// The [Self::check_success] field specifies that a mode sequence executor should check that the +/// target mode was actually reached before executing the next sequence. +#[derive(Debug)] +pub struct SequenceTableEntry { + pub common: ModeTableEntryCommon, + pub check_success: bool, +} + +impl SequenceTableEntry { + pub fn new( + name: &'static str, + target_id: ComponentId, + mode_submode: ModeAndSubmode, + check_success: bool, + ) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode_submode, + allowed_submode_mask: None, + }, + check_success, + } + } + + delegate::delegate! { + to self.common { + pub fn set_allowed_submode_mask(&mut self, mask: Submode); + pub fn allowed_submode_mask(&self) -> Option; + } + } +} + +#[derive(Debug, thiserror::Error)] +#[error("target {0} not in mode store")] +pub struct TargetNotInModeStoreError(pub ComponentId); + +/// Mode store value type. +#[derive(Debug, Copy, Clone)] +pub struct ModeStoreValue { + /// ID of the mode component. + id: ComponentId, + /// Current mode and submode of the component. + pub mode_and_submode: ModeAndSubmode, + /// State information to track whether a reply should be awaited for the mode component. + pub awaiting_reply: bool, +} + +impl ModeStoreValue { + 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 + } +} + +pub trait ModeStoreProvider { + fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode); + + fn has_component(&self, target_id: ComponentId) -> bool; + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue>; + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue>; + + /// Generic handler for mode replies received from child components. + /// + /// Implementation should clear the awaition flag if the `handle_reply_awaition` argument is + /// true and returns whether any children are still awaiting replies. If the flag is not set + fn mode_reply_handler_with_reply_awaition( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + ) -> bool { + self.mode_reply_handler(sender_id, reported_mode_and_submode, true) + .unwrap_or(false) + } + + fn mode_reply_handler_without_reply_awaition( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + ) { + self.mode_reply_handler(sender_id, reported_mode_and_submode, false); + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + with_reply_awaition: bool, + ) -> Option; +} + +#[cfg(feature = "alloc")] +pub mod alloc_mod { + use crate::subsystem::ModeRaw; + +use super::*; + + #[derive(Debug)] + pub struct TargetTablesMapValue { + /// Name for a given mode table entry. + pub name: &'static str, + /// Optional fallback mode if the target mode can not be kept. + pub fallback_mode: Option, + /// These are the rows of the a target table. + pub entries: Vec, + } + + impl TargetTablesMapValue { + pub fn new(name: &'static str, fallback_mode: Option) -> Self { + Self { + name, + fallback_mode, + entries: Default::default(), + } + } + + pub fn add_entry(&mut self, entry: TargetTableEntry) { + self.entries.push(entry); + } + } + + /// One sequence of a [SequenceTablesMapValue] in a [SequenceModeTables]. + /// + /// It contains all mode requests which need to be executed for a sequence step and it also + /// associates a [Self::name] with the sequence. + #[derive(Debug)] + 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, + } + + impl SequenceTableMapTable { + pub fn new(name: &'static str) -> Self { + Self { + name, + entries: Default::default(), + } + } + + pub fn add_entry(&mut self, entry: SequenceTableEntry) { + self.entries.push(entry); + } + } + + /// A sequence table entry. + /// + /// This is simply a list of [SequenceTableMapTable]s which also associates a [Self::name] + /// with the sequence. The order of sub-tables in the list also specifies the execution order + /// in the mode sequence. + #[derive(Debug)] + pub struct SequenceTablesMapValue { + /// Name for a given mode sequence. + pub name: &'static str, + /// Each sequence can consists of multiple sequences that are executed consecutively. + pub entries: Vec, + } + + impl SequenceTablesMapValue { + 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); + + impl TargetModeTables { + pub fn name(&self, mode: Mode) -> Option<&'static str> { + self.0.get(&mode).map(|value| value.name) + } + } + + impl SequenceModeTables { + pub fn name(&self, mode: Mode) -> Option<&'static str> { + self.0.get(&mode).map(|value| value.name) + } + + pub fn name_of_sequence(&self, mode: Mode, seq_idx: usize) -> Option<&'static str> { + self.0 + .get(&mode) + .map(|value| value.entries.get(seq_idx).map(|v| v.name))? + } + } + + /// This is the core data structure used to store mode sequence tables. + /// + /// A mode sequence table specifies which commands have to be sent in which order + /// to reach a certain [Mode]. Therefore, it simply maps a [Mode] to a [SequenceTablesMapValue]. + #[derive(Debug, Default)] + pub struct SequenceModeTables(pub HashMap); + + /// Mode store which tracks the [mode information][ModeStoreValue] inside a [Vec] + #[derive(Debug, Default)] + pub struct ModeStoreVec(pub alloc::vec::Vec); + + impl<'a> IntoIterator for &'a ModeStoreVec { + type Item = &'a ModeStoreValue; + type IntoIter = std::slice::Iter<'a, ModeStoreValue>; + + fn into_iter(self) -> Self::IntoIter { + self.0.iter() + } + } + + impl<'a> IntoIterator for &'a mut ModeStoreVec { + type Item = &'a mut ModeStoreValue; + type IntoIter = std::slice::IterMut<'a, ModeStoreValue>; + + fn into_iter(self) -> Self::IntoIter { + self.0.iter_mut() + } + } + + /// Mode store which tracks the mode information inside a [hashbrown::HashMap] + #[derive(Debug, Default)] + pub struct ModeStoreMap(pub hashbrown::HashMap); + + impl<'a> IntoIterator for &'a ModeStoreMap { + type Item = (&'a ComponentId, &'a ModeStoreValue); + type IntoIter = hashbrown::hash_map::Iter<'a, ComponentId, ModeStoreValue>; + + fn into_iter(self) -> Self::IntoIter { + self.0.iter() + } + } + + impl ModeStoreProvider for ModeStoreVec { + fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { + self.0.push(ModeStoreValue::new(target_id, mode)); + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.0.iter().any(|val| val.id == target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.0.iter().find(|val| val.id == target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.0.iter_mut().find(|val| val.id == target_id) + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + handle_reply_awaition: bool, + ) -> Option { + let mut still_awating_replies = None; + if handle_reply_awaition { + still_awating_replies = Some(false); + } + self.0.iter_mut().for_each(|val| { + if val.id() == sender_id { + if let Some(mode_and_submode) = reported_mode_and_submode { + val.mode_and_submode = mode_and_submode; + } + if handle_reply_awaition { + val.awaiting_reply = false; + } + } + if handle_reply_awaition && val.awaiting_reply { + still_awating_replies = Some(true); + } + }); + still_awating_replies + } + } + + impl ModeStoreProvider for ModeStoreMap { + fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { + self.0 + .insert(target_id, ModeStoreValue::new(target_id, mode)); + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.0.contains_key(&target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.0.get(&target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.0.get_mut(&target_id) + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + handle_reply_awaition: bool, + ) -> Option { + let mut still_awating_replies = None; + if handle_reply_awaition { + still_awating_replies = Some(false); + } + for val in self.0.values_mut() { + if val.id() == sender_id { + if let Some(mode_and_submode) = reported_mode_and_submode { + val.mode_and_submode = mode_and_submode; + } + if handle_reply_awaition { + val.awaiting_reply = false; + } + } + if handle_reply_awaition && val.awaiting_reply { + still_awating_replies = Some(true); + } + } + still_awating_replies + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn generic_test(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + assert!(mode_store.has_component(1)); + assert!(mode_store.has_component(2)); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(0, 0) + ); + assert!(!mode_store.get(1).unwrap().awaiting_reply); + assert!(!mode_store.get(2).unwrap().awaiting_reply); + assert_eq!(mode_store.get(1).unwrap().id, 1); + assert_eq!(mode_store.get(2).unwrap().id, 2); + assert!(mode_store.get(3).is_none()); + assert!(mode_store.get_mut(3).is_none()); + } + + fn generic_reply_handling_with_reply_awaition(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.get_mut(1).unwrap().awaiting_reply = true; + mode_store.get_mut(2).unwrap().awaiting_reply = true; + let mut reply_awation_pending = + mode_store.mode_reply_handler_with_reply_awaition(1, Some(ModeAndSubmode::new(2, 0))); + assert!(reply_awation_pending); + reply_awation_pending = mode_store.mode_reply_handler_with_reply_awaition(2, None); + assert!(!reply_awation_pending); + assert!(!mode_store.get(1).unwrap().awaiting_reply); + assert!(!mode_store.get(2).unwrap().awaiting_reply); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(2, 0) + ); + assert_eq!( + mode_store.get(2).unwrap().mode_and_submode(), + ModeAndSubmode::new(1, 0) + ); + } + + fn generic_reply_handling_test_no_reply_awaition(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.get_mut(1).unwrap().awaiting_reply = true; + mode_store.get_mut(2).unwrap().awaiting_reply = true; + mode_store.mode_reply_handler_without_reply_awaition(1, Some(ModeAndSubmode::new(2, 0))); + mode_store.mode_reply_handler_without_reply_awaition(2, None); + assert!(mode_store.get(1).unwrap().awaiting_reply); + assert!(mode_store.get(2).unwrap().awaiting_reply); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(2, 0) + ); + assert_eq!( + mode_store.get(2).unwrap().mode_and_submode(), + ModeAndSubmode::new(1, 0) + ); + } + + fn generic_reply_handling_with_reply_awaition_2(mode_store: &mut impl ModeStoreProvider) { + mode_store.add_component(1, ModeAndSubmode::new(0, 0)); + mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.get_mut(1).unwrap().awaiting_reply = true; + mode_store.get_mut(2).unwrap().awaiting_reply = true; + let mut reply_awation_pending = + mode_store.mode_reply_handler(1, Some(ModeAndSubmode::new(2, 0)), true); + assert!(reply_awation_pending.unwrap()); + reply_awation_pending = mode_store.mode_reply_handler(2, None, true); + assert!(!reply_awation_pending.unwrap()); + assert!(!mode_store.get(1).unwrap().awaiting_reply); + assert!(!mode_store.get(2).unwrap().awaiting_reply); + assert_eq!( + mode_store.get(1).unwrap().mode_and_submode(), + ModeAndSubmode::new(2, 0) + ); + assert_eq!( + mode_store.get(2).unwrap().mode_and_submode(), + ModeAndSubmode::new(1, 0) + ); + } + + #[test] + fn test_vec_mode_store() { + let mut mode_store = ModeStoreVec::default(); + generic_test(&mut mode_store); + } + + #[test] + fn test_map_mode_store() { + let mut mode_store = ModeStoreMap::default(); + generic_test(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_vec_with_reply_awaition() { + let mut mode_store = ModeStoreVec::default(); + generic_reply_handling_with_reply_awaition(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_vec_with_reply_awaition_2() { + let mut mode_store = ModeStoreVec::default(); + generic_reply_handling_with_reply_awaition_2(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_map_with_reply_awaition() { + let mut mode_store = ModeStoreMap::default(); + generic_reply_handling_with_reply_awaition(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_map_with_reply_awaition_2() { + let mut mode_store = ModeStoreMap::default(); + generic_reply_handling_with_reply_awaition_2(&mut mode_store); + } + + #[test] + fn test_generic_reply_handler_vec_no_reply_awaition() { + let mut mode_store = ModeStoreVec::default(); + generic_reply_handling_test_no_reply_awaition(&mut mode_store); + } + #[test] + fn test_generic_reply_handler_map_no_reply_awaition() { + let mut mode_store = ModeStoreMap::default(); + generic_reply_handling_test_no_reply_awaition(&mut mode_store); + } +} diff --git a/satrs/src/legacy/subsystem.rs b/satrs/src/legacy/subsystem.rs new file mode 100644 index 0000000..09836ed --- /dev/null +++ b/satrs/src/legacy/subsystem.rs @@ -0,0 +1,1610 @@ +use crate::{ + ComponentId, + health::{HealthState, HealthTableProvider}, + mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender, UNKNOWN_MODE_VAL}, + mode_tree::{ + ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableMapTable, + SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError, TargetTablesMapValue, + }, + queue::GenericTargetedMessagingError, + request::{GenericMessage, RequestId}, +}; + +#[derive(Debug, PartialEq, Eq, Copy, Clone)] +pub enum SequenceExecutionHelperState { + /// The sequence execution is IDLE, no command is loaded or the sequence exection has + /// finished + Idle, + /// The sequence helper is executing a sequence and no replies need to be awaited. + Busy, + /// The sequence helper is still awaiting a reply from a mode children. The reply awaition + /// is a property of a mode commanding sequence + AwaitingSuccessCheck, +} + +#[derive(Debug, PartialEq, Eq)] +pub enum ModeCommandingResult { + /// The commanding of all children is finished + Done, + /// One step of a commanding chain is finished + StepDone, + /// Reply awaition is required for some children + AwaitingSuccessCheck, +} + +#[derive(Debug, thiserror::Error)] +#[error("mode {0} does not exist")] +pub struct ModeDoesNotExistError(Mode); + +#[derive(Debug, thiserror::Error)] +pub enum StartSequenceError { + #[error("mode {0} does not exist")] + ModeDoesNotExist(#[from] ModeDoesNotExistError), + #[error("invalid request ID")] + InvalidRequestId(RequestId), +} + +/// This sequence execution helper includes some boilerplate logic to +/// execute [SequenceModeTables]. +/// +/// It takes care of commanding the [ModeRequest]s specified in those tables and also includes the +/// states required to track the current progress of a sequence execution and take care of +/// reply and success awaition. +#[derive(Debug)] +pub struct SequenceExecutionHelper { + target_mode: Option, + state: SequenceExecutionHelperState, + request_id: Option, + current_sequence_index: Option, + last_sequence_index: Option, +} + +impl Default for SequenceExecutionHelper { + fn default() -> Self { + Self { + target_mode: None, + state: SequenceExecutionHelperState::Idle, + request_id: None, + current_sequence_index: None, + last_sequence_index: None, + } + } +} + +pub trait IsChildCommandable { + fn is_commandable(&self, id: ComponentId) -> bool; +} + +impl IsChildCommandable for T +where + T: HealthTableProvider, +{ + fn is_commandable(&self, id: ComponentId) -> bool { + self.health(id) + .is_none_or(|h| h != HealthState::ExternalControl) + } +} + +impl SequenceExecutionHelper { + pub fn new() -> Self { + Default::default() + } + + /// Load a new mode sequence to be executed + pub fn load( + &mut self, + mode: Mode, + request_id: RequestId, + sequence_tables: &SequenceModeTables, + ) -> Result<(), ModeDoesNotExistError> { + if !sequence_tables.0.contains_key(&mode) { + return Err(ModeDoesNotExistError(mode)); + } + self.target_mode = Some(mode); + self.request_id = Some(request_id); + self.state = SequenceExecutionHelperState::Busy; + self.current_sequence_index = None; + Ok(()) + } + + /// Run the sequence execution helper. + /// + /// This function will execute the sequence in the given [SequenceModeTables] based on the + /// mode loaded in [Self::load]. It calls [Self::execute_sequence_and_map_to_result] and + /// automatically takes care of state management, including increments of the sequence table + /// index. + /// + /// The returnvalues of the helper have the following meaning. + /// + /// * [ModeCommandingResult::AwaitingSuccessCheck] - The sequence is still awaiting a success. + /// The user should check whether all children have reached the commanded target mode, for + /// example by checking [mode replies][ModeReply] received by the children components, and + /// then calling [Self::confirm_sequence_done] to advance to the sequence or complete the + /// sequence. + /// * [ModeCommandingResult::Done] - The sequence is done. The user can load a new + /// sequence now without overwriting the last one. The sequence executor is in + /// [SequenceExecutionHelperState::Idle] again. + /// * [ModeCommandingResult::StepDone] - The sequence has advanced one step. The user + /// can now call [Self::run] again to immediately execute the next step in the sequence. + /// + /// Generally, periodic execution of the [Self::run] method should be performed while + /// [Self::state] is not [SequenceExecutionHelperState::Idle]. + /// + /// # Arguments + /// + /// * `table` - This table contains the sequence tables to reach the mode previously loaded + /// with [Self::load] + /// * `sender` - The sender to send mode requests to the components + /// * `children_mode_store` - The mode store vector to keep track of the mode states of + /// children components + pub fn run( + &mut self, + table: &SequenceModeTables, + sender: &impl ModeRequestSender, + children_mode_store: &mut ModeStoreVec, + is_commandable: &impl IsChildCommandable, + ) -> Result { + if self.state == SequenceExecutionHelperState::Idle { + return Ok(ModeCommandingResult::Done); + } + if self.state == SequenceExecutionHelperState::AwaitingSuccessCheck { + return Ok(ModeCommandingResult::AwaitingSuccessCheck); + } + if self.target_mode.is_none() { + return Ok(ModeCommandingResult::Done); + } + match self.current_sequence_index { + Some(idx) => { + // Execute the sequence. + let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); + self.execute_sequence_and_map_to_result( + seq_table_value, + idx, + sender, + children_mode_store, + is_commandable, + ) + } + None => { + // Find the first sequence + let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); + self.last_sequence_index = Some(seq_table_value.entries.len() - 1); + if seq_table_value.entries.is_empty() { + Ok(ModeCommandingResult::Done) + } else { + self.current_sequence_index = Some(0); + self.execute_sequence_and_map_to_result( + seq_table_value, + 0, + sender, + children_mode_store, + is_commandable, + ) + } + } + } + } + + /// Retrieve the currently loaded target mode + pub fn target_mode(&self) -> Option { + self.target_mode + } + + /// Confirm that a sequence which is awaiting a success check is done + pub fn confirm_sequence_done(&mut self) { + if let SequenceExecutionHelperState::AwaitingSuccessCheck = self.state { + self.state = SequenceExecutionHelperState::Busy; + if let (Some(last_sequence_index), Some(current_sequence_index)) = + (self.last_sequence_index, self.current_sequence_index) + { + if current_sequence_index == last_sequence_index { + self.state = SequenceExecutionHelperState::Idle; + } + } + self.current_sequence_index = Some(self.current_sequence_index.unwrap() + 1); + } + } + + /// Internal state of the execution helper. + pub fn state(&self) -> SequenceExecutionHelperState { + self.state + } + + pub fn request_id(&self) -> Option { + self.request_id + } + + pub fn set_request_id(&mut self, request_id: RequestId) { + self.request_id = Some(request_id); + } + + pub fn awaiting_success_check(&self) -> bool { + self.state == SequenceExecutionHelperState::AwaitingSuccessCheck + } + + pub fn current_sequence_index(&self) -> Option { + self.current_sequence_index + } + + /// Execute a sequence at the given sequence index for a given [SequenceTablesMapValue]. + /// + /// This method calls [Self::execute_sequence] and maps the result to a [ModeCommandingResult]. + /// It is also called by the [Self::run] method of this helper. + pub fn execute_sequence_and_map_to_result( + &mut self, + seq_table_value: &SequenceTablesMapValue, + sequence_idx: usize, + sender: &impl ModeRequestSender, + mode_store_vec: &mut ModeStoreVec, + is_commandable: &impl IsChildCommandable, + ) -> Result { + if self.state() == SequenceExecutionHelperState::Idle || self.request_id.is_none() { + return Ok(ModeCommandingResult::Done); + } + if Self::execute_sequence( + self.request_id.unwrap(), + &seq_table_value.entries[sequence_idx], + sender, + mode_store_vec, + is_commandable, + )? { + self.state = SequenceExecutionHelperState::AwaitingSuccessCheck; + Ok(ModeCommandingResult::AwaitingSuccessCheck) + } else if seq_table_value.entries.len() - 1 == sequence_idx { + self.state = SequenceExecutionHelperState::Idle; + Ok(ModeCommandingResult::Done) + } else { + self.current_sequence_index = Some(sequence_idx + 1); + Ok(ModeCommandingResult::StepDone) + } + } + + /// Generic stateless execution helper method. + /// + /// The [RequestId] and the [SequenceTableMapTable] to be executed are passed explicitely + /// here. This method is called by [Self::execute_sequence_and_map_to_result]. + /// + /// This method itereates through the entries of the given sequence table and sends out + /// [ModeRequest]s to set the modes of the children according to the table entries. + /// It also sets the reply awaition field in the children mode store where a success + /// check is required to true. + /// + /// It returns whether any commanding success check is required by any entry in the table. + pub fn execute_sequence( + request_id: RequestId, + map_table: &SequenceTableMapTable, + sender: &impl ModeRequestSender, + children_mode_store: &mut ModeStoreVec, + commandable: &impl IsChildCommandable, + ) -> Result { + let mut some_succes_check_required = false; + for entry in &map_table.entries { + if !commandable.is_commandable(entry.common.target_id) { + continue; + } + sender.send_mode_request( + request_id, + entry.common.target_id, + ModeRequest::SetMode { + mode_and_submode: entry.common.mode_submode, + forced: false, + }, + )?; + if entry.check_success { + children_mode_store.0.iter_mut().for_each(|val| { + if val.id() == entry.common.target_id { + val.awaiting_reply = true; + } + }); + some_succes_check_required = true; + } + } + Ok(some_succes_check_required) + } +} + +#[derive(Debug, Default, PartialEq, Eq, Clone, Copy)] +pub enum ModeTreeHelperState { + #[default] + Idle, + /// The helper is currently trying to keep a target mode. + TargetKeeping, + /// The helper is currently busy to command a mode. + ModeCommanding, +} + +#[derive(Debug, Default, PartialEq, Eq)] +pub enum SubsystemHelperResult { + #[default] + Idle, + /// Busy with target keeping. + TargetKeeping, + /// Result of a mode commanding operation + ModeCommanding(ModeCommandingResult), +} + +impl From for SubsystemHelperResult { + fn from(value: ModeCommandingResult) -> Self { + Self::ModeCommanding(value) + } +} + +#[derive(Debug, thiserror::Error)] +pub enum ModeTreeHelperError { + #[error("generic targeted messaging error: {0}")] + Message(#[from] GenericTargetedMessagingError), + #[error("current mode {0} is not contained in target table")] + CurrentModeNotInTargetTable(Mode), + /// Mode command has failed, for example while executing a mode table. + #[error("mode command failed")] + ModeCommmandFailure { + /// Table index of the sequence table entry which failed. + seq_table_index: Option, + }, + /// Target mode keeping violation. + #[error("target keeping violation")] + TargetKeepingViolation { + /// Table index of the sequence table entry which failed. + fallback_mode: Option, + }, +} + +/// This is a helper object which can be used by a subsystem component to execute mode sequences +/// and perform target keeping. +/// +/// This helper object tries to compose as much data and state information as possible which is +/// required for this process. +pub struct SubsystemCommandingHelper { + /// State of the helper. + state: ModeTreeHelperState, + /// Current mode of the owner subsystem. + current_mode: Mode, + /// This data structure is used to track all mode children. + pub children_mode_store: ModeStoreVec, + /// This field is set when a mode sequence is executed. It is used to determine whether mode + /// replies are relevant for reply awaition logic. + active_internal_request_id: Option, + /// The primary data structure to keep the target state information for subsystem + /// [modes][Mode]. it specifies the mode each child should have for a certain subsystem mode + /// and is relevant for target keeping. + pub target_tables: TargetModeTables, + /// The primary data structure to keep the sequence commanding information for commanded + /// subsystem [modes][Mode]. It specifies the actual commands and the order they should be + /// sent in to reach a certain [mode][Mode]. + pub sequence_tables: SequenceModeTables, + /// The sequence execution helper is used to execute sequences in the [Self::sequence_tables]. + pub seq_exec_helper: SequenceExecutionHelper, +} + +impl Default for SubsystemCommandingHelper { + fn default() -> Self { + Self { + current_mode: UNKNOWN_MODE_VAL, + state: Default::default(), + children_mode_store: Default::default(), + active_internal_request_id: None, + target_tables: Default::default(), + sequence_tables: Default::default(), + seq_exec_helper: Default::default(), + } + } +} + +impl SubsystemCommandingHelper { + /// Create a new substem commanding helper with an intial [ModeTreeHelperState::Idle] state, + /// an empty mode children store and empty target and sequence mode tables. + pub fn new( + children_mode_store: ModeStoreVec, + target_tables: TargetModeTables, + sequence_tables: SequenceModeTables, + ) -> Self { + Self { + current_mode: UNKNOWN_MODE_VAL, + state: ModeTreeHelperState::Idle, + children_mode_store, + active_internal_request_id: None, + target_tables, + sequence_tables, + seq_exec_helper: Default::default(), + } + } + + pub fn state(&self) -> ModeTreeHelperState { + self.state + } + + pub fn mode(&self) -> Mode { + self.current_mode + } + + pub fn request_id(&self) -> Option { + self.active_internal_request_id.map(|v| v >> 8) + } + + /// This returns the internal request ID, which is the regular [Self::request_id] specified + /// by the user shifter 8 to the right and then increment with the current sequence commanding + /// step. The value can still be retrieved because it might be required for reply verification. + /// + /// The state machine specifies this request ID for all mode commands related to the + /// current step of sequence commanding. + pub fn internal_request_id(&self) -> Option { + self.active_internal_request_id + } + + /// Retrieve the fallback mode for the current mode of the subsystem by trying to retrieve + /// it from the target table. + /// + /// If the current mode does not have a fallback mode, returns [None]. + /// If the current mode is not inside the target table, returns a [ModeDoesNotExistError]. + /// The fallback mode can and should be commanded when a target keeping violation was detected + /// or after self-commanding to the current mode has failed, which can happen after a failed + /// mode table execution. + pub fn fallback_mode(&self) -> Result, ModeDoesNotExistError> { + self.target_tables + .0 + .get(&self.current_mode) + .ok_or(ModeDoesNotExistError(self.current_mode)) + .map(|v| v.fallback_mode) + } + + /// Add a mode child to the internal [Self::children_mode_store]. + pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeAndSubmode) { + self.children_mode_store.add_component(child, mode); + } + + /// Add a target mode table and an associated sequence mode table. + pub fn add_target_and_sequence_table( + &mut self, + mode: Mode, + target_table_val: TargetTablesMapValue, + sequence_table_val: SequenceTablesMapValue, + ) { + self.target_tables.0.insert(mode, target_table_val); + self.sequence_tables.0.insert(mode, sequence_table_val); + } + + /// Starts a command sequence for a given [mode][Mode]. + /// + /// # Arguments + /// + /// - `mode` - The mode to command + /// - `request_id` - Request ID associated with the command sequence. The value of this value + /// should not be larger than the maximum possible value for 24 bits: (2 ^ 24) - 1 = 16777215 + /// because 8 bits are reserved for internal sequence index tracking. + pub fn start_command_sequence( + &mut self, + mode: Mode, + request_id: RequestId, + ) -> Result<(), StartSequenceError> { + if request_id > 2_u32.pow(24) - 1 { + return Err(StartSequenceError::InvalidRequestId(request_id)); + } + self.active_internal_request_id = Some(request_id << 8); + self.seq_exec_helper.load( + mode, + self.active_internal_request_id.unwrap(), + &self.sequence_tables, + )?; + self.state = ModeTreeHelperState::ModeCommanding; + Ok(()) + } + + pub fn send_announce_mode_cmd_to_children( + &self, + request_id: RequestId, + req_sender: &impl ModeRequestSender, + recursive: bool, + ) -> Result<(), GenericTargetedMessagingError> { + let mut request = ModeRequest::AnnounceMode; + if recursive { + request = ModeRequest::AnnounceModeRecursive; + } + for child in &self.children_mode_store.0 { + req_sender.send_mode_request(request_id, child.id(), request)?; + } + Ok(()) + } + + pub fn state_machine( + &mut self, + opt_reply: Option>, + req_sender: &impl ModeRequestSender, + is_commandable: &impl IsChildCommandable, + ) -> Result { + if let Some(reply) = opt_reply { + if self.handle_mode_reply(&reply)? { + if self.seq_exec_helper.state() == SequenceExecutionHelperState::Idle { + self.transition_to_target_keeping(); + return Ok(SubsystemHelperResult::ModeCommanding( + ModeCommandingResult::Done, + )); + } + return Ok(SubsystemHelperResult::ModeCommanding( + ModeCommandingResult::StepDone, + )); + } + } + match self.state { + ModeTreeHelperState::Idle => Ok(SubsystemHelperResult::Idle), + ModeTreeHelperState::TargetKeeping => { + // We check whether the current mode is modelled by a target table first. + if let Some(target_table) = self.target_tables.0.get(&self.current_mode) { + self.perform_target_keeping(target_table)?; + } + Ok(SubsystemHelperResult::TargetKeeping) + } + ModeTreeHelperState::ModeCommanding => { + let result = self.seq_exec_helper.run( + &self.sequence_tables, + req_sender, + &mut self.children_mode_store, + is_commandable, + )?; + match result { + ModeCommandingResult::Done => { + // By default, the helper will automatically transition into the target keeping + // mode after an executed sequence. + self.transition_to_target_keeping(); + } + ModeCommandingResult::StepDone => { + // Normally, this step is done after all replies were received, but if no + // reply checking is required for a command sequence, the step would never + // be performed, so this function needs to be called here as well. + self.update_internal_req_id(); + } + ModeCommandingResult::AwaitingSuccessCheck => (), + } + Ok(result.into()) + } + } + } + + fn transition_to_target_keeping(&mut self) { + self.state = ModeTreeHelperState::TargetKeeping; + self.current_mode = self.seq_exec_helper.target_mode().unwrap(); + } + + fn perform_target_keeping( + &self, + target_table: &TargetTablesMapValue, + ) -> Result<(), ModeTreeHelperError> { + for entry in &target_table.entries { + if !entry.monitor_state { + continue; + } + let mut target_mode_violated = false; + self.children_mode_store.0.iter().for_each(|val| { + if val.id() == entry.common.target_id { + target_mode_violated = + if let Some(allowed_submode_mask) = entry.allowed_submode_mask() { + let fixed_bits = !allowed_submode_mask; + (val.mode_and_submode().mode() != entry.common.mode_submode.mode()) + && (val.mode_and_submode().submode() & fixed_bits + != entry.common.mode_submode.submode() & fixed_bits) + } else { + val.mode_and_submode() != entry.common.mode_submode + }; + } + }); + if target_mode_violated { + // Target keeping violated. Report violation and fallback mode to user. + return Err(ModeTreeHelperError::TargetKeepingViolation { + fallback_mode: target_table.fallback_mode, + }); + } + } + Ok(()) + } + + fn update_internal_req_id(&mut self) { + let new_internal_req_id = (self.request_id().unwrap() << 8) + | self.seq_exec_helper.current_sequence_index().unwrap() as u32; + self.seq_exec_helper.set_request_id(new_internal_req_id); + self.active_internal_request_id = Some(new_internal_req_id); + } + + // Handles a mode reply message and returns whether the reply completes a step of sequence + // commanding. + fn handle_mode_reply( + &mut self, + reply: &GenericMessage, + ) -> Result { + if !self.children_mode_store.has_component(reply.sender_id()) { + return Ok(false); + } + let mut generic_mode_reply_handler = + |sender_id, mode_and_submode: Option, success: bool| { + let mut partial_step_done = false; + // Tying the reply awaition to the request ID ensures that something like replies + // belonging to older requests do not interfere with the completion handling of + // the mode commanding. This is important for forced mode commands. + let mut handle_awaition = false; + if self.state == ModeTreeHelperState::ModeCommanding + && self.active_internal_request_id.is_some() + && reply.request_id() == self.active_internal_request_id.unwrap() + { + handle_awaition = true; + } + let still_awating_replies = self.children_mode_store.mode_reply_handler( + sender_id, + mode_and_submode, + handle_awaition, + ); + if self.state == ModeTreeHelperState::ModeCommanding + && handle_awaition + && !still_awating_replies.unwrap_or(false) + { + self.seq_exec_helper.confirm_sequence_done(); + self.update_internal_req_id(); + partial_step_done = true; + } + if !success && self.state == ModeTreeHelperState::ModeCommanding { + // The user has to decide how to proceed. + self.state = ModeTreeHelperState::Idle; + return Err(ModeTreeHelperError::ModeCommmandFailure { + seq_table_index: self.seq_exec_helper.current_sequence_index(), + }); + } + Ok(partial_step_done) + }; + match reply.message { + ModeReply::ModeInfo(mode_and_submode) => { + generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true) + } + ModeReply::ModeReply(mode_and_submode) => { + generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true) + } + ModeReply::CantReachMode(_) => { + generic_mode_reply_handler(reply.sender_id(), None, false) + } + ModeReply::WrongMode { reached, .. } => { + generic_mode_reply_handler(reply.sender_id(), Some(reached), true) + } + } + } + + pub fn update_child_mode( + &mut self, + child: ComponentId, + mode: ModeAndSubmode, + ) -> Result<(), TargetNotInModeStoreError> { + let val_mut = self + .children_mode_store + .get_mut(child) + .ok_or(TargetNotInModeStoreError(child))?; + val_mut.mode_and_submode = mode; + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + use crate::{ + ComponentId, + mode::{ + Mode, ModeAndSubmode, ModeReply, ModeRequest, UNKNOWN_MODE, + tests::{ModeReqSenderMock, ModeReqWrapper}, + }, + mode_tree::{ + ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, + SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables, + }, + queue::GenericTargetedMessagingError, + request::{GenericMessage, MessageMetadata, RequestId}, + subsystem::{ModeCommandingResult, ModeTreeHelperState, SequenceExecutionHelperState}, + }; + + #[derive(Debug)] + pub enum ExampleTargetId { + Target0 = 1, + Target1 = 2, + Target2 = 3, + } + + #[derive(Debug)] + pub enum ExampleMode { + Mode0 = 1, + Mode1 = 2, + Mode2 = 3, + } + + #[derive(Debug, Default)] + pub struct IsCommandableMock { + pub commandable_map: std::collections::HashMap, + } + + impl IsChildCommandable for IsCommandableMock { + fn is_commandable(&self, id: ComponentId) -> bool { + self.commandable_map.get(&id).copied().unwrap_or(true) + } + } + + pub struct SequenceExecutorTestbench { + pub sender: ModeReqSenderMock, + pub mode_store: ModeStoreVec, + pub seq_tables: SequenceModeTables, + pub execution_helper: SequenceExecutionHelper, + pub is_commandable_mock: IsCommandableMock, + } + + impl SequenceExecutorTestbench { + pub fn new() -> Self { + let mode_store = create_default_mode_store(); + let (seq_tables, _) = create_simple_sample_seq_tables(); + Self { + sender: ModeReqSenderMock::default(), + mode_store, + seq_tables, + execution_helper: SequenceExecutionHelper::new(), + is_commandable_mock: IsCommandableMock::default(), + } + } + + pub fn get_mode_table(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue { + self.seq_tables.0.get_mut(&(mode as Mode)).unwrap() + } + + pub fn run(&mut self) -> Result { + self.execution_helper.run( + &self.seq_tables, + &self.sender, + &mut self.mode_store, + &self.is_commandable_mock, + ) + } + + fn check_run_is_no_op(&mut self) { + // Assure that no unexpected behaviour occurs. + assert_eq!( + self.execution_helper + .run( + &self.seq_tables, + &self.sender, + &mut self.mode_store, + &self.is_commandable_mock + ) + .unwrap(), + ModeCommandingResult::Done + ); + assert_eq!( + self.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + assert!(self.sender.requests.borrow().is_empty()); + } + + fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) { + assert_eq!( + self.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!(self.sender.requests.borrow().len(), 2); + let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(req_0.target_id, ExampleTargetId::Target0 as ComponentId); + assert_eq!(req_0.request_id, expected_req_id); + assert_eq!( + req_0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE, + forced: false + } + ); + let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req_1.target_id, ExampleTargetId::Target1 as ComponentId); + assert_eq!( + req_1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE, + forced: false + } + ); + } + fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) { + assert_eq!( + self.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!(self.sender.requests.borrow().len(), 1); + let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(req_0.target_id, ExampleTargetId::Target2 as ComponentId); + assert_eq!(req_0.request_id, expected_req_id); + assert_eq!( + req_0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + forced: false + } + ); + } + + fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) { + assert_eq!( + self.execution_helper.target_mode().unwrap(), + ExampleMode::Mode0 as Mode + ); + assert_eq!(self.execution_helper.current_sequence_index().unwrap(), 0); + assert_eq!(self.sender.requests.borrow().len(), 2); + let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(req_0.target_id, ExampleTargetId::Target0 as ComponentId); + assert_eq!(req_0.request_id, expected_req_id); + assert_eq!( + req_0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE, + forced: false + } + ); + let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req_1.target_id, ExampleTargetId::Target1 as ComponentId); + assert_eq!( + req_1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE, + forced: false + } + ); + } + } + + fn create_default_mode_store() -> ModeStoreVec { + let mut mode_store = ModeStoreVec::default(); + mode_store.add_component(ExampleTargetId::Target0 as ComponentId, UNKNOWN_MODE); + mode_store.add_component(ExampleTargetId::Target1 as ComponentId, UNKNOWN_MODE); + mode_store.add_component(ExampleTargetId::Target2 as ComponentId, UNKNOWN_MODE); + mode_store + } + + fn create_simple_sample_seq_tables() -> (SequenceModeTables, TargetModeTables) { + let mut seq_tables = SequenceModeTables::default(); + // Mode 0 - One step command + let mut table_val = SequenceTablesMapValue::new("MODE_0"); + let mut table_seq_0 = SequenceTableMapTable::new("MODE_0_SEQ_0"); + table_seq_0.add_entry(SequenceTableEntry::new( + "TARGET_0", + ExampleTargetId::Target0 as ComponentId, + SUBSYSTEM_MD0_TGT0_MODE, + false, + )); + table_seq_0.add_entry(SequenceTableEntry::new( + "TARGET_1", + ExampleTargetId::Target1 as ComponentId, + SUBSYSTEM_MD0_TGT1_MODE, + false, + )); + table_val.add_sequence_table(table_seq_0); + seq_tables.0.insert(ExampleMode::Mode0 as u32, table_val); + + // Mode 1 - Multi Step command + let mut table_val = SequenceTablesMapValue::new("MODE_1"); + let mut table_seq_0 = SequenceTableMapTable::new("MODE_1_SEQ_0"); + table_seq_0.add_entry(SequenceTableEntry::new( + "MD1_SEQ0_TGT0", + ExampleTargetId::Target0 as ComponentId, + SUBSYSTEM_MD1_ST0_TGT0_MODE, + false, + )); + table_seq_0.add_entry(SequenceTableEntry::new( + "MD1_SEQ0_TGT1", + ExampleTargetId::Target1 as ComponentId, + SUBSYSTEM_MD1_ST0_TGT1_MODE, + false, + )); + table_val.add_sequence_table(table_seq_0); + let mut table_seq_1 = SequenceTableMapTable::new("MODE_1_SEQ_1"); + table_seq_1.add_entry(SequenceTableEntry::new( + "MD1_SEQ1_TGT2", + ExampleTargetId::Target2 as ComponentId, + SUBSYSTEM_MD1_ST1_TGT2_MODE, + false, + )); + table_val.add_sequence_table(table_seq_1); + seq_tables.0.insert(ExampleMode::Mode1 as u32, table_val); + + let mode_tables = TargetModeTables::default(); + // TODO: Write mode tables. + (seq_tables, mode_tables) + } + + pub struct SubsystemHelperTestbench { + pub sender: ModeReqSenderMock, + pub helper: SubsystemCommandingHelper, + pub is_commandable_mock: IsCommandableMock, + } + + impl SubsystemHelperTestbench { + pub fn new() -> Self { + let (sequence_tables, target_tables) = create_simple_sample_seq_tables(); + Self { + sender: ModeReqSenderMock::default(), + helper: SubsystemCommandingHelper::new( + create_default_mode_store(), + target_tables, + sequence_tables, + ), + is_commandable_mock: IsCommandableMock::default(), + } + } + + pub fn start_command_sequence( + &mut self, + mode: ExampleMode, + request_id: RequestId, + ) -> Result<(), StartSequenceError> { + self.helper.start_command_sequence(mode as Mode, request_id) + } + + pub fn send_announce_mode_cmd_to_children( + &mut self, + request_id: RequestId, + recursive: bool, + ) -> Result<(), GenericTargetedMessagingError> { + self.helper + .send_announce_mode_cmd_to_children(request_id, &self.sender, recursive) + } + + pub fn get_sequence_tables(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue { + self.helper + .sequence_tables + .0 + .get_mut(&(mode as Mode)) + .unwrap() + } + + pub fn state_machine( + &mut self, + opt_reply: Option>, + ) -> Result { + self.helper + .state_machine(opt_reply, &self.sender, &self.is_commandable_mock) + } + + pub fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) { + assert_eq!(self.sender.requests.borrow().len(), 2); + let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req0.request_id, expected_req_id); + assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId); + assert_eq!( + req0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE, + forced: false + } + ); + + let req1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req1.request_id, expected_req_id); + assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId); + assert_eq!( + req1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE, + forced: false + } + ); + } + + pub fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) { + assert_eq!(self.sender.requests.borrow().len(), 2); + let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req0.request_id, expected_req_id); + assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId); + assert_eq!( + req0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE, + forced: false + } + ); + + let req1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req1.request_id, expected_req_id); + assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId); + assert_eq!( + req1.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE, + forced: false + } + ); + } + + pub fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) { + assert_eq!(self.sender.requests.borrow().len(), 1); + let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req0.request_id, expected_req_id); + assert_eq!(req0.target_id, ExampleTargetId::Target2 as ComponentId); + assert_eq!( + req0.request, + ModeRequest::SetMode { + mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + forced: false + } + ); + } + } + + const SUBSYSTEM_MD0_TGT0_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0); + const SUBSYSTEM_MD0_TGT1_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); + + const SUBSYSTEM_MD1_ST0_TGT0_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0); + const SUBSYSTEM_MD1_ST0_TGT1_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0); + const SUBSYSTEM_MD1_ST1_TGT2_MODE: ModeAndSubmode = + ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); + + #[test] + fn test_init_state() { + let execution_helper = SequenceExecutionHelper::new(); + assert_eq!(execution_helper.state(), SequenceExecutionHelperState::Idle); + assert!(!execution_helper.awaiting_success_check()); + assert!(execution_helper.target_mode().is_none()); + assert!(execution_helper.current_sequence_index().is_none()); + } + + #[test] + fn test_sequence_execution_helper_no_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode0 as Mode + ); + assert_eq!( + tb.run().expect("sequence exeecution helper run failure"), + ModeCommandingResult::Done + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + assert!(!tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md0(expected_req_id); + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_with_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let mode0_table = tb.get_mode_table(ExampleMode::Mode0); + mode0_table.entries[0].entries[0].check_success = true; + mode0_table.entries[0].entries[1].check_success = true; + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode0 as Mode + ); + assert_eq!( + tb.run().expect("sequence exeecution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + // These are not cleared, even if the execution helper is already IDLE. This is okay for + // now. + assert!(tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md0(expected_req_id); + tb.execution_helper.confirm_sequence_done(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_with_partial_check() { + let mut tb = SequenceExecutorTestbench::new(); + let mode0_table = tb.get_mode_table(ExampleMode::Mode0); + mode0_table.entries[0].entries[0].check_success = true; + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + // These are not cleared, even if the execution helper is already IDLE. This is okay for + // now. + assert!(tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md0(expected_req_id); + tb.execution_helper.confirm_sequence_done(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_multi_step_no_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::StepDone + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md1_step0(expected_req_id); + assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1); + + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::Done + ); + tb.generic_checks_subsystem_md1_step1(expected_req_id); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Idle + ); + tb.check_run_is_no_op(); + } + + #[test] + fn test_sequence_execution_helper_multi_step_full_success_check() { + let mut tb = SequenceExecutorTestbench::new(); + let expected_req_id = 1; + tb.execution_helper + .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables) + .unwrap(); + let mode1_table = tb.get_mode_table(ExampleMode::Mode1); + mode1_table.entries[0].entries[0].check_success = true; + mode1_table.entries[0].entries[1].check_success = true; + mode1_table.entries[1].entries[0].check_success = true; + + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::Busy + ); + assert!(!tb.execution_helper.awaiting_success_check()); + assert_eq!( + tb.execution_helper.target_mode().unwrap(), + ExampleMode::Mode1 as Mode + ); + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + assert!(tb.execution_helper.awaiting_success_check()); + tb.generic_checks_subsystem_md1_step0(expected_req_id); + assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 0); + tb.execution_helper.confirm_sequence_done(); + + assert_eq!( + tb.run().expect("sequence execution helper run failure"), + ModeCommandingResult::AwaitingSuccessCheck + ); + assert_eq!( + tb.execution_helper.state(), + SequenceExecutionHelperState::AwaitingSuccessCheck + ); + assert!(tb.execution_helper.awaiting_success_check()); + assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1); + tb.generic_checks_subsystem_md1_step1(expected_req_id); + tb.execution_helper.confirm_sequence_done(); + tb.check_run_is_no_op(); + } + + // TODO: Test subsystem commanding helper + #[test] + fn test_subsystem_helper_basic_state() { + let tb = SubsystemHelperTestbench::new(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::Idle); + assert!(tb.helper.active_internal_request_id.is_none()); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + assert!(tb.helper.request_id().is_none()); + } + + #[test] + fn test_subsystem_helper_announce_recursive() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.send_announce_mode_cmd_to_children(expected_req_id, true) + .unwrap(); + assert_eq!(tb.sender.requests.borrow().len(), 3); + let check_req = |req: ModeReqWrapper, target_id: ComponentId| { + assert_eq!(req.target_id, target_id); + assert_eq!(req.request_id, expected_req_id); + assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); + }; + let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req0, ExampleTargetId::Target0 as ComponentId); + let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req1, ExampleTargetId::Target1 as ComponentId); + let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req2, ExampleTargetId::Target2 as ComponentId); + } + + #[test] + fn test_subsystem_helper_announce() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.send_announce_mode_cmd_to_children(expected_req_id, false) + .unwrap(); + assert_eq!(tb.sender.requests.borrow().len(), 3); + let check_req = |req: ModeReqWrapper, target_id: ComponentId| { + assert_eq!(req.target_id, target_id); + assert_eq!(req.request_id, expected_req_id); + assert_eq!(req.request, ModeRequest::AnnounceMode); + }; + let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req0, ExampleTargetId::Target0 as ComponentId); + let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req1, ExampleTargetId::Target1 as ComponentId); + let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap(); + check_req(req2, ExampleTargetId::Target2 as ComponentId); + } + + #[test] + fn test_subsystem_helper_cmd_mode0_no_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.start_command_sequence(ExampleMode::Mode0, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.request_id().unwrap(), 1); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap()); + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_no_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + // Need to cache this before it is incremented, because it is incremented + // immediately in the state machine (no reply checking) + let expected_req_id = tb.helper.internal_request_id().unwrap(); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + // Assert that this was already incremented because no reply checking is necessary. + assert_eq!( + tb.helper.internal_request_id().unwrap(), + expected_req_id + 1 + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(expected_req_id); + // Second commanding step. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode0_with_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode0); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = true; + tb.start_command_sequence(ExampleMode::Mode0, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // One success reply still expected. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_with_success_checks() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = true; + seq_tables.entries[1].entries[0].check_success = true; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // One success reply still expected. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + let mode_reply_ok = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target2 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_0() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = false; + seq_tables.entries[1].entries[0].check_success = false; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // One success reply still expected. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + + // Inserting the reply makes no difference: This call completes the sequence commanding. + let mode_reply_ok = GenericMessage::new( + MessageMetadata::new(expected_req_id, ExampleTargetId::Target2 as ComponentId), + ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + // The internal request ID is still cached. + tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } + + #[test] + fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_1() { + let mut tb = SubsystemHelperTestbench::new(); + let expected_req_id = 1; + let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); + seq_tables.entries[0].entries[0].check_success = true; + seq_tables.entries[0].entries[1].check_success = false; + seq_tables.entries[1].entries[0].check_success = false; + tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) + .unwrap(); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); + assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); + tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); + let mode_reply_ok_0 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target0 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), + ); + let mode_reply_ok_1 = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target1 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), + ); + // This completes the step, so the next FSM call will perform the next step + // in sequence commanding. + assert_eq!( + tb.state_machine(Some(mode_reply_ok_0)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok_1)).unwrap(), + SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) + ); + + // Inserting the reply makes no difference: Sequence command is done and target keeping + // is performed. + let mode_reply_ok = GenericMessage::new( + MessageMetadata::new( + tb.helper.internal_request_id().unwrap(), + ExampleTargetId::Target2 as ComponentId, + ), + ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), + ); + assert_eq!( + tb.state_machine(Some(mode_reply_ok)).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + // The internal request ID is still cached. + tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + + // FSM call should be a no-op. + assert_eq!( + tb.state_machine(None).unwrap(), + SubsystemHelperResult::TargetKeeping + ); + assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + } +} diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs index c5679ff..ea2953c 100644 --- a/satrs/src/mode.rs +++ b/satrs/src/mode.rs @@ -1,761 +1,4 @@ -use core::mem::size_of; -use satrs_shared::res_code::ResultU16; -#[cfg(feature = "serde")] -use serde::{Deserialize, Serialize}; -use spacepackets::ByteConversionError; - -#[cfg(feature = "alloc")] -pub use alloc_mod::*; - -#[cfg(feature = "std")] -pub use std_mod::*; - -use crate::{ - ComponentId, - queue::{GenericReceiveError, GenericSendError}, - request::{ - GenericMessage, MessageMetadata, MessageReceiverProvider, MessageReceiverWithId, RequestId, - }, -}; - -pub type Mode = u32; -pub type Submode = u16; - -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct ModeAndSubmode { - mode: Mode, - submode: Submode, -} - -pub const INVALID_MODE_VAL: Mode = Mode::MAX; -pub const UNKNOWN_MODE_VAL: Mode = Mode::MAX - 1; -pub const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(INVALID_MODE_VAL, 0); -pub const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(UNKNOWN_MODE_VAL, 0); - -impl ModeAndSubmode { - pub const RAW_LEN: usize = size_of::() + size_of::(); - - pub const fn new_mode_only(mode: Mode) -> Self { - Self { mode, submode: 0 } - } - - pub const fn new(mode: Mode, submode: Submode) -> Self { - Self { mode, submode } - } - - pub fn from_be_bytes(buf: &[u8]) -> Result { - if buf.len() < 6 { - return Err(ByteConversionError::FromSliceTooSmall { - expected: Self::RAW_LEN, - found: buf.len(), - }); - } - Ok(Self { - mode: Mode::from_be_bytes(buf[0..size_of::()].try_into().unwrap()), - submode: Submode::from_be_bytes( - buf[size_of::()..size_of::() + size_of::()] - .try_into() - .unwrap(), - ), - }) - } - - pub fn write_to_be_bytes(&self, buf: &mut [u8]) -> Result { - if buf.len() < Self::RAW_LEN { - return Err(ByteConversionError::ToSliceTooSmall { - expected: Self::RAW_LEN, - found: buf.len(), - }); - } - buf[0..size_of::()].copy_from_slice(&self.mode.to_be_bytes()); - buf[size_of::()..Self::RAW_LEN].copy_from_slice(&self.submode.to_be_bytes()); - Ok(Self::RAW_LEN) - } - - pub fn mode(&self) -> Mode { - self.mode - } - - pub fn submode(&self) -> Submode { - self.submode - } -} -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct TargetedModeCommand { - pub address: ComponentId, - pub mode_submode: ModeAndSubmode, -} - -impl TargetedModeCommand { - pub const fn new(address: ComponentId, mode_submode: ModeAndSubmode) -> Self { - Self { - address, - mode_submode, - } - } - - pub fn address(&self) -> ComponentId { - self.address - } - - pub fn mode_submode(&self) -> ModeAndSubmode { - self.mode_submode - } - - pub fn mode(&self) -> u32 { - self.mode_submode.mode - } - - pub fn submode(&self) -> u16 { - self.mode_submode.submode - } -} - -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub enum ModeRequest { - /// Mode information. Can be used to notify other components of changed modes. - ModeInfo(ModeAndSubmode), - SetMode { - mode_and_submode: ModeAndSubmode, - forced: bool, - }, - ReadMode, - AnnounceMode, - AnnounceModeRecursive, -} - -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct TargetedModeRequest { - target_id: ComponentId, - mode_request: ModeRequest, -} - -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub enum ModeReply { - /// Mode information. Can be used to notify other components of changed modes. - ModeInfo(ModeAndSubmode), - /// Reply to a mode request to confirm the commanded mode was reached. - ModeReply(ModeAndSubmode), - // Can not reach the commanded mode. Contains a reason as a [ResultU16]. - CantReachMode(ResultU16), - /// We are in the wrong mode for unknown reasons. Contains the expected and reached mode. - WrongMode { - expected: ModeAndSubmode, - reached: ModeAndSubmode, - }, -} - -pub type GenericModeReply = GenericMessage; - -pub trait ModeRequestSender { - fn local_channel_id(&self) -> ComponentId; - fn send_mode_request( - &self, - request_id: RequestId, - target_id: ComponentId, - request: ModeRequest, - ) -> Result<(), GenericSendError>; -} - -pub trait ModeRequestReceiver { - fn try_recv_mode_request( - &self, - ) -> Result>, GenericReceiveError>; -} - -impl> ModeRequestReceiver - for MessageReceiverWithId -{ - fn try_recv_mode_request( - &self, - ) -> Result>, GenericReceiveError> { - self.try_recv_message() - } -} - -#[derive(Debug, Clone, thiserror::Error)] -pub enum ModeError { - #[error("Messaging send error: {0}")] - Send(#[from] GenericSendError), - #[error("Messaging receive error: {0}")] - Receive(#[from] GenericReceiveError), - #[error("busy with other mode request")] - Busy, -} - -pub trait ModeProvider { - fn mode_and_submode(&self) -> ModeAndSubmode; - - fn mode(&self) -> Mode { - self.mode_and_submode().mode() - } - - fn submode(&self) -> Submode { - self.mode_and_submode().submode() - } -} - -pub trait ModeRequestHandler: ModeProvider { - type Error; - - fn start_transition( - &mut self, - requestor: MessageMetadata, - mode_and_submode: ModeAndSubmode, - forced: bool, - ) -> Result<(), Self::Error>; - - fn announce_mode(&self, requestor_info: Option, recursive: bool); - - fn handle_mode_reached( - &mut self, - requestor_info: Option, - ) -> Result<(), Self::Error>; - - fn handle_mode_info( - &mut self, - requestor_info: MessageMetadata, - info: ModeAndSubmode, - ) -> Result<(), Self::Error>; - - fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - reply: ModeReply, - ) -> Result<(), Self::Error>; - - fn handle_mode_request( - &mut self, - request: GenericMessage, - ) -> Result<(), Self::Error> { - match request.message { - ModeRequest::SetMode { - 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()), - ), - ModeRequest::AnnounceMode => { - self.announce_mode(Some(request.requestor_info), false); - Ok(()) - } - ModeRequest::AnnounceModeRecursive => { - self.announce_mode(Some(request.requestor_info), true); - Ok(()) - } - ModeRequest::ModeInfo(info) => self.handle_mode_info(request.requestor_info, info), - } - } -} - -pub trait ModeReplyReceiver { - fn try_recv_mode_reply(&self) - -> Result>, GenericReceiveError>; -} - -impl> ModeReplyReceiver - for MessageReceiverWithId -{ - fn try_recv_mode_reply( - &self, - ) -> Result>, GenericReceiveError> { - self.try_recv_message() - } -} - -pub trait ModeReplySender { - fn local_channel_id(&self) -> ComponentId; - - /// The requestor is assumed to be the target of the reply. - fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - reply: ModeReply, - ) -> Result<(), GenericSendError>; -} - -#[cfg(feature = "alloc")] -pub mod alloc_mod { - use crate::{ - queue::{GenericReceiveError, GenericSendError}, - request::{ - MessageSenderAndReceiver, MessageSenderMap, MessageSenderProvider, - MessageSenderStoreProvider, RequestAndReplySenderAndReceiver, - }, - }; - - use super::*; - - impl> MessageSenderMap { - pub fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - target_id: ComponentId, - request: ModeReply, - ) -> Result<(), GenericSendError> { - self.send_message(requestor_info, target_id, request) - } - - pub fn add_reply_target(&mut self, target_id: ComponentId, request_sender: S) { - self.add_message_target(target_id, request_sender) - } - } - - impl< - From, - Sender: MessageSenderProvider, - Receiver: MessageReceiverProvider, - SenderStore: MessageSenderStoreProvider, - > ModeReplySender for MessageSenderAndReceiver - { - fn local_channel_id(&self) -> ComponentId { - self.local_channel_id_generic() - } - - fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - request: ModeReply, - ) -> Result<(), GenericSendError> { - self.message_sender_store.send_message( - MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()), - requestor_info.sender_id(), - request, - ) - } - } - - impl< - To, - Sender: MessageSenderProvider, - Receiver: MessageReceiverProvider, - SenderStore: MessageSenderStoreProvider, - > ModeReplyReceiver for MessageSenderAndReceiver - { - fn try_recv_mode_reply( - &self, - ) -> Result>, GenericReceiveError> { - self.message_receiver.try_recv_message() - } - } - - impl< - Request, - ReqSender: MessageSenderProvider, - ReqReceiver: MessageReceiverProvider, - ReqSenderStore: MessageSenderStoreProvider, - Reply, - ReplySender: MessageSenderProvider, - ReplyReceiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > - RequestAndReplySenderAndReceiver< - Request, - ReqSender, - ReqReceiver, - ReqSenderStore, - Reply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > - { - pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: ReplySender) { - self.reply_sender_store - .add_message_target(target_id, reply_sender) - } - } - - impl< - Request, - ReqSender: MessageSenderProvider, - ReqReceiver: MessageReceiverProvider, - ReqSenderStore: MessageSenderStoreProvider, - ReplySender: MessageSenderProvider, - ReplyReceiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > ModeReplySender - for RequestAndReplySenderAndReceiver< - Request, - ReqSender, - ReqReceiver, - ReqSenderStore, - ModeReply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > - { - fn local_channel_id(&self) -> ComponentId { - self.local_channel_id_generic() - } - - fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - reply: ModeReply, - ) -> Result<(), GenericSendError> { - self.reply_sender_store.send_message( - MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()), - requestor_info.sender_id(), - reply, - ) - } - } - - impl< - Request, - ReqSender: MessageSenderProvider, - ReqReceiver: MessageReceiverProvider, - ReqSenderStore: MessageSenderStoreProvider, - ReplySender: MessageSenderProvider, - ReplyReceiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > ModeReplyReceiver - for RequestAndReplySenderAndReceiver< - Request, - ReqSender, - ReqReceiver, - ReqSenderStore, - ModeReply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > - { - fn try_recv_mode_reply( - &self, - ) -> Result>, GenericReceiveError> { - self.reply_receiver.try_recv_message() - } - } - - /// Helper type definition for a mode handler which can handle mode requests. - pub type ModeRequestHandlerInterface = - MessageSenderAndReceiver; - - impl< - Sender: MessageSenderProvider, - Receiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > ModeRequestHandlerInterface - { - pub fn try_recv_mode_request( - &self, - ) -> Result>, GenericReceiveError> { - self.try_recv_message() - } - - pub fn send_mode_reply( - &self, - requestor_info: MessageMetadata, - reply: ModeReply, - ) -> Result<(), GenericSendError> { - self.send_message( - requestor_info.request_id(), - requestor_info.sender_id(), - reply, - ) - } - } - - /// Helper type defintion for a mode handler object which can send mode requests and receive - /// mode replies. - pub type ModeRequestorInterface = - MessageSenderAndReceiver; - - impl< - Sender: MessageSenderProvider, - Receiver: MessageReceiverProvider, - RequestSenderStore: MessageSenderStoreProvider, - > ModeRequestorInterface - { - pub fn try_recv_mode_reply( - &self, - ) -> Result>, GenericReceiveError> { - self.try_recv_message() - } - - pub fn send_mode_request( - &self, - request_id: RequestId, - target_id: ComponentId, - reply: ModeRequest, - ) -> Result<(), GenericSendError> { - self.send_message(request_id, target_id, reply) - } - } - - /// Helper type defintion for a mode handler object which can both send mode requests and - /// process mode requests. - pub type ModeInterface< - ReqSender, - ReqReceiver, - ReqSenderStore, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > = RequestAndReplySenderAndReceiver< - ModeRequest, - ReqSender, - ReqReceiver, - ReqSenderStore, - ModeReply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - >; - - impl> MessageSenderMap { - pub fn send_mode_request( - &self, - requestor_info: MessageMetadata, - target_id: ComponentId, - request: ModeRequest, - ) -> Result<(), GenericSendError> { - self.send_message(requestor_info, target_id, request) - } - - pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: S) { - self.add_message_target(target_id, request_sender) - } - } - - impl< - To, - Sender: MessageSenderProvider, - Receiver: MessageReceiverProvider, - SenderStore: MessageSenderStoreProvider, - > ModeRequestReceiver - for MessageSenderAndReceiver - { - fn try_recv_mode_request( - &self, - ) -> Result>, GenericReceiveError> { - self.message_receiver.try_recv_message() - } - } - - impl< - From, - Sender: MessageSenderProvider, - Receiver: MessageReceiverProvider, - SenderStore: MessageSenderStoreProvider, - > ModeRequestSender - for MessageSenderAndReceiver - { - fn local_channel_id(&self) -> ComponentId { - self.local_channel_id_generic() - } - - fn send_mode_request( - &self, - request_id: RequestId, - target_id: ComponentId, - request: ModeRequest, - ) -> Result<(), GenericSendError> { - self.message_sender_store.send_message( - MessageMetadata::new(request_id, self.local_channel_id()), - target_id, - request, - ) - } - } - - impl< - ReqSender: MessageSenderProvider, - ReqReceiver: MessageReceiverProvider, - ReqSenderStore: MessageSenderStoreProvider, - Reply, - ReplySender: MessageSenderProvider, - ReplyReceiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > - RequestAndReplySenderAndReceiver< - ModeRequest, - ReqSender, - ReqReceiver, - ReqSenderStore, - Reply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > - { - pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: ReqSender) { - self.request_sender_store - .add_message_target(target_id, request_sender) - } - } - - impl< - ReqSender: MessageSenderProvider, - ReqReceiver: MessageReceiverProvider, - ReqSenderStore: MessageSenderStoreProvider, - Reply, - ReplySender: MessageSenderProvider, - ReplyReceiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > ModeRequestSender - for RequestAndReplySenderAndReceiver< - ModeRequest, - ReqSender, - ReqReceiver, - ReqSenderStore, - Reply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > - { - fn local_channel_id(&self) -> ComponentId { - self.local_channel_id_generic() - } - - fn send_mode_request( - &self, - request_id: RequestId, - target_id: ComponentId, - request: ModeRequest, - ) -> Result<(), GenericSendError> { - self.request_sender_store.send_message( - MessageMetadata::new(request_id, self.local_channel_id()), - target_id, - request, - ) - } - } - - impl< - ReqSender: MessageSenderProvider, - ReqReceiver: MessageReceiverProvider, - ReqSenderStore: MessageSenderStoreProvider, - Reply, - ReplySender: MessageSenderProvider, - ReplyReceiver: MessageReceiverProvider, - ReplySenderStore: MessageSenderStoreProvider, - > ModeRequestReceiver - for RequestAndReplySenderAndReceiver< - ModeRequest, - ReqSender, - ReqReceiver, - ReqSenderStore, - Reply, - ReplySender, - ReplyReceiver, - ReplySenderStore, - > - { - fn try_recv_mode_request( - &self, - ) -> Result>, GenericReceiveError> { - self.request_receiver.try_recv_message() - } - } -} - -#[cfg(feature = "std")] -pub mod std_mod { - use std::sync::mpsc; - - use crate::request::{MessageSenderList, OneMessageSender}; - - use super::*; - - pub type ModeRequestHandlerMpsc = ModeRequestHandlerInterface< - mpsc::Sender>, - mpsc::Receiver>, - MessageSenderList>>, - >; - pub type ModeRequestHandlerMpscBounded = ModeRequestHandlerInterface< - mpsc::SyncSender>, - mpsc::Receiver>, - MessageSenderList>>, - >; - - pub type ModeRequestorOneChildMpsc = ModeRequestorInterface< - mpsc::Sender>, - mpsc::Receiver>, - OneMessageSender>>, - >; - pub type ModeRequestorOneChildBoundedMpsc = ModeRequestorInterface< - mpsc::SyncSender>, - mpsc::Receiver>, - OneMessageSender>>, - >; - pub type ModeRequestorChildListMpsc = ModeRequestorInterface< - mpsc::Sender>, - mpsc::Receiver>, - MessageSenderList>>, - >; - pub type ModeRequestorChildListBoundedMpsc = ModeRequestorInterface< - mpsc::SyncSender>, - mpsc::Receiver>, - MessageSenderList>>, - >; - - pub type ModeRequestorAndHandlerMpsc = ModeInterface< - mpsc::Sender>, - mpsc::Receiver>, - MessageSenderList>>, - mpsc::Sender>, - mpsc::Receiver>, - MessageSenderList>>, - >; - pub type ModeRequestorAndHandlerMpscBounded = ModeInterface< - mpsc::SyncSender>, - mpsc::Receiver>, - MessageSenderList>>, - mpsc::SyncSender>, - mpsc::Receiver>, - MessageSenderList>>, - >; -} +pub type ModeRaw = u32; #[cfg(test)] -pub(crate) mod tests { - use core::cell::RefCell; - use std::collections::VecDeque; - - use crate::{ComponentId, request::RequestId}; - - use super::*; - - pub struct ModeReqWrapper { - pub request_id: RequestId, - pub target_id: ComponentId, - pub request: ModeRequest, - } - - #[derive(Default)] - pub struct ModeReqSenderMock { - pub requests: RefCell>, - } - - impl ModeRequestSender for ModeReqSenderMock { - fn local_channel_id(&self) -> crate::ComponentId { - 0 - } - - fn send_mode_request( - &self, - request_id: RequestId, - target_id: ComponentId, - request: ModeRequest, - ) -> Result<(), GenericSendError> { - self.requests.borrow_mut().push_back(ModeReqWrapper { - request_id, - target_id, - request, - }); - Ok(()) - } - } -} +pub(crate) mod tests {} diff --git a/satrs/src/mode_tree.rs b/satrs/src/mode_tree.rs index 883ef21..c4d57db 100644 --- a/satrs/src/mode_tree.rs +++ b/satrs/src/mode_tree.rs @@ -1,11 +1,7 @@ use alloc::vec::Vec; use hashbrown::HashMap; -use crate::{ - ComponentId, - mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, Submode}, - request::MessageSenderProvider, -}; +use crate::{ComponentId, mode::ModeRaw}; #[cfg(feature = "alloc")] pub use alloc_mod::*; @@ -14,44 +10,6 @@ pub use alloc_mod::*; pub trait ModeNode { fn id(&self) -> ComponentId; } -/// 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: ModeNode { - type Sender: MessageSenderProvider; - - 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: ModeNode { - type Sender: MessageSenderProvider; - - 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( - parent: &mut impl ModeParent, - request_sender: ReqSender, - child: &mut impl ModeChild, - reply_sender: ReplySender, -) { - parent.add_mode_child(child.id(), request_sender); - child.add_mode_parent(parent.id(), reply_sender); -} #[derive(Debug, Copy, Clone, PartialEq, Eq)] pub enum TableEntryType { @@ -78,68 +36,48 @@ pub struct ModeTableEntryCommon { /// - For sequence tables, this denotes the mode which will be commanded /// - For target tables, this is the mode which the target children should have and which /// might be monitored depending on configuration. - pub mode_submode: ModeAndSubmode, - /// This mask allows to specify multiple allowed submodes for a given mode. - pub allowed_submode_mask: Option, -} - -impl ModeTableEntryCommon { - pub fn set_allowed_submode_mask(&mut self, mask: Submode) { - self.allowed_submode_mask = Some(mask); - } - - pub fn allowed_submode_mask(&self) -> Option { - self.allowed_submode_mask - } + pub mode: ModeRaw, } /// An entry for the target tables. #[derive(Debug)] pub struct TargetTableEntry { pub common: ModeTableEntryCommon, + /// To allow multiple modes, you can specify mask bits which are ignored during mode + /// checking. + pub ignored_bits: Option, pub monitor_state: bool, } impl TargetTableEntry { - pub fn new( + pub fn new(name: &'static str, target_id: ComponentId, mode: ModeRaw) -> Self { + Self { + common: ModeTableEntryCommon { + name, + target_id, + mode, + }, + ignored_bits: None, + monitor_state: true, + } + } + + pub fn new_with_ignored_bits( name: &'static str, target_id: ComponentId, - mode_submode: ModeAndSubmode, - allowed_submode_mask: Option, + mode: ModeRaw, + ignored_bits: Option, ) -> Self { Self { common: ModeTableEntryCommon { name, target_id, - mode_submode, - allowed_submode_mask, + mode, }, + ignored_bits, monitor_state: true, } } - - pub fn new_with_precise_submode( - name: &'static str, - target_id: ComponentId, - mode_submode: ModeAndSubmode, - ) -> Self { - Self { - common: ModeTableEntryCommon { - name, - target_id, - mode_submode, - allowed_submode_mask: None, - }, - monitor_state: true, - } - } - - delegate::delegate! { - to self.common { - pub fn set_allowed_submode_mask(&mut self, mask: Submode); - pub fn allowed_submode_mask(&self) -> Option; - } - } } /// An entry for the sequence tables. @@ -156,15 +94,14 @@ impl SequenceTableEntry { pub fn new( name: &'static str, target_id: ComponentId, - mode_submode: ModeAndSubmode, + mode: ModeRaw, check_success: bool, ) -> Self { Self { common: ModeTableEntryCommon { name, target_id, - mode_submode, - allowed_submode_mask: None, + mode, }, check_success, } @@ -172,8 +109,6 @@ impl SequenceTableEntry { delegate::delegate! { to self.common { - pub fn set_allowed_submode_mask(&mut self, mask: Submode); - pub fn allowed_submode_mask(&self) -> Option; } } } @@ -188,16 +123,16 @@ pub struct ModeStoreValue { /// ID of the mode component. id: ComponentId, /// Current mode and submode of the component. - pub mode_and_submode: ModeAndSubmode, + pub mode: ModeRaw, /// State information to track whether a reply should be awaited for the mode component. pub awaiting_reply: bool, } impl ModeStoreValue { - pub fn new(id: ComponentId, mode_and_submode: ModeAndSubmode) -> Self { + pub fn new(id: ComponentId, mode: ModeRaw) -> Self { Self { id, - mode_and_submode, + mode, awaiting_reply: false, } } @@ -206,13 +141,26 @@ impl ModeStoreValue { self.id } - pub fn mode_and_submode(&self) -> ModeAndSubmode { - self.mode_and_submode + pub fn mode(&self) -> ModeRaw { + self.mode } } +pub trait SequenceTableProvider { + fn sequence_at_index(&self, index: u8) -> Option<&[SequenceTableEntry]>; + fn number_of_sequences(&self) -> usize; +} + +#[derive(Debug, thiserror::Error)] +#[error("mode store is full")] +pub struct FullError; + pub trait ModeStoreProvider { - fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode); + fn add_component( + &mut self, + target_id: ComponentId, + initial_mode: ModeRaw, + ) -> Result<(), FullError>; fn has_component(&self, target_id: ComponentId) -> bool; @@ -223,11 +171,11 @@ pub trait ModeStoreProvider { /// Generic handler for mode replies received from child components. /// /// Implementation should clear the awaition flag if the `handle_reply_awaition` argument is - /// true and returns whether any children are still awaiting replies. If the flag is not set + /// true and returns whether any children are still awaiting replies. fn mode_reply_handler_with_reply_awaition( &mut self, sender_id: ComponentId, - reported_mode_and_submode: Option, + reported_mode_and_submode: Option, ) -> bool { self.mode_reply_handler(sender_id, reported_mode_and_submode, true) .unwrap_or(false) @@ -236,21 +184,125 @@ pub trait ModeStoreProvider { fn mode_reply_handler_without_reply_awaition( &mut self, sender_id: ComponentId, - reported_mode_and_submode: Option, + reported_mode_and_submode: Option, ) { self.mode_reply_handler(sender_id, reported_mode_and_submode, false); } + /// This function should return whether any children are still awaiting replies. + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + with_reply_awaition: bool, + ) -> Option; + + fn set_reply_awaition_flag(&mut self, target_id: ComponentId); +} + +impl ModeStoreProvider for heapless::Vec { + fn add_component( + &mut self, + target_id: ComponentId, + initial_mode: ModeRaw, + ) -> Result<(), FullError> { + if self.is_full() { + return Err(FullError); + } + self.push(ModeStoreValue::new(target_id, initial_mode)) + .unwrap(); + Ok(()) + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.iter().any(|val| val.id == target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.iter().find(|val| val.id == target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.iter_mut().find(|val| val.id == target_id) + } + fn mode_reply_handler( &mut self, sender_id: ComponentId, - reported_mode_and_submode: Option, + reported_mode_and_submode: Option, with_reply_awaition: bool, - ) -> Option; + ) -> Option { + let mut still_awating_replies = None; + if with_reply_awaition { + still_awating_replies = Some(false); + } + self.iter_mut().for_each(|val| { + if val.id() == sender_id { + if let Some(mode) = reported_mode_and_submode { + val.mode = mode; + } + if with_reply_awaition { + val.awaiting_reply = false; + } + } + if with_reply_awaition && val.awaiting_reply { + still_awating_replies = Some(true); + } + }); + still_awating_replies + } + + fn set_reply_awaition_flag(&mut self, target_id: ComponentId) { + self.iter_mut().for_each(|val| { + if val.id() == target_id { + val.awaiting_reply = true; + } + }); + } +} + +pub struct HeaplessModeStoreProvider(pub heapless::Vec); + +impl ModeStoreProvider for HeaplessModeStoreProvider { + fn add_component( + &mut self, + target_id: ComponentId, + initial_mode: ModeRaw, + ) -> Result<(), FullError> { + self.0.add_component(target_id, initial_mode) + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.0.has_component(target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.0.get(target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.0.get_mut(target_id) + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + with_reply_awaition: bool, + ) -> Option { + self.0 + .mode_reply_handler(sender_id, reported_mode_and_submode, with_reply_awaition) + } + + fn set_reply_awaition_flag(&mut self, target_id: ComponentId) { + self.0.set_reply_awaition_flag(target_id) + } } #[cfg(feature = "alloc")] pub mod alloc_mod { + use crate::subsystem::ModeRaw; + use super::*; #[derive(Debug)] @@ -258,13 +310,13 @@ pub mod alloc_mod { /// 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, + pub fallback_mode: Option, /// These are the rows of the a target table. pub entries: Vec, } impl TargetTablesMapValue { - pub fn new(name: &'static str, fallback_mode: Option) -> Self { + pub fn new(name: &'static str, fallback_mode: Option) -> Self { Self { name, fallback_mode, @@ -328,21 +380,33 @@ pub mod alloc_mod { } } + impl SequenceTableProvider for SequenceTablesMapValue { + fn sequence_at_index(&self, index: u8) -> Option<&[SequenceTableEntry]> { + self.entries + .get(index as usize) + .map(|table| table.entries.as_slice()) + } + + fn number_of_sequences(&self) -> usize { + self.entries.len() + } + } + #[derive(Debug, Default)] - pub struct TargetModeTables(pub HashMap); + pub struct TargetModeTables(pub HashMap); impl TargetModeTables { - pub fn name(&self, mode: Mode) -> Option<&'static str> { + pub fn name(&self, mode: ModeRaw) -> Option<&'static str> { self.0.get(&mode).map(|value| value.name) } } impl SequenceModeTables { - pub fn name(&self, mode: Mode) -> Option<&'static str> { + pub fn name(&self, mode: ModeRaw) -> Option<&'static str> { self.0.get(&mode).map(|value| value.name) } - pub fn name_of_sequence(&self, mode: Mode, seq_idx: usize) -> Option<&'static str> { + pub fn name_of_sequence(&self, mode: ModeRaw, seq_idx: usize) -> Option<&'static str> { self.0 .get(&mode) .map(|value| value.entries.get(seq_idx).map(|v| v.name))? @@ -352,9 +416,9 @@ pub mod alloc_mod { /// This is the core data structure used to store mode sequence tables. /// /// A mode sequence table specifies which commands have to be sent in which order - /// to reach a certain [Mode]. Therefore, it simply maps a [Mode] to a [SequenceTablesMapValue]. + /// to reach a certain [mode][ModeRaw]. Therefore, it simply maps a [mode][ModeRaw] to a [SequenceTablesMapValue]. #[derive(Debug, Default)] - pub struct SequenceModeTables(pub HashMap); + pub struct SequenceModeTables(pub HashMap); /// Mode store which tracks the [mode information][ModeStoreValue] inside a [Vec] #[derive(Debug, Default)] @@ -391,37 +455,42 @@ pub mod alloc_mod { } } - impl ModeStoreProvider for ModeStoreVec { - fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { - self.0.push(ModeStoreValue::new(target_id, mode)); + impl ModeStoreProvider for alloc::vec::Vec { + fn add_component( + &mut self, + target_id: ComponentId, + mode: ModeRaw, + ) -> Result<(), FullError> { + self.push(ModeStoreValue::new(target_id, mode)); + Ok(()) } fn has_component(&self, target_id: ComponentId) -> bool { - self.0.iter().any(|val| val.id == target_id) + self.iter().any(|val| val.id == target_id) } fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { - self.0.iter().find(|val| val.id == target_id) + self.iter().find(|val| val.id == target_id) } fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { - self.0.iter_mut().find(|val| val.id == target_id) + self.iter_mut().find(|val| val.id == target_id) } fn mode_reply_handler( &mut self, sender_id: ComponentId, - reported_mode_and_submode: Option, + reported_mode_and_submode: Option, handle_reply_awaition: bool, ) -> Option { let mut still_awating_replies = None; if handle_reply_awaition { still_awating_replies = Some(false); } - self.0.iter_mut().for_each(|val| { + self.iter_mut().for_each(|val| { if val.id() == sender_id { - if let Some(mode_and_submode) = reported_mode_and_submode { - val.mode_and_submode = mode_and_submode; + if let Some(mode) = reported_mode_and_submode { + val.mode = mode; } if handle_reply_awaition { val.awaiting_reply = false; @@ -433,12 +502,60 @@ pub mod alloc_mod { }); still_awating_replies } + + fn set_reply_awaition_flag(&mut self, target_id: ComponentId) { + self.iter_mut().for_each(|val| { + if val.id() == target_id { + val.awaiting_reply = true; + } + }); + } + } + impl ModeStoreProvider for ModeStoreVec { + fn add_component( + &mut self, + target_id: ComponentId, + mode: ModeRaw, + ) -> Result<(), FullError> { + self.0.add_component(target_id, mode) + } + + fn has_component(&self, target_id: ComponentId) -> bool { + self.0.has_component(target_id) + } + + fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> { + self.0.get(target_id) + } + + fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> { + self.0.get_mut(target_id) + } + + fn mode_reply_handler( + &mut self, + sender_id: ComponentId, + reported_mode_and_submode: Option, + handle_reply_awaition: bool, + ) -> Option { + self.0 + .mode_reply_handler(sender_id, reported_mode_and_submode, handle_reply_awaition) + } + + fn set_reply_awaition_flag(&mut self, target_id: ComponentId) { + self.0.set_reply_awaition_flag(target_id) + } } impl ModeStoreProvider for ModeStoreMap { - fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { + fn add_component( + &mut self, + target_id: ComponentId, + mode: ModeRaw, + ) -> Result<(), FullError> { self.0 .insert(target_id, ModeStoreValue::new(target_id, mode)); + Ok(()) } fn has_component(&self, target_id: ComponentId) -> bool { @@ -456,7 +573,7 @@ pub mod alloc_mod { fn mode_reply_handler( &mut self, sender_id: ComponentId, - reported_mode_and_submode: Option, + reported_mode_and_submode: Option, handle_reply_awaition: bool, ) -> Option { let mut still_awating_replies = None; @@ -465,8 +582,8 @@ pub mod alloc_mod { } for val in self.0.values_mut() { if val.id() == sender_id { - if let Some(mode_and_submode) = reported_mode_and_submode { - val.mode_and_submode = mode_and_submode; + if let Some(mode) = reported_mode_and_submode { + val.mode = mode; } if handle_reply_awaition { val.awaiting_reply = false; @@ -478,6 +595,14 @@ pub mod alloc_mod { } still_awating_replies } + + fn set_reply_awaition_flag(&mut self, target_id: ComponentId) { + self.0.iter_mut().for_each(|(_, val)| { + if val.id() == target_id { + val.awaiting_reply = true; + } + }); + } } } @@ -486,14 +611,11 @@ mod tests { use super::*; fn generic_test(mode_store: &mut impl ModeStoreProvider) { - mode_store.add_component(1, ModeAndSubmode::new(0, 0)); - mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.add_component(1, 0).unwrap(); + mode_store.add_component(2, 1).unwrap(); assert!(mode_store.has_component(1)); assert!(mode_store.has_component(2)); - assert_eq!( - mode_store.get(1).unwrap().mode_and_submode(), - ModeAndSubmode::new(0, 0) - ); + assert_eq!(mode_store.get(1).unwrap().mode(), 0); assert!(!mode_store.get(1).unwrap().awaiting_reply); assert!(!mode_store.get(2).unwrap().awaiting_reply); assert_eq!(mode_store.get(1).unwrap().id, 1); @@ -503,66 +625,47 @@ mod tests { } fn generic_reply_handling_with_reply_awaition(mode_store: &mut impl ModeStoreProvider) { - mode_store.add_component(1, ModeAndSubmode::new(0, 0)); - mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.add_component(1, 0).unwrap(); + mode_store.add_component(2, 1).unwrap(); mode_store.get_mut(1).unwrap().awaiting_reply = true; mode_store.get_mut(2).unwrap().awaiting_reply = true; let mut reply_awation_pending = - mode_store.mode_reply_handler_with_reply_awaition(1, Some(ModeAndSubmode::new(2, 0))); + mode_store.mode_reply_handler_with_reply_awaition(1, Some(2)); assert!(reply_awation_pending); reply_awation_pending = mode_store.mode_reply_handler_with_reply_awaition(2, None); assert!(!reply_awation_pending); assert!(!mode_store.get(1).unwrap().awaiting_reply); assert!(!mode_store.get(2).unwrap().awaiting_reply); - assert_eq!( - mode_store.get(1).unwrap().mode_and_submode(), - ModeAndSubmode::new(2, 0) - ); - assert_eq!( - mode_store.get(2).unwrap().mode_and_submode(), - ModeAndSubmode::new(1, 0) - ); + assert_eq!(mode_store.get(1).unwrap().mode(), 2); + assert_eq!(mode_store.get(2).unwrap().mode(), 1); } fn generic_reply_handling_test_no_reply_awaition(mode_store: &mut impl ModeStoreProvider) { - mode_store.add_component(1, ModeAndSubmode::new(0, 0)); - mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.add_component(1, 0).unwrap(); + mode_store.add_component(2, 1).unwrap(); mode_store.get_mut(1).unwrap().awaiting_reply = true; mode_store.get_mut(2).unwrap().awaiting_reply = true; - mode_store.mode_reply_handler_without_reply_awaition(1, Some(ModeAndSubmode::new(2, 0))); + mode_store.mode_reply_handler_without_reply_awaition(1, Some(2)); mode_store.mode_reply_handler_without_reply_awaition(2, None); assert!(mode_store.get(1).unwrap().awaiting_reply); assert!(mode_store.get(2).unwrap().awaiting_reply); - assert_eq!( - mode_store.get(1).unwrap().mode_and_submode(), - ModeAndSubmode::new(2, 0) - ); - assert_eq!( - mode_store.get(2).unwrap().mode_and_submode(), - ModeAndSubmode::new(1, 0) - ); + assert_eq!(mode_store.get(1).unwrap().mode(), 2); + assert_eq!(mode_store.get(2).unwrap().mode(), 1); } fn generic_reply_handling_with_reply_awaition_2(mode_store: &mut impl ModeStoreProvider) { - mode_store.add_component(1, ModeAndSubmode::new(0, 0)); - mode_store.add_component(2, ModeAndSubmode::new(1, 0)); + mode_store.add_component(1, 0).unwrap(); + mode_store.add_component(2, 1).unwrap(); mode_store.get_mut(1).unwrap().awaiting_reply = true; mode_store.get_mut(2).unwrap().awaiting_reply = true; - let mut reply_awation_pending = - mode_store.mode_reply_handler(1, Some(ModeAndSubmode::new(2, 0)), true); + let mut reply_awation_pending = mode_store.mode_reply_handler(1, Some(2), true); assert!(reply_awation_pending.unwrap()); reply_awation_pending = mode_store.mode_reply_handler(2, None, true); assert!(!reply_awation_pending.unwrap()); assert!(!mode_store.get(1).unwrap().awaiting_reply); assert!(!mode_store.get(2).unwrap().awaiting_reply); - assert_eq!( - mode_store.get(1).unwrap().mode_and_submode(), - ModeAndSubmode::new(2, 0) - ); - assert_eq!( - mode_store.get(2).unwrap().mode_and_submode(), - ModeAndSubmode::new(1, 0) - ); + assert_eq!(mode_store.get(1).unwrap().mode(), 2); + assert_eq!(mode_store.get(2).unwrap().mode(), 1); } #[test] diff --git a/satrs/src/pus/mode.rs b/satrs/src/pus/mode.rs index 17f9923..659674c 100644 --- a/satrs/src/pus/mode.rs +++ b/satrs/src/pus/mode.rs @@ -32,126 +32,4 @@ pub mod alloc_mod {} pub mod std_mod {} #[cfg(test)] -mod tests { - - use std::sync::mpsc; - - use crate::{ - ComponentId, - mode::{ - ModeAndSubmode, ModeReply, ModeReplySender, ModeRequest, ModeRequestSender, - ModeRequestorAndHandlerMpsc, ModeRequestorOneChildMpsc, - }, - request::{GenericMessage, MessageMetadata}, - }; - - const TEST_COMPONENT_ID_0: ComponentId = 5; - const TEST_COMPONENT_ID_1: ComponentId = 6; - const TEST_COMPONENT_ID_2: ComponentId = 7; - - #[test] - fn test_simple_mode_requestor() { - let (reply_sender, reply_receiver) = mpsc::channel(); - let (request_sender, request_receiver) = mpsc::channel(); - let mut mode_requestor = - ModeRequestorOneChildMpsc::new(TEST_COMPONENT_ID_0, reply_receiver); - mode_requestor.add_message_target(TEST_COMPONENT_ID_1, request_sender); - - // Send a request and verify it arrives at the receiver. - let request_id = 2; - let sent_request = ModeRequest::ReadMode; - mode_requestor - .send_mode_request(request_id, TEST_COMPONENT_ID_1, sent_request) - .expect("send failed"); - let request = request_receiver.recv().expect("recv failed"); - assert_eq!(request.request_id(), 2); - assert_eq!(request.sender_id(), TEST_COMPONENT_ID_0); - assert_eq!(request.message, sent_request); - - // Send a reply and verify it arrives at the requestor. - let mode_reply = ModeReply::ModeReply(ModeAndSubmode::new(1, 5)); - reply_sender - .send(GenericMessage::new( - MessageMetadata::new(request_id, TEST_COMPONENT_ID_1), - mode_reply, - )) - .expect("send failed"); - let reply = mode_requestor.try_recv_mode_reply().expect("recv failed"); - assert!(reply.is_some()); - let reply = reply.unwrap(); - assert_eq!(reply.sender_id(), TEST_COMPONENT_ID_1); - assert_eq!(reply.request_id(), 2); - assert_eq!(reply.message, mode_reply); - } - - #[test] - fn test_mode_requestor_and_request_handler_request_sending() { - let (_reply_sender_to_connector, reply_receiver_of_connector) = mpsc::channel(); - let (_request_sender_to_connector, request_receiver_of_connector) = mpsc::channel(); - - 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 = ModeRequestorAndHandlerMpsc::new( - TEST_COMPONENT_ID_0, - request_receiver_of_connector, - reply_receiver_of_connector, - ); - assert_eq!( - ModeRequestSender::local_channel_id(&mode_connector), - TEST_COMPONENT_ID_0 - ); - assert_eq!( - ModeReplySender::local_channel_id(&mode_connector), - TEST_COMPONENT_ID_0 - ); - assert_eq!( - mode_connector.local_channel_id_generic(), - TEST_COMPONENT_ID_0 - ); - - mode_connector.add_request_target(TEST_COMPONENT_ID_1, request_sender_to_channel_1); - - // Send a request and verify it arrives at the receiver. - let request_id = 2; - let sent_request = ModeRequest::ReadMode; - mode_connector - .send_mode_request(request_id, TEST_COMPONENT_ID_1, sent_request) - .expect("send failed"); - - let request = request_receiver_channel_1.recv().expect("recv failed"); - assert_eq!(request.request_id(), 2); - assert_eq!(request.sender_id(), TEST_COMPONENT_ID_0); - assert_eq!(request.message, ModeRequest::ReadMode); - } - - #[test] - fn test_mode_requestor_and_request_handler_reply_sending() { - let (_reply_sender_to_connector, reply_receiver_of_connector) = mpsc::channel(); - 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 = ModeRequestorAndHandlerMpsc::new( - TEST_COMPONENT_ID_0, - request_receiver_of_connector, - reply_receiver_of_connector, - ); - mode_connector.add_reply_target(TEST_COMPONENT_ID_2, reply_sender_to_channel_2); - - // Send a reply and verify it arrives at the receiver. - let request_id = 2; - let sent_reply = ModeReply::ModeReply(ModeAndSubmode::new(3, 5)); - mode_connector - .send_mode_reply( - MessageMetadata::new(request_id, TEST_COMPONENT_ID_2), - sent_reply, - ) - .expect("send failed"); - let reply = reply_receiver_channel_2.recv().expect("recv failed"); - assert_eq!(reply.request_id(), 2); - assert_eq!(reply.sender_id(), TEST_COMPONENT_ID_0); - assert_eq!(reply.message, sent_reply); - } - - #[test] - fn test_mode_reply_handler() {} -} +mod tests {} diff --git a/satrs/src/request.rs b/satrs/src/request.rs index d8ee70b..c99c4e9 100644 --- a/satrs/src/request.rs +++ b/satrs/src/request.rs @@ -85,8 +85,9 @@ impl fmt::Display for UniqueApidTargetId { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, - "Target and APID ID with APID {:#03x} and target {}", - self.apid, self.unique_id + "Target and APID ID with APID {:#05x} and target {}", + self.apid.value(), + self.unique_id ) } } diff --git a/satrs/src/subsystem.rs b/satrs/src/subsystem.rs index 09836ed..aacb82d 100644 --- a/satrs/src/subsystem.rs +++ b/satrs/src/subsystem.rs @@ -1,15 +1,17 @@ +use arbitrary_int::{traits::Integer as _, u24}; + use crate::{ ComponentId, - health::{HealthState, HealthTableProvider}, - mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender, UNKNOWN_MODE_VAL}, mode_tree::{ - ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableMapTable, - SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError, TargetTablesMapValue, + ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, + SequenceTableProvider, SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError, + TargetTablesMapValue, }, - queue::GenericTargetedMessagingError, - request::{GenericMessage, RequestId}, + request::RequestId, }; +pub type ModeRaw = u32; + #[derive(Debug, PartialEq, Eq, Copy, Clone)] pub enum SequenceExecutionHelperState { /// The sequence execution is IDLE, no command is loaded or the sequence exection has @@ -34,76 +36,68 @@ pub enum ModeCommandingResult { #[derive(Debug, thiserror::Error)] #[error("mode {0} does not exist")] -pub struct ModeDoesNotExistError(Mode); +pub struct ModeDoesNotExistError(ModeRaw); #[derive(Debug, thiserror::Error)] pub enum StartSequenceError { #[error("mode {0} does not exist")] ModeDoesNotExist(#[from] ModeDoesNotExistError), - #[error("invalid request ID")] - InvalidRequestId(RequestId), +} + +#[derive(Debug, thiserror::Error)] +#[error("invalid sequence index")] +pub struct InvalidSequenceIndexError; + +#[derive(Debug, Copy, Clone)] +pub struct SequenceExecutionInfo { + target_mode: ModeRaw, + current_sequence_index: Option, + number_of_sequences: u8, } /// This sequence execution helper includes some boilerplate logic to -/// execute [SequenceModeTables]. +/// execute mode sequences. /// -/// It takes care of commanding the [ModeRequest]s specified in those tables and also includes the +/// It contains some boilerplate logic required for child mode commanding as specified in subsystem +/// sequence tables and also includes the /// states required to track the current progress of a sequence execution and take care of /// reply and success awaition. #[derive(Debug)] pub struct SequenceExecutionHelper { - target_mode: Option, state: SequenceExecutionHelperState, - request_id: Option, - current_sequence_index: Option, - last_sequence_index: Option, + info: Option, } impl Default for SequenceExecutionHelper { fn default() -> Self { Self { - target_mode: None, state: SequenceExecutionHelperState::Idle, - request_id: None, - current_sequence_index: None, - last_sequence_index: None, + info: None, } } } -pub trait IsChildCommandable { - fn is_commandable(&self, id: ComponentId) -> bool; -} - -impl IsChildCommandable for T -where - T: HealthTableProvider, -{ - fn is_commandable(&self, id: ComponentId) -> bool { - self.health(id) - .is_none_or(|h| h != HealthState::ExternalControl) - } -} - impl SequenceExecutionHelper { pub fn new() -> Self { Default::default() } /// Load a new mode sequence to be executed + #[cfg(feature = "alloc")] pub fn load( &mut self, - mode: Mode, - request_id: RequestId, + mode: ModeRaw, sequence_tables: &SequenceModeTables, ) -> Result<(), ModeDoesNotExistError> { if !sequence_tables.0.contains_key(&mode) { return Err(ModeDoesNotExistError(mode)); } - self.target_mode = Some(mode); - self.request_id = Some(request_id); self.state = SequenceExecutionHelperState::Busy; - self.current_sequence_index = None; + self.info = Some(SequenceExecutionInfo { + target_mode: mode, + current_sequence_index: None, + number_of_sequences: 0, + }); Ok(()) } @@ -118,7 +112,7 @@ impl SequenceExecutionHelper { /// /// * [ModeCommandingResult::AwaitingSuccessCheck] - The sequence is still awaiting a success. /// The user should check whether all children have reached the commanded target mode, for - /// example by checking [mode replies][ModeReply] received by the children components, and + /// example by checking mode replies received by the children components, and /// then calling [Self::confirm_sequence_done] to advance to the sequence or complete the /// sequence. /// * [ModeCommandingResult::Done] - The sequence is done. The user can load a new @@ -132,174 +126,169 @@ impl SequenceExecutionHelper { /// /// # Arguments /// - /// * `table` - This table contains the sequence tables to reach the mode previously loaded - /// with [Self::load] - /// * `sender` - The sender to send mode requests to the components + /// * `sequence_table_provider` - This table contains the sequence tables to reach the mode + /// previously loaded with [Self::load] /// * `children_mode_store` - The mode store vector to keep track of the mode states of /// children components - pub fn run( + /// * `mode_request_handler` - A function which is called to send out mode requests to the + /// children or cache the required requests so they can be sent after the function call. + pub fn run( &mut self, - table: &SequenceModeTables, - sender: &impl ModeRequestSender, - children_mode_store: &mut ModeStoreVec, - is_commandable: &impl IsChildCommandable, - ) -> Result { + sequence_table_provider: &impl SequenceTableProvider, + children_mode_store: &mut impl ModeStoreProvider, + mode_request_handler: F, + ) -> Result { + // TODO: Check whether sequence table length is larger than 255. Improbable, but let's + // solve this cleanly. if self.state == SequenceExecutionHelperState::Idle { return Ok(ModeCommandingResult::Done); } if self.state == SequenceExecutionHelperState::AwaitingSuccessCheck { return Ok(ModeCommandingResult::AwaitingSuccessCheck); } - if self.target_mode.is_none() { + if self.info.is_none() { return Ok(ModeCommandingResult::Done); } - match self.current_sequence_index { - Some(idx) => { + let mut current_info = self.info.unwrap(); + match current_info.current_sequence_index { + Some(index) => { // Execute the sequence. - let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); - self.execute_sequence_and_map_to_result( - seq_table_value, - idx, - sender, + Ok(self.execute_sequence_and_map_to_result( + index, + sequence_table_provider.sequence_at_index(index).unwrap(), + current_info.number_of_sequences == index + 1, children_mode_store, - is_commandable, - ) + mode_request_handler, + )) } None => { + let sequence_index = 0; + let sequence = sequence_table_provider + .sequence_at_index(sequence_index) + .unwrap(); + current_info.number_of_sequences = + sequence_table_provider.number_of_sequences() as u8; // Find the first sequence - let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap(); - self.last_sequence_index = Some(seq_table_value.entries.len() - 1); - if seq_table_value.entries.is_empty() { + if sequence.is_empty() { Ok(ModeCommandingResult::Done) } else { - self.current_sequence_index = Some(0); - self.execute_sequence_and_map_to_result( - seq_table_value, - 0, - sender, + current_info.current_sequence_index = Some(0); + // Update state. + self.info = Some(current_info); + Ok(self.execute_sequence_and_map_to_result( + sequence_index, + sequence, + current_info.number_of_sequences == 1, children_mode_store, - is_commandable, - ) + mode_request_handler, + )) } } } } /// Retrieve the currently loaded target mode - pub fn target_mode(&self) -> Option { - self.target_mode + pub fn target_mode(&self) -> Option { + Some(self.info?.target_mode) } /// Confirm that a sequence which is awaiting a success check is done pub fn confirm_sequence_done(&mut self) { - if let SequenceExecutionHelperState::AwaitingSuccessCheck = self.state { + if let SequenceExecutionHelperState::AwaitingSuccessCheck = self.state + && let Some(info) = &mut self.info + && let Some(current_sequence_index) = info.current_sequence_index + { self.state = SequenceExecutionHelperState::Busy; - if let (Some(last_sequence_index), Some(current_sequence_index)) = - (self.last_sequence_index, self.current_sequence_index) - { - if current_sequence_index == last_sequence_index { - self.state = SequenceExecutionHelperState::Idle; - } + if current_sequence_index + 1 == info.number_of_sequences { + self.state = SequenceExecutionHelperState::Idle; } - self.current_sequence_index = Some(self.current_sequence_index.unwrap() + 1); + info.current_sequence_index = Some(current_sequence_index + 1); } } /// Internal state of the execution helper. + #[inline] pub fn state(&self) -> SequenceExecutionHelperState { self.state } - pub fn request_id(&self) -> Option { - self.request_id - } - - pub fn set_request_id(&mut self, request_id: RequestId) { - self.request_id = Some(request_id); - } - + #[inline] pub fn awaiting_success_check(&self) -> bool { self.state == SequenceExecutionHelperState::AwaitingSuccessCheck } - pub fn current_sequence_index(&self) -> Option { - self.current_sequence_index + #[inline] + pub fn current_sequence_index(&self) -> Option { + self.info?.current_sequence_index } - /// Execute a sequence at the given sequence index for a given [SequenceTablesMapValue]. + /// Execute a sequence at the given sequence index for a given sequence. + /// + /// The sequence is identifier by a sequence index. It is represented by a list of + /// [SequenceTableEntry] values. /// /// This method calls [Self::execute_sequence] and maps the result to a [ModeCommandingResult]. /// It is also called by the [Self::run] method of this helper. pub fn execute_sequence_and_map_to_result( &mut self, - seq_table_value: &SequenceTablesMapValue, - sequence_idx: usize, - sender: &impl ModeRequestSender, - mode_store_vec: &mut ModeStoreVec, - is_commandable: &impl IsChildCommandable, - ) -> Result { - if self.state() == SequenceExecutionHelperState::Idle || self.request_id.is_none() { - return Ok(ModeCommandingResult::Done); + sequence_index: u8, + commands_for_sequence: &[SequenceTableEntry], + is_last_sequence: bool, + children_mode_store: &mut impl ModeStoreProvider, + mode_request_handler: impl FnMut(ModeSetRequest), + ) -> ModeCommandingResult { + if self.state() == SequenceExecutionHelperState::Idle { + return ModeCommandingResult::Done; } if Self::execute_sequence( - self.request_id.unwrap(), - &seq_table_value.entries[sequence_idx], - sender, - mode_store_vec, - is_commandable, - )? { + commands_for_sequence, + children_mode_store, + mode_request_handler, + ) { self.state = SequenceExecutionHelperState::AwaitingSuccessCheck; - Ok(ModeCommandingResult::AwaitingSuccessCheck) - } else if seq_table_value.entries.len() - 1 == sequence_idx { + ModeCommandingResult::AwaitingSuccessCheck + } else if is_last_sequence { self.state = SequenceExecutionHelperState::Idle; - Ok(ModeCommandingResult::Done) + ModeCommandingResult::Done } else { - self.current_sequence_index = Some(sequence_idx + 1); - Ok(ModeCommandingResult::StepDone) + if let Some(info) = &mut self.info { + info.current_sequence_index = Some(sequence_index + 1); + } + ModeCommandingResult::StepDone } } /// Generic stateless execution helper method. /// - /// The [RequestId] and the [SequenceTableMapTable] to be executed are passed explicitely + /// The [RequestId] and the [SequenceTableEntry] list to be executed are passed explicitely /// here. This method is called by [Self::execute_sequence_and_map_to_result]. /// - /// This method itereates through the entries of the given sequence table and sends out - /// [ModeRequest]s to set the modes of the children according to the table entries. + /// This method itereates through the entries of the given sequence table, creates + /// mode requests to set the modes of the children according to the table entries and passes. + /// them to the provided `mode_request_handler` closure. + /// /// It also sets the reply awaition field in the children mode store where a success /// check is required to true. /// /// It returns whether any commanding success check is required by any entry in the table. pub fn execute_sequence( - request_id: RequestId, - map_table: &SequenceTableMapTable, - sender: &impl ModeRequestSender, - children_mode_store: &mut ModeStoreVec, - commandable: &impl IsChildCommandable, - ) -> Result { + table_entries: &[SequenceTableEntry], + children_mode_store: &mut impl ModeStoreProvider, + mut mode_request_handler: impl FnMut(ModeSetRequest), + ) -> bool { let mut some_succes_check_required = false; - for entry in &map_table.entries { - if !commandable.is_commandable(entry.common.target_id) { - continue; - } - sender.send_mode_request( - request_id, - entry.common.target_id, - ModeRequest::SetMode { - mode_and_submode: entry.common.mode_submode, - forced: false, - }, - )?; + for entry in table_entries { + let mode_set_request = ModeSetRequest { + target_id: entry.common.target_id, + mode: entry.common.mode, + }; + mode_request_handler(mode_set_request); if entry.check_success { - children_mode_store.0.iter_mut().for_each(|val| { - if val.id() == entry.common.target_id { - val.awaiting_reply = true; - } - }); + children_mode_store.set_reply_awaition_flag(entry.common.target_id); some_succes_check_required = true; } } - Ok(some_succes_check_required) + some_succes_check_required } } @@ -331,46 +320,53 @@ impl From for SubsystemHelperResult { #[derive(Debug, thiserror::Error)] pub enum ModeTreeHelperError { - #[error("generic targeted messaging error: {0}")] - Message(#[from] GenericTargetedMessagingError), #[error("current mode {0} is not contained in target table")] - CurrentModeNotInTargetTable(Mode), + CurrentModeNotInTargetTable(ModeRaw), + #[error("No sequence table found for mode {0:?}")] + NoSequenceTableFound(Option), + #[error("invalid sequence index")] + InvalidSequenceIndex(#[from] InvalidSequenceIndexError), /// Mode command has failed, for example while executing a mode table. #[error("mode command failed")] ModeCommmandFailure { /// Table index of the sequence table entry which failed. - seq_table_index: Option, + seq_table_index: Option, }, /// Target mode keeping violation. #[error("target keeping violation")] TargetKeepingViolation { /// Table index of the sequence table entry which failed. - fallback_mode: Option, + fallback_mode: Option, }, } /// This is a helper object which can be used by a subsystem component to execute mode sequences /// and perform target keeping. /// +/// It currently only works on systems with allocation support and it will also allocate at +/// run-time. +/// /// This helper object tries to compose as much data and state information as possible which is /// required for this process. +#[derive(Debug)] pub struct SubsystemCommandingHelper { /// State of the helper. state: ModeTreeHelperState, /// Current mode of the owner subsystem. - current_mode: Mode, + current_mode: ModeRaw, /// This data structure is used to track all mode children. pub children_mode_store: ModeStoreVec, - /// This field is set when a mode sequence is executed. It is used to determine whether mode - /// replies are relevant for reply awaition logic. - active_internal_request_id: Option, + /// Sequence counter used to generate unique request IDs. + sequence_counter: u24, + // Active internal request ID, whic his built from the sequence counter and sequence index. + //active_internal_request_id: Option, /// The primary data structure to keep the target state information for subsystem - /// [modes][Mode]. it specifies the mode each child should have for a certain subsystem mode + /// [modes][ModeRaw]. it specifies the mode each child should have for a certain subsystem mode /// and is relevant for target keeping. pub target_tables: TargetModeTables, /// The primary data structure to keep the sequence commanding information for commanded - /// subsystem [modes][Mode]. It specifies the actual commands and the order they should be - /// sent in to reach a certain [mode][Mode]. + /// subsystem [modes][ModeRaw]. It specifies the actual commands and the order they should be + /// sent in to reach a certain [mode][ModeRaw]. pub sequence_tables: SequenceModeTables, /// The sequence execution helper is used to execute sequences in the [Self::sequence_tables]. pub seq_exec_helper: SequenceExecutionHelper, @@ -379,10 +375,10 @@ pub struct SubsystemCommandingHelper { impl Default for SubsystemCommandingHelper { fn default() -> Self { Self { - current_mode: UNKNOWN_MODE_VAL, + current_mode: u32::MAX, + sequence_counter: u24::ZERO, state: Default::default(), children_mode_store: Default::default(), - active_internal_request_id: None, target_tables: Default::default(), sequence_tables: Default::default(), seq_exec_helper: Default::default(), @@ -390,6 +386,23 @@ impl Default for SubsystemCommandingHelper { } } +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[non_exhaustive] +pub struct ModeSetRequest { + pub target_id: ComponentId, + pub mode: ModeRaw, +} + +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct ModeResponse { + pub request_id: RequestId, + pub sender_id: ComponentId, + pub reported_mode: ModeRaw, + pub success: bool, +} + impl SubsystemCommandingHelper { /// Create a new substem commanding helper with an intial [ModeTreeHelperState::Idle] state, /// an empty mode children store and empty target and sequence mode tables. @@ -399,10 +412,10 @@ impl SubsystemCommandingHelper { sequence_tables: SequenceModeTables, ) -> Self { Self { - current_mode: UNKNOWN_MODE_VAL, + current_mode: 0, state: ModeTreeHelperState::Idle, children_mode_store, - active_internal_request_id: None, + sequence_counter: u24::ZERO, target_tables, sequence_tables, seq_exec_helper: Default::default(), @@ -413,24 +426,10 @@ impl SubsystemCommandingHelper { self.state } - pub fn mode(&self) -> Mode { + pub fn mode(&self) -> ModeRaw { self.current_mode } - pub fn request_id(&self) -> Option { - self.active_internal_request_id.map(|v| v >> 8) - } - - /// This returns the internal request ID, which is the regular [Self::request_id] specified - /// by the user shifter 8 to the right and then increment with the current sequence commanding - /// step. The value can still be retrieved because it might be required for reply verification. - /// - /// The state machine specifies this request ID for all mode commands related to the - /// current step of sequence commanding. - pub fn internal_request_id(&self) -> Option { - self.active_internal_request_id - } - /// Retrieve the fallback mode for the current mode of the subsystem by trying to retrieve /// it from the target table. /// @@ -439,7 +438,7 @@ impl SubsystemCommandingHelper { /// The fallback mode can and should be commanded when a target keeping violation was detected /// or after self-commanding to the current mode has failed, which can happen after a failed /// mode table execution. - pub fn fallback_mode(&self) -> Result, ModeDoesNotExistError> { + pub fn fallback_mode(&self) -> Result, ModeDoesNotExistError> { self.target_tables .0 .get(&self.current_mode) @@ -448,14 +447,15 @@ impl SubsystemCommandingHelper { } /// Add a mode child to the internal [Self::children_mode_store]. - pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeAndSubmode) { - self.children_mode_store.add_component(child, mode); + pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeRaw) { + // Can not fail for regular vector. + self.children_mode_store.add_component(child, mode).unwrap(); } /// Add a target mode table and an associated sequence mode table. pub fn add_target_and_sequence_table( &mut self, - mode: Mode, + mode: ModeRaw, target_table_val: TargetTablesMapValue, sequence_table_val: SequenceTablesMapValue, ) { @@ -463,56 +463,38 @@ impl SubsystemCommandingHelper { self.sequence_tables.0.insert(mode, sequence_table_val); } - /// Starts a command sequence for a given [mode][Mode]. + /// Starts a command sequence for a given [mode][ModeRaw]. /// /// # Arguments /// /// - `mode` - The mode to command - /// - `request_id` - Request ID associated with the command sequence. The value of this value - /// should not be larger than the maximum possible value for 24 bits: (2 ^ 24) - 1 = 16777215 - /// because 8 bits are reserved for internal sequence index tracking. - pub fn start_command_sequence( - &mut self, - mode: Mode, - request_id: RequestId, - ) -> Result<(), StartSequenceError> { - if request_id > 2_u32.pow(24) - 1 { - return Err(StartSequenceError::InvalidRequestId(request_id)); - } - self.active_internal_request_id = Some(request_id << 8); - self.seq_exec_helper.load( - mode, - self.active_internal_request_id.unwrap(), - &self.sequence_tables, - )?; + pub fn start_command_sequence(&mut self, mode: ModeRaw) -> Result<(), StartSequenceError> { + self.sequence_counter = self.sequence_counter.wrapping_add(u24::new(1)); + self.seq_exec_helper.load(mode, &self.sequence_tables)?; self.state = ModeTreeHelperState::ModeCommanding; Ok(()) } - pub fn send_announce_mode_cmd_to_children( - &self, - request_id: RequestId, - req_sender: &impl ModeRequestSender, - recursive: bool, - ) -> Result<(), GenericTargetedMessagingError> { - let mut request = ModeRequest::AnnounceMode; - if recursive { - request = ModeRequest::AnnounceModeRecursive; + /// In mode commanding mode, returns the current sequence index. + pub fn current_sequence_index(&self) -> Option { + if self.state != ModeTreeHelperState::ModeCommanding { + return None; } - for child in &self.children_mode_store.0 { - req_sender.send_mode_request(request_id, child.id(), request)?; - } - Ok(()) + self.seq_exec_helper.current_sequence_index() } - pub fn state_machine( + /// State machine which can be used to drive the susbystem helper. + /// + /// During mode commanding, it is the responsibility of the user to discard mode responses + /// not related to the current transition. One way to do this is to assign unique IDs to all + /// requests sent to the device and only handle responses related to those IDs. + pub fn state_machine( &mut self, - opt_reply: Option>, - req_sender: &impl ModeRequestSender, - is_commandable: &impl IsChildCommandable, + opt_mode_reponse: Option, + mode_request_handler: F, ) -> Result { - if let Some(reply) = opt_reply { - if self.handle_mode_reply(&reply)? { + if let Some(reply) = opt_mode_reponse { + if self.handle_children_mode_changed(reply)? { if self.seq_exec_helper.state() == SequenceExecutionHelperState::Idle { self.transition_to_target_keeping(); return Ok(SubsystemHelperResult::ModeCommanding( @@ -534,11 +516,22 @@ impl SubsystemCommandingHelper { Ok(SubsystemHelperResult::TargetKeeping) } ModeTreeHelperState::ModeCommanding => { + if self.seq_exec_helper.target_mode().is_none() { + return Err(ModeTreeHelperError::NoSequenceTableFound(None)); + } + let seq_table = self + .sequence_tables + .0 + .get(&self.seq_exec_helper.target_mode().unwrap()); + if seq_table.is_none() { + return Err(ModeTreeHelperError::NoSequenceTableFound( + self.seq_exec_helper.target_mode(), + )); + } let result = self.seq_exec_helper.run( - &self.sequence_tables, - req_sender, + seq_table.unwrap(), &mut self.children_mode_store, - is_commandable, + mode_request_handler, )?; match result { ModeCommandingResult::Done => { @@ -550,7 +543,7 @@ impl SubsystemCommandingHelper { // Normally, this step is done after all replies were received, but if no // reply checking is required for a command sequence, the step would never // be performed, so this function needs to be called here as well. - self.update_internal_req_id(); + //self.update_internal_req_id(); } ModeCommandingResult::AwaitingSuccessCheck => (), } @@ -572,18 +565,16 @@ impl SubsystemCommandingHelper { if !entry.monitor_state { continue; } - let mut target_mode_violated = false; + let target_mode_violated = false; self.children_mode_store.0.iter().for_each(|val| { if val.id() == entry.common.target_id { - target_mode_violated = - if let Some(allowed_submode_mask) = entry.allowed_submode_mask() { - let fixed_bits = !allowed_submode_mask; - (val.mode_and_submode().mode() != entry.common.mode_submode.mode()) - && (val.mode_and_submode().submode() & fixed_bits - != entry.common.mode_submode.submode() & fixed_bits) - } else { - val.mode_and_submode() != entry.common.mode_submode - }; + if let Some(ignored_bits) = entry.ignored_bits { + let mode_for_check = val.mode & !ignored_bits; + let entry_mode_for_check = entry.common.mode & !ignored_bits; + mode_for_check != entry_mode_for_check + } else { + val.mode != entry.common.mode + }; } }); if target_mode_violated { @@ -596,104 +587,78 @@ impl SubsystemCommandingHelper { Ok(()) } - fn update_internal_req_id(&mut self) { - let new_internal_req_id = (self.request_id().unwrap() << 8) - | self.seq_exec_helper.current_sequence_index().unwrap() as u32; - self.seq_exec_helper.set_request_id(new_internal_req_id); - self.active_internal_request_id = Some(new_internal_req_id); - } - // Handles a mode reply message and returns whether the reply completes a step of sequence // commanding. - fn handle_mode_reply( + // + // During mode commanding, it is the responsibility of the user to discard mode responses + // not related to the current transition. One way to do this is to assigned unique IDs to all + // requests sent to the device and only handle responses related to those IDs. + fn handle_children_mode_changed( &mut self, - reply: &GenericMessage, + mode_response: ModeResponse, ) -> Result { - if !self.children_mode_store.has_component(reply.sender_id()) { + if !self + .children_mode_store + .has_component(mode_response.sender_id) + { return Ok(false); } - let mut generic_mode_reply_handler = - |sender_id, mode_and_submode: Option, success: bool| { - let mut partial_step_done = false; - // Tying the reply awaition to the request ID ensures that something like replies - // belonging to older requests do not interfere with the completion handling of - // the mode commanding. This is important for forced mode commands. - let mut handle_awaition = false; - if self.state == ModeTreeHelperState::ModeCommanding - && self.active_internal_request_id.is_some() - && reply.request_id() == self.active_internal_request_id.unwrap() - { - handle_awaition = true; - } - let still_awating_replies = self.children_mode_store.mode_reply_handler( - sender_id, - mode_and_submode, - handle_awaition, - ); - if self.state == ModeTreeHelperState::ModeCommanding - && handle_awaition - && !still_awating_replies.unwrap_or(false) - { - self.seq_exec_helper.confirm_sequence_done(); - self.update_internal_req_id(); - partial_step_done = true; - } - if !success && self.state == ModeTreeHelperState::ModeCommanding { - // The user has to decide how to proceed. - self.state = ModeTreeHelperState::Idle; - return Err(ModeTreeHelperError::ModeCommmandFailure { - seq_table_index: self.seq_exec_helper.current_sequence_index(), - }); - } - Ok(partial_step_done) - }; - match reply.message { - ModeReply::ModeInfo(mode_and_submode) => { - generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true) - } - ModeReply::ModeReply(mode_and_submode) => { - generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true) - } - ModeReply::CantReachMode(_) => { - generic_mode_reply_handler(reply.sender_id(), None, false) - } - ModeReply::WrongMode { reached, .. } => { - generic_mode_reply_handler(reply.sender_id(), Some(reached), true) - } + let mut partial_step_done = false; + let mut handle_awaition = false; + if self.state == ModeTreeHelperState::ModeCommanding + && self.seq_exec_helper.awaiting_success_check() + { + handle_awaition = true; } + let still_awating_replies = self.children_mode_store.mode_reply_handler( + mode_response.sender_id, + Some(mode_response.reported_mode), + handle_awaition, + ); + if self.state == ModeTreeHelperState::ModeCommanding + && handle_awaition + && !still_awating_replies.unwrap_or(false) + { + self.seq_exec_helper.confirm_sequence_done(); + partial_step_done = true; + } + if !mode_response.success && self.state == ModeTreeHelperState::ModeCommanding { + // The user has to decide how to proceed. + self.state = ModeTreeHelperState::Idle; + return Err(ModeTreeHelperError::ModeCommmandFailure { + seq_table_index: self.seq_exec_helper.current_sequence_index(), + }); + } + Ok(partial_step_done) } pub fn update_child_mode( &mut self, child: ComponentId, - mode: ModeAndSubmode, + mode: ModeRaw, ) -> Result<(), TargetNotInModeStoreError> { let val_mut = self .children_mode_store .get_mut(child) .ok_or(TargetNotInModeStoreError(child))?; - val_mut.mode_and_submode = mode; + val_mut.mode = mode; Ok(()) } } #[cfg(test)] mod tests { + use std::vec::Vec; + use super::*; use crate::{ ComponentId, - mode::{ - Mode, ModeAndSubmode, ModeReply, ModeRequest, UNKNOWN_MODE, - tests::{ModeReqSenderMock, ModeReqWrapper}, - }, mode_tree::{ ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables, }, - queue::GenericTargetedMessagingError, - request::{GenericMessage, MessageMetadata, RequestId}, - subsystem::{ModeCommandingResult, ModeTreeHelperState, SequenceExecutionHelperState}, + subsystem::{ModeCommandingResult, SequenceExecutionHelperState}, }; #[derive(Debug)] @@ -710,23 +675,10 @@ mod tests { Mode2 = 3, } - #[derive(Debug, Default)] - pub struct IsCommandableMock { - pub commandable_map: std::collections::HashMap, - } - - impl IsChildCommandable for IsCommandableMock { - fn is_commandable(&self, id: ComponentId) -> bool { - self.commandable_map.get(&id).copied().unwrap_or(true) - } - } - pub struct SequenceExecutorTestbench { - pub sender: ModeReqSenderMock, pub mode_store: ModeStoreVec, pub seq_tables: SequenceModeTables, pub execution_helper: SequenceExecutionHelper, - pub is_commandable_mock: IsCommandableMock, } impl SequenceExecutorTestbench { @@ -734,24 +686,29 @@ mod tests { let mode_store = create_default_mode_store(); let (seq_tables, _) = create_simple_sample_seq_tables(); Self { - sender: ModeReqSenderMock::default(), mode_store, seq_tables, execution_helper: SequenceExecutionHelper::new(), - is_commandable_mock: IsCommandableMock::default(), } } pub fn get_mode_table(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue { - self.seq_tables.0.get_mut(&(mode as Mode)).unwrap() + self.seq_tables.0.get_mut(&(mode as ModeRaw)).unwrap() } - pub fn run(&mut self) -> Result { + pub fn run( + &mut self, + command_list: &mut Vec, + ) -> Result { self.execution_helper.run( - &self.seq_tables, - &self.sender, + self.seq_tables + .0 + .get(&self.execution_helper.target_mode().unwrap()) + .unwrap(), &mut self.mode_store, - &self.is_commandable_mock, + |request| { + command_list.push(request); + }, ) } @@ -760,10 +717,12 @@ mod tests { assert_eq!( self.execution_helper .run( - &self.seq_tables, - &self.sender, + self.seq_tables + .0 + .get(&self.execution_helper.target_mode().unwrap()) + .unwrap(), &mut self.mode_store, - &self.is_commandable_mock + |_| { panic!("should not have been called") } ) .unwrap(), ModeCommandingResult::Done @@ -772,90 +731,69 @@ mod tests { self.execution_helper.state(), SequenceExecutionHelperState::Idle ); - assert!(self.sender.requests.borrow().is_empty()); } - fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) { + fn generic_checks_subsystem_md1_step0(&mut self, mode_set_requests: &[ModeSetRequest]) { assert_eq!( self.execution_helper.target_mode().unwrap(), - ExampleMode::Mode1 as Mode + ExampleMode::Mode1 as ModeRaw ); - assert_eq!(self.sender.requests.borrow().len(), 2); - let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(mode_set_requests.len(), 2); + let req_0 = mode_set_requests[0]; assert_eq!(req_0.target_id, ExampleTargetId::Target0 as ComponentId); - assert_eq!(req_0.request_id, expected_req_id); - assert_eq!( - req_0.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE, - forced: false - } - ); - let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req_0.mode, SUBSYSTEM_MD1_ST0_TGT0_MODE); + let req_1 = mode_set_requests[1]; assert_eq!(req_1.target_id, ExampleTargetId::Target1 as ComponentId); - assert_eq!( - req_1.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE, - forced: false - } - ); + assert_eq!(req_1.mode, SUBSYSTEM_MD1_ST0_TGT1_MODE); } - fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) { + fn generic_checks_subsystem_md1_step1(&mut self, mode_set_requests: &[ModeSetRequest]) { assert_eq!( self.execution_helper.target_mode().unwrap(), - ExampleMode::Mode1 as Mode + ExampleMode::Mode1 as ModeRaw ); - assert_eq!(self.sender.requests.borrow().len(), 1); - let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(mode_set_requests.len(), 1); + let req_0 = mode_set_requests[0]; assert_eq!(req_0.target_id, ExampleTargetId::Target2 as ComponentId); - assert_eq!(req_0.request_id, expected_req_id); - assert_eq!( - req_0.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE, - forced: false - } - ); + assert_eq!(req_0.mode, SUBSYSTEM_MD1_ST1_TGT2_MODE,); } - fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) { + fn generic_checks_subsystem_md0(&mut self, mode_set_requests: &[ModeSetRequest]) { assert_eq!( self.execution_helper.target_mode().unwrap(), - ExampleMode::Mode0 as Mode + ExampleMode::Mode0 as ModeRaw ); assert_eq!(self.execution_helper.current_sequence_index().unwrap(), 0); - assert_eq!(self.sender.requests.borrow().len(), 2); - let req_0 = self.sender.requests.get_mut().pop_front().unwrap(); + assert_eq!(mode_set_requests.len(), 2); + let req_0 = mode_set_requests[0]; assert_eq!(req_0.target_id, ExampleTargetId::Target0 as ComponentId); - assert_eq!(req_0.request_id, expected_req_id); - assert_eq!( - req_0.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE, - forced: false - } - ); - let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap(); + assert_eq!(req_0.mode, SUBSYSTEM_MD0_TGT0_MODE); + let req_1 = mode_set_requests[1]; assert_eq!(req_1.target_id, ExampleTargetId::Target1 as ComponentId); - assert_eq!( - req_1.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE, - forced: false - } - ); + assert_eq!(req_1.mode, SUBSYSTEM_MD0_TGT1_MODE,); } } fn create_default_mode_store() -> ModeStoreVec { let mut mode_store = ModeStoreVec::default(); - mode_store.add_component(ExampleTargetId::Target0 as ComponentId, UNKNOWN_MODE); - mode_store.add_component(ExampleTargetId::Target1 as ComponentId, UNKNOWN_MODE); - mode_store.add_component(ExampleTargetId::Target2 as ComponentId, UNKNOWN_MODE); + mode_store + .add_component(ExampleTargetId::Target0 as ComponentId, 0) + .unwrap(); + mode_store + .add_component(ExampleTargetId::Target1 as ComponentId, 0) + .unwrap(); + mode_store + .add_component(ExampleTargetId::Target2 as ComponentId, 0) + .unwrap(); mode_store } + const SUBSYSTEM_MD0_TGT0_MODE: ModeRaw = ExampleMode::Mode0 as u32; + const SUBSYSTEM_MD0_TGT1_MODE: ModeRaw = ExampleMode::Mode1 as u32; + + const SUBSYSTEM_MD1_ST0_TGT0_MODE: ModeRaw = ExampleMode::Mode2 as u32; + const SUBSYSTEM_MD1_ST0_TGT1_MODE: ModeRaw = ExampleMode::Mode0 as u32; + const SUBSYSTEM_MD1_ST1_TGT2_MODE: ModeRaw = ExampleMode::Mode1 as u32; + fn create_simple_sample_seq_tables() -> (SequenceModeTables, TargetModeTables) { let mut seq_tables = SequenceModeTables::default(); // Mode 0 - One step command @@ -908,135 +846,81 @@ mod tests { } pub struct SubsystemHelperTestbench { - pub sender: ModeReqSenderMock, + pub mode_request_queue: Vec, pub helper: SubsystemCommandingHelper, - pub is_commandable_mock: IsCommandableMock, } impl SubsystemHelperTestbench { pub fn new() -> Self { let (sequence_tables, target_tables) = create_simple_sample_seq_tables(); Self { - sender: ModeReqSenderMock::default(), helper: SubsystemCommandingHelper::new( create_default_mode_store(), target_tables, sequence_tables, ), - is_commandable_mock: IsCommandableMock::default(), + mode_request_queue: Vec::new(), } } pub fn start_command_sequence( &mut self, mode: ExampleMode, - request_id: RequestId, ) -> Result<(), StartSequenceError> { - self.helper.start_command_sequence(mode as Mode, request_id) - } - - pub fn send_announce_mode_cmd_to_children( - &mut self, - request_id: RequestId, - recursive: bool, - ) -> Result<(), GenericTargetedMessagingError> { - self.helper - .send_announce_mode_cmd_to_children(request_id, &self.sender, recursive) + self.helper.start_command_sequence(mode as ModeRaw) } + #[allow(dead_code)] pub fn get_sequence_tables(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue { self.helper .sequence_tables .0 - .get_mut(&(mode as Mode)) + .get_mut(&(mode as ModeRaw)) .unwrap() } pub fn state_machine( &mut self, - opt_reply: Option>, + opt_reply: Option, ) -> Result { - self.helper - .state_machine(opt_reply, &self.sender, &self.is_commandable_mock) + self.helper.state_machine(opt_reply, |val| { + self.mode_request_queue.push(val); + }) } - pub fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) { - assert_eq!(self.sender.requests.borrow().len(), 2); - let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req0.request_id, expected_req_id); + pub fn generic_checks_subsystem_md0(&mut self) { + assert_eq!(self.mode_request_queue.len(), 2); + let req0 = self.mode_request_queue[0]; assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId); - assert_eq!( - req0.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE, - forced: false - } - ); + assert_eq!(req0.mode, SUBSYSTEM_MD0_TGT0_MODE); - let req1 = self.sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req1.request_id, expected_req_id); + let req1 = self.mode_request_queue[1]; assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId); - assert_eq!( - req1.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE, - forced: false - } - ); + assert_eq!(req1.mode, SUBSYSTEM_MD0_TGT1_MODE); } - pub fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) { - assert_eq!(self.sender.requests.borrow().len(), 2); - let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req0.request_id, expected_req_id); + pub fn generic_checks_subsystem_md1_step0(&mut self) { + assert_eq!(self.mode_request_queue.len(), 2); + let req0 = self.mode_request_queue[0]; assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId); - assert_eq!( - req0.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE, - forced: false - } - ); + assert_eq!(req0.mode, SUBSYSTEM_MD1_ST0_TGT0_MODE); - let req1 = self.sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req1.request_id, expected_req_id); + let req1 = self.mode_request_queue[1]; assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId); - assert_eq!( - req1.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE, - forced: false - } - ); + assert_eq!(req1.mode, SUBSYSTEM_MD1_ST0_TGT1_MODE); + + self.mode_request_queue.clear(); } - pub fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) { - assert_eq!(self.sender.requests.borrow().len(), 1); - let req0 = self.sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req0.request_id, expected_req_id); + pub fn generic_checks_subsystem_md1_step1(&mut self) { + assert_eq!(self.mode_request_queue.len(), 1); + let req0 = self.mode_request_queue[0]; assert_eq!(req0.target_id, ExampleTargetId::Target2 as ComponentId); - assert_eq!( - req0.request, - ModeRequest::SetMode { - mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE, - forced: false - } - ); + assert_eq!(req0.mode, SUBSYSTEM_MD1_ST1_TGT2_MODE); + self.mode_request_queue.clear(); } } - const SUBSYSTEM_MD0_TGT0_MODE: ModeAndSubmode = - ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0); - const SUBSYSTEM_MD0_TGT1_MODE: ModeAndSubmode = - ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); - - const SUBSYSTEM_MD1_ST0_TGT0_MODE: ModeAndSubmode = - ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0); - const SUBSYSTEM_MD1_ST0_TGT1_MODE: ModeAndSubmode = - ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0); - const SUBSYSTEM_MD1_ST1_TGT2_MODE: ModeAndSubmode = - ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); - #[test] fn test_init_state() { let execution_helper = SequenceExecutionHelper::new(); @@ -1049,9 +933,8 @@ mod tests { #[test] fn test_sequence_execution_helper_no_success_check() { let mut tb = SequenceExecutorTestbench::new(); - let expected_req_id = 1; tb.execution_helper - .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .load(ExampleMode::Mode0 as u32, &tb.seq_tables) .unwrap(); assert_eq!( tb.execution_helper.state(), @@ -1060,18 +943,21 @@ mod tests { assert!(!tb.execution_helper.awaiting_success_check()); assert_eq!( tb.execution_helper.target_mode().unwrap(), - ExampleMode::Mode0 as Mode + ExampleMode::Mode0 as ModeRaw ); + let mut list = Vec::new(); assert_eq!( - tb.run().expect("sequence exeecution helper run failure"), + tb.run(&mut list) + .expect("sequence exeecution helper run failure"), ModeCommandingResult::Done ); + assert!(!tb.execution_helper.awaiting_success_check()); assert_eq!( tb.execution_helper.state(), SequenceExecutionHelperState::Idle ); assert!(!tb.execution_helper.awaiting_success_check()); - tb.generic_checks_subsystem_md0(expected_req_id); + tb.generic_checks_subsystem_md0(&list); tb.check_run_is_no_op(); } @@ -1081,9 +967,8 @@ mod tests { let mode0_table = tb.get_mode_table(ExampleMode::Mode0); mode0_table.entries[0].entries[0].check_success = true; mode0_table.entries[0].entries[1].check_success = true; - let expected_req_id = 1; tb.execution_helper - .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .load(ExampleMode::Mode0 as u32, &tb.seq_tables) .unwrap(); assert_eq!( @@ -1093,10 +978,12 @@ mod tests { assert!(!tb.execution_helper.awaiting_success_check()); assert_eq!( tb.execution_helper.target_mode().unwrap(), - ExampleMode::Mode0 as Mode + ExampleMode::Mode0 as ModeRaw ); + let mut list = Vec::new(); assert_eq!( - tb.run().expect("sequence exeecution helper run failure"), + tb.run(&mut list) + .expect("sequence exeecution helper run failure"), ModeCommandingResult::AwaitingSuccessCheck ); assert_eq!( @@ -1106,7 +993,7 @@ mod tests { // These are not cleared, even if the execution helper is already IDLE. This is okay for // now. assert!(tb.execution_helper.awaiting_success_check()); - tb.generic_checks_subsystem_md0(expected_req_id); + tb.generic_checks_subsystem_md0(&list); tb.execution_helper.confirm_sequence_done(); assert_eq!( tb.execution_helper.state(), @@ -1121,9 +1008,8 @@ mod tests { let mut tb = SequenceExecutorTestbench::new(); let mode0_table = tb.get_mode_table(ExampleMode::Mode0); mode0_table.entries[0].entries[0].check_success = true; - let expected_req_id = 1; tb.execution_helper - .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables) + .load(ExampleMode::Mode0 as u32, &tb.seq_tables) .unwrap(); assert_eq!( @@ -1131,8 +1017,10 @@ mod tests { SequenceExecutionHelperState::Busy ); assert!(!tb.execution_helper.awaiting_success_check()); + let mut list = Vec::new(); assert_eq!( - tb.run().expect("sequence execution helper run failure"), + tb.run(&mut list) + .expect("sequence execution helper run failure"), ModeCommandingResult::AwaitingSuccessCheck ); assert_eq!( @@ -1142,7 +1030,7 @@ mod tests { // These are not cleared, even if the execution helper is already IDLE. This is okay for // now. assert!(tb.execution_helper.awaiting_success_check()); - tb.generic_checks_subsystem_md0(expected_req_id); + tb.generic_checks_subsystem_md0(&list); tb.execution_helper.confirm_sequence_done(); assert_eq!( tb.execution_helper.state(), @@ -1154,9 +1042,8 @@ mod tests { #[test] fn test_sequence_execution_helper_multi_step_no_success_check() { let mut tb = SequenceExecutorTestbench::new(); - let expected_req_id = 1; tb.execution_helper - .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables) + .load(ExampleMode::Mode1 as u32, &tb.seq_tables) .unwrap(); assert_eq!( tb.execution_helper.state(), @@ -1165,10 +1052,12 @@ mod tests { assert!(!tb.execution_helper.awaiting_success_check()); assert_eq!( tb.execution_helper.target_mode().unwrap(), - ExampleMode::Mode1 as Mode + ExampleMode::Mode1 as ModeRaw ); + let mut list = Vec::new(); assert_eq!( - tb.run().expect("sequence execution helper run failure"), + tb.run(&mut list) + .expect("sequence execution helper run failure"), ModeCommandingResult::StepDone ); assert_eq!( @@ -1176,14 +1065,16 @@ mod tests { SequenceExecutionHelperState::Busy ); assert!(!tb.execution_helper.awaiting_success_check()); - tb.generic_checks_subsystem_md1_step0(expected_req_id); + tb.generic_checks_subsystem_md1_step0(&list); assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1); + list.clear(); assert_eq!( - tb.run().expect("sequence execution helper run failure"), + tb.run(&mut list) + .expect("sequence execution helper run failure"), ModeCommandingResult::Done ); - tb.generic_checks_subsystem_md1_step1(expected_req_id); + tb.generic_checks_subsystem_md1_step1(&list); assert_eq!( tb.execution_helper.state(), SequenceExecutionHelperState::Idle @@ -1194,9 +1085,8 @@ mod tests { #[test] fn test_sequence_execution_helper_multi_step_full_success_check() { let mut tb = SequenceExecutorTestbench::new(); - let expected_req_id = 1; tb.execution_helper - .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables) + .load(ExampleMode::Mode1 as u32, &tb.seq_tables) .unwrap(); let mode1_table = tb.get_mode_table(ExampleMode::Mode1); mode1_table.entries[0].entries[0].check_success = true; @@ -1210,10 +1100,12 @@ mod tests { assert!(!tb.execution_helper.awaiting_success_check()); assert_eq!( tb.execution_helper.target_mode().unwrap(), - ExampleMode::Mode1 as Mode + ExampleMode::Mode1 as ModeRaw ); + let mut list = Vec::new(); assert_eq!( - tb.run().expect("sequence execution helper run failure"), + tb.run(&mut list) + .expect("sequence execution helper run failure"), ModeCommandingResult::AwaitingSuccessCheck ); assert_eq!( @@ -1221,12 +1113,14 @@ mod tests { SequenceExecutionHelperState::AwaitingSuccessCheck ); assert!(tb.execution_helper.awaiting_success_check()); - tb.generic_checks_subsystem_md1_step0(expected_req_id); + tb.generic_checks_subsystem_md1_step0(&list); assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 0); tb.execution_helper.confirm_sequence_done(); + list.clear(); assert_eq!( - tb.run().expect("sequence execution helper run failure"), + tb.run(&mut list) + .expect("sequence execution helper run failure"), ModeCommandingResult::AwaitingSuccessCheck ); assert_eq!( @@ -1235,117 +1129,62 @@ mod tests { ); assert!(tb.execution_helper.awaiting_success_check()); assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1); - tb.generic_checks_subsystem_md1_step1(expected_req_id); + tb.generic_checks_subsystem_md1_step1(&list); tb.execution_helper.confirm_sequence_done(); tb.check_run_is_no_op(); } - // TODO: Test subsystem commanding helper #[test] fn test_subsystem_helper_basic_state() { let tb = SubsystemHelperTestbench::new(); assert_eq!(tb.helper.state(), ModeTreeHelperState::Idle); - assert!(tb.helper.active_internal_request_id.is_none()); - assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); - assert!(tb.helper.request_id().is_none()); - } - - #[test] - fn test_subsystem_helper_announce_recursive() { - let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; - tb.send_announce_mode_cmd_to_children(expected_req_id, true) - .unwrap(); - assert_eq!(tb.sender.requests.borrow().len(), 3); - let check_req = |req: ModeReqWrapper, target_id: ComponentId| { - assert_eq!(req.target_id, target_id); - assert_eq!(req.request_id, expected_req_id); - assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); - }; - let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap(); - check_req(req0, ExampleTargetId::Target0 as ComponentId); - let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap(); - check_req(req1, ExampleTargetId::Target1 as ComponentId); - let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap(); - check_req(req2, ExampleTargetId::Target2 as ComponentId); - } - - #[test] - fn test_subsystem_helper_announce() { - let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; - tb.send_announce_mode_cmd_to_children(expected_req_id, false) - .unwrap(); - assert_eq!(tb.sender.requests.borrow().len(), 3); - let check_req = |req: ModeReqWrapper, target_id: ComponentId| { - assert_eq!(req.target_id, target_id); - assert_eq!(req.request_id, expected_req_id); - assert_eq!(req.request, ModeRequest::AnnounceMode); - }; - let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap(); - check_req(req0, ExampleTargetId::Target0 as ComponentId); - let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap(); - check_req(req1, ExampleTargetId::Target1 as ComponentId); - let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap(); - check_req(req2, ExampleTargetId::Target2 as ComponentId); + assert_eq!(tb.helper.mode(), 0); } #[test] fn test_subsystem_helper_cmd_mode0_no_success_checks() { let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; - tb.start_command_sequence(ExampleMode::Mode0, expected_req_id) - .unwrap(); - assert_eq!(tb.helper.request_id().unwrap(), 1); + tb.start_command_sequence(ExampleMode::Mode0).unwrap(); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.sender.requests.borrow().len(), 0); + assert_eq!(tb.mode_request_queue.len(), 0); assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); - tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap()); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as ModeRaw); + tb.generic_checks_subsystem_md0(); // FSM call should be a no-op. assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::TargetKeeping ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as ModeRaw); } #[test] fn test_subsystem_helper_cmd_mode1_no_success_checks() { let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; - tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) - .unwrap(); + tb.start_command_sequence(ExampleMode::Mode1).unwrap(); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.sender.requests.borrow().len(), 0); - // Need to cache this before it is incremented, because it is incremented - // immediately in the state machine (no reply checking) - let expected_req_id = tb.helper.internal_request_id().unwrap(); + assert!(tb.mode_request_queue.is_empty()); assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone) ); // Assert that this was already incremented because no reply checking is necessary. - assert_eq!( - tb.helper.internal_request_id().unwrap(), - expected_req_id + 1 - ); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); - tb.generic_checks_subsystem_md1_step0(expected_req_id); + assert_eq!(tb.helper.mode(), 0); + tb.generic_checks_subsystem_md1_step0(); // Second commanding step. assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); - tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as ModeRaw); + tb.generic_checks_subsystem_md1_step1(); // FSM call should be a no-op. assert_eq!( @@ -1353,41 +1192,37 @@ mod tests { SubsystemHelperResult::TargetKeeping ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as ModeRaw); } #[test] fn test_subsystem_helper_cmd_mode0_with_success_checks() { let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; let seq_tables = tb.get_sequence_tables(ExampleMode::Mode0); seq_tables.entries[0].entries[0].check_success = true; seq_tables.entries[0].entries[1].check_success = true; - tb.start_command_sequence(ExampleMode::Mode0, expected_req_id) - .unwrap(); + tb.start_command_sequence(ExampleMode::Mode0).unwrap(); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.sender.requests.borrow().len(), 0); + assert!(tb.mode_request_queue.is_empty()); assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) ); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); - tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap()); - let mode_reply_ok_0 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target0 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), - ); - let mode_reply_ok_1 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target1 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), - ); + assert_eq!(tb.helper.mode(), 0); + tb.generic_checks_subsystem_md0(); + let mode_reply_ok_0 = ModeResponse { + request_id: 0, + sender_id: ExampleTargetId::Target0 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT0_MODE, + success: true, + }; + let mode_reply_ok_1 = ModeResponse { + request_id: 1, + sender_id: ExampleTargetId::Target1 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT1_MODE, + success: true, + }; // One success reply still expected. assert_eq!( tb.state_machine(Some(mode_reply_ok_0)).unwrap(), @@ -1404,42 +1239,38 @@ mod tests { SubsystemHelperResult::TargetKeeping ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode); + assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as ModeRaw); } #[test] fn test_subsystem_helper_cmd_mode1_with_success_checks() { let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); seq_tables.entries[0].entries[0].check_success = true; seq_tables.entries[0].entries[1].check_success = true; seq_tables.entries[1].entries[0].check_success = true; - tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) - .unwrap(); + tb.start_command_sequence(ExampleMode::Mode1).unwrap(); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.sender.requests.borrow().len(), 0); + assert!(tb.mode_request_queue.is_empty()); assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) ); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); - tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); - let mode_reply_ok_0 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target0 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), - ); - let mode_reply_ok_1 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target1 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), - ); + assert_eq!(tb.helper.mode(), 0); + tb.generic_checks_subsystem_md1_step0(); + let mode_reply_ok_0 = ModeResponse { + request_id: 0, + sender_id: ExampleTargetId::Target0 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT0_MODE, + success: true, + }; + let mode_reply_ok_1 = ModeResponse { + request_id: 1, + sender_id: ExampleTargetId::Target1 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT1_MODE, + success: true, + }; // One success reply still expected. assert_eq!( tb.state_machine(Some(mode_reply_ok_0)).unwrap(), @@ -1454,13 +1285,12 @@ mod tests { tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) ); - let mode_reply_ok = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target2 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), - ); + let mode_reply_ok = ModeResponse { + request_id: 2, + sender_id: ExampleTargetId::Target2 as ComponentId, + reported_mode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + success: true, + }; assert_eq!( tb.state_machine(Some(mode_reply_ok)).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) @@ -1472,42 +1302,38 @@ mod tests { SubsystemHelperResult::TargetKeeping ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as ModeRaw); } #[test] fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_0() { let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); seq_tables.entries[0].entries[0].check_success = true; seq_tables.entries[0].entries[1].check_success = false; seq_tables.entries[1].entries[0].check_success = false; - tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) - .unwrap(); + tb.start_command_sequence(ExampleMode::Mode1).unwrap(); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.sender.requests.borrow().len(), 0); + assert!(tb.mode_request_queue.is_empty()); assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) ); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); - tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); - let mode_reply_ok_0 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target0 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), - ); - let mode_reply_ok_1 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target1 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), - ); + assert_eq!(tb.helper.mode(), 0); + tb.generic_checks_subsystem_md1_step0(); + let mode_reply_ok_0 = ModeResponse { + request_id: 0, + sender_id: ExampleTargetId::Target0 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT0_MODE, + success: true, + }; + let mode_reply_ok_1 = ModeResponse { + request_id: 1, + sender_id: ExampleTargetId::Target1 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT1_MODE, + success: true, + }; // One success reply still expected. assert_eq!( tb.state_machine(Some(mode_reply_ok_1)).unwrap(), @@ -1519,16 +1345,18 @@ mod tests { ); // Inserting the reply makes no difference: This call completes the sequence commanding. - let mode_reply_ok = GenericMessage::new( - MessageMetadata::new(expected_req_id, ExampleTargetId::Target2 as ComponentId), - ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), - ); + let mode_reply_ok = ModeResponse { + request_id: 2, + sender_id: ExampleTargetId::Target2 as ComponentId, + reported_mode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + success: true, + }; assert_eq!( tb.state_machine(Some(mode_reply_ok)).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) ); // The internal request ID is still cached. - tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + tb.generic_checks_subsystem_md1_step1(); // FSM call should be a no-op. assert_eq!( @@ -1536,42 +1364,38 @@ mod tests { SubsystemHelperResult::TargetKeeping ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as ModeRaw); } #[test] fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_1() { let mut tb = SubsystemHelperTestbench::new(); - let expected_req_id = 1; let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1); seq_tables.entries[0].entries[0].check_success = true; seq_tables.entries[0].entries[1].check_success = false; seq_tables.entries[1].entries[0].check_success = false; - tb.start_command_sequence(ExampleMode::Mode1, expected_req_id) - .unwrap(); + tb.start_command_sequence(ExampleMode::Mode1).unwrap(); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.sender.requests.borrow().len(), 0); + assert!(tb.mode_request_queue.is_empty()); assert_eq!( tb.state_machine(None).unwrap(), SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck) ); assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding); - assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL); - tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap()); - let mode_reply_ok_0 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target0 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE), - ); - let mode_reply_ok_1 = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target1 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE), - ); + assert_eq!(tb.helper.mode(), 0); + tb.generic_checks_subsystem_md1_step0(); + let mode_reply_ok_0 = ModeResponse { + request_id: 0, + sender_id: ExampleTargetId::Target0 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT0_MODE, + success: true, + }; + let mode_reply_ok_1 = ModeResponse { + request_id: 0, + sender_id: ExampleTargetId::Target1 as ComponentId, + reported_mode: SUBSYSTEM_MD0_TGT1_MODE, + success: true, + }; // This completes the step, so the next FSM call will perform the next step // in sequence commanding. assert_eq!( @@ -1585,19 +1409,18 @@ mod tests { // Inserting the reply makes no difference: Sequence command is done and target keeping // is performed. - let mode_reply_ok = GenericMessage::new( - MessageMetadata::new( - tb.helper.internal_request_id().unwrap(), - ExampleTargetId::Target2 as ComponentId, - ), - ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE), - ); + let mode_reply_ok = ModeResponse { + request_id: 0, + sender_id: ExampleTargetId::Target2 as ComponentId, + reported_mode: SUBSYSTEM_MD1_ST1_TGT2_MODE, + success: true, + }; assert_eq!( tb.state_machine(Some(mode_reply_ok)).unwrap(), SubsystemHelperResult::TargetKeeping ); // The internal request ID is still cached. - tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap()); + tb.generic_checks_subsystem_md1_step1(); // FSM call should be a no-op. assert_eq!( @@ -1605,6 +1428,6 @@ mod tests { SubsystemHelperResult::TargetKeeping ); assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping); - assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode); + assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as ModeRaw); } }