diff --git a/satrs-example/Cargo.toml b/satrs-example/Cargo.toml index 684d432..2c285ec 100644 --- a/satrs-example/Cargo.toml +++ b/satrs-example/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "satrs-example" version = "0.1.1" -edition = "2021" +edition = "2024" authors = ["Robin Mueller "] default-run = "satrs-example" homepage = "https://egit.irs.uni-stuttgart.de/rust/sat-rs" @@ -18,12 +18,13 @@ csv = "1" num_enum = "0.7" thiserror = "2" lazy_static = "1" -strum = { version = "0.27", features = ["derive"] } +strum = { version = "0.28", features = ["derive"] } derive-new = "0.7" cfg-if = "1" arbitrary-int = "2" -bitbybit = "1.4" +bitbybit = "2" postcard = "1" +ctrlc = "3" serde = { version = "1", features = ["derive"] } serde_json = "1" diff --git a/satrs-example/client/Cargo.toml b/satrs-example/client/Cargo.toml index 56a97e7..692333d 100644 --- a/satrs-example/client/Cargo.toml +++ b/satrs-example/client/Cargo.toml @@ -12,7 +12,7 @@ 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 } -bitbybit = "1.4" +bitbybit = "2" arbitrary-int = "2" ctrlc = { version = "3.5" } postcard = { version = "1" } diff --git a/satrs-example/client/src/main.rs b/satrs-example/client/src/main.rs index 3dbfe51..f6af5fa 100644 --- a/satrs-example/client/src/main.rs +++ b/satrs-example/client/src/main.rs @@ -28,6 +28,7 @@ pub struct Cli { enum Commands { Mgm0(MgmArgs), Mgm1(MgmArgs), + MgmAssy(MgmAssemblyArgs), } impl Commands { @@ -36,6 +37,7 @@ impl Commands { match self { Commands::Mgm0(_mgm_args) => models::ComponentId::AcsMgm0, Commands::Mgm1(_mgm_args) => models::ComponentId::AcsMgm1, + Commands::MgmAssy(_mgm_assembly_args) => models::ComponentId::AcsMgmAssembly, } } } @@ -47,11 +49,26 @@ struct MgmArgs { #[arg(long)] request_hk: bool, #[arg(short, long)] - mode: Option, + mode: Option, +} + +#[derive(Debug, PartialEq, Eq, Clone, Copy, clap::Parser)] +struct MgmAssemblyArgs { + #[arg(short, long)] + ping: bool, + #[arg(short, long)] + mode: Option, } #[derive(Debug, PartialEq, Eq, Clone, Copy, clap::ValueEnum)] -pub enum ModeSelect { +pub enum DeviceModeSelect { + Off, + Normal, +} + +#[derive(Debug, PartialEq, Eq, Clone, Copy, clap::ValueEnum)] +pub enum AssemblyModeSelect { + NoModeKeeping, Off, Normal, } @@ -150,14 +167,62 @@ fn main() -> anyhow::Result<()> { } if let Some(mode) = args.mode { let dev_mode = match mode { - ModeSelect::Off => models::DeviceMode::Off, - ModeSelect::Normal => models::DeviceMode::Normal, + DeviceModeSelect::Off => models::DeviceMode::Off, + DeviceModeSelect::Normal => 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::request::Request::Mode(dev_mode), + models::mgm::request::Request::Mode( + models::mgm::request::ModeRequest::SetMode(dev_mode), + ), + ); + let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); + log::info!( + "sending {:?} HK request with TC ID {:#010x}", + target_id, + sent_tc_id.raw() + ); + let request_packet = request.to_vec(); + client.send_to(&request_packet, addr).unwrap(); + } + } + Commands::MgmAssy(mgm_assembly_args) => { + if mgm_assembly_args.ping { + 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, + ); + let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); + log::info!( + "sending {:?} ping request with TC ID {:#010x}", + target_id, + sent_tc_id.raw() + ); + let request_packet = request.to_vec(); + client.send_to(&request_packet, addr).unwrap(); + } + if let Some(mode) = mgm_assembly_args.mode { + let assembly_mode = match mode { + AssemblyModeSelect::NoModeKeeping => { + models::mgm_assembly::AssemblyMode::NoModeKeeping + } + AssemblyModeSelect::Off => { + models::mgm_assembly::AssemblyMode::Device(models::DeviceMode::Off) + } + AssemblyModeSelect::Normal => { + models::mgm_assembly::AssemblyMode::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), + ), ); let sent_tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&request.sp_header); log::info!( @@ -238,7 +303,14 @@ fn handle_raw_tm_packet(data: &[u8]) -> anyhow::Result<()> { log::info!("Received response from controller: {:?}", response.unwrap()); } models::ComponentId::AcsSubsystem => todo!(), - models::ComponentId::AcsMgmAssembly => todo!(), + models::ComponentId::AcsMgmAssembly => { + let response = + postcard::from_bytes::(remainder); + log::info!( + "Received response from MGM Assembly: {:?}", + response.unwrap() + ); + } models::ComponentId::AcsMgm0 => { let response = postcard::from_bytes::(remainder); diff --git a/satrs-example/minisim/Cargo.toml b/satrs-example/minisim/Cargo.toml index c54ef13..ec69ee2 100644 --- a/satrs-example/minisim/Cargo.toml +++ b/satrs-example/minisim/Cargo.toml @@ -11,7 +11,7 @@ serde_json = "1" log = "0.4" thiserror = "2" fern = "0.7" -strum = { version = "0.27", features = ["derive"] } +strum = { version = "0.28", features = ["derive"] } num_enum = "0.7" humantime = "2" tai-time = { version = "0.3", features = ["serde"] } diff --git a/satrs-example/models/Cargo.toml b/satrs-example/models/Cargo.toml index aacf377..e145bc4 100644 --- a/satrs-example/models/Cargo.toml +++ b/satrs-example/models/Cargo.toml @@ -8,8 +8,8 @@ serde = { version = "1", features = ["derive"] } spacepackets = { version = "0.17", git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git", default-features = false } satrs = { path = "../../satrs" } num_enum = { version = "0.7" } -strum = { version = "0.27", features = ["derive"] } +strum = { version = "0.28", features = ["derive"] } postcard = { version = "1" } thiserror = { version = "2" } -bitbybit = "1.4" +bitbybit = "2" arbitrary-int = "2" diff --git a/satrs-example/models/src/lib.rs b/satrs-example/models/src/lib.rs index 3206a3a..f7351c4 100644 --- a/satrs-example/models/src/lib.rs +++ b/satrs-example/models/src/lib.rs @@ -1,4 +1,6 @@ extern crate alloc; +use core::str::FromStr; + use spacepackets::{ CcsdsPacketIdAndPsc, time::cds::{CdsTime, MIN_CDS_FIELD_LEN}, @@ -7,6 +9,7 @@ use spacepackets::{ pub mod ccsds; pub mod control; pub mod mgm; +pub mod mgm_assembly; pub mod pcdu; #[derive( @@ -152,13 +155,32 @@ pub trait Message { fn message_type(&self) -> MessageType; } +/// Generic device mode which covers the requirements of most devices. +/// +/// 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)] pub enum DeviceMode { Off = 0, On = 1, + /// Normal operation mode where periodic polling might be done as well. Normal = 2, } +impl FromStr for DeviceMode { + type Err = (); + + fn from_str(s: &str) -> Result { + match s.to_lowercase().as_str() { + "off" => Ok(DeviceMode::Off), + "on" => Ok(DeviceMode::On), + "normal" => Ok(DeviceMode::Normal), + _ => Err(()), + } + } +} + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] #[non_exhaustive] pub enum HkRequestType { diff --git a/satrs-example/models/src/mgm.rs b/satrs-example/models/src/mgm.rs index 3bd88fb..0584542 100644 --- a/satrs-example/models/src/mgm.rs +++ b/satrs-example/models/src/mgm.rs @@ -1,5 +1,11 @@ pub mod request { - use crate::{HkRequestType, Message}; + use crate::{DeviceMode, HkRequestType, Message}; + + #[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)] + pub enum ModeRequest { + SetMode(DeviceMode), + ReadMode, + } #[derive(Debug, PartialEq, Eq, Clone, Copy, serde::Serialize, serde::Deserialize)] pub enum HkId { @@ -16,7 +22,7 @@ pub mod request { pub enum Request { Ping, Hk(HkRequest), - Mode(crate::DeviceMode), + Mode(ModeRequest), } impl Request { @@ -44,17 +50,26 @@ pub struct MgmData { } pub mod response { - use crate::{Message, mgm::MgmData}; + use crate::{DeviceMode, Message, mgm::MgmData}; #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] pub enum HkResponse { MgmData(MgmData), } + #[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy)] + pub enum ModeResponse { + /// New mode has been set. + Mode(DeviceMode), + /// Setting a mode timed out. + SetModeTimeout, + } + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] pub enum Response { Ok, Hk(HkResponse), + Mode(ModeResponse), } impl Response { @@ -62,6 +77,7 @@ pub mod response { match self { Response::Ok => crate::MessageType::Verification, Response::Hk(_hk_response) => crate::MessageType::Hk, + Response::Mode(_mode_failure) => crate::MessageType::Mode, } } } diff --git a/satrs-example/models/src/mgm_assembly.rs b/satrs-example/models/src/mgm_assembly.rs new file mode 100644 index 0000000..f0f7417 --- /dev/null +++ b/satrs-example/models/src/mgm_assembly.rs @@ -0,0 +1,109 @@ +use core::str::FromStr; + +use crate::DeviceMode; + +#[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)] +pub enum AssemblyMode { + /// 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), + /// Mode keeping disabled. + NoModeKeeping, +} + +impl FromStr for AssemblyMode { + 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), + _ => Err(()), + } + } +} + +pub mod request { + use crate::{HkRequestType, Message, mgm_assembly::AssemblyMode}; + + #[derive(Debug, PartialEq, Eq, Clone, Copy, serde::Serialize, serde::Deserialize)] + pub enum HkId { + Sensor, + } + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeRequest { + SetMode(AssemblyMode), + ReadMode, + } + + #[derive(Debug, PartialEq, Eq, Clone, Copy, serde::Serialize, serde::Deserialize)] + pub struct HkRequest { + pub id: HkId, + pub req_type: HkRequestType, + } + + #[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::{DeviceMode, Message}; + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug, PartialEq, Eq)] + pub enum ModeCommandFailure { + Timeout, + } + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeReport { + /// Mode of the assembly. + Mode(super::AssemblyMode), + /// Timeout failure setting the children modes. + SetModeTimeout([Option; 2]), + /// Children are in wrong mode after commanding. + WrongMode([Option; 2]), + /// An assembly tried modekeeping but can not keep its mode. + CanNotKeepMode([Option; 2]), + } + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug, PartialEq, Eq)] + pub enum Response { + Ok, + Mode(ModeReport), + } + + 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/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index a1e6cab..e65923f 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -1,25 +1,23 @@ -use models::ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}; use models::mgm::MgmData; +use models::mgm::request::ModeRequest; +use models::mgm::response::ModeResponse; use models::pcdu::SwitchId; -use models::{mgm, ComponentId, DeviceMode, HkRequestType}; +use models::{ComponentId, DeviceMode, HkRequestType, mgm}; use satrs::spacepackets::CcsdsPacketIdAndPsc; -use satrs_example::{HkHelperSingleSet, TimestampHelper}; -use satrs_minisim::acs::lis3mdl::{ - MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, -}; +use satrs_example::{HkHelperSingleSet, ModeHelper, TimestampHelper, TmtcQueues}; use satrs_minisim::acs::MgmRequestLis3Mdl; +use satrs_minisim::acs::lis3mdl::{ + FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, MgmLis3MdlReply, MgmLis3RawValues, +}; use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; -use std::fmt::Debug; -use std::sync::mpsc::{self}; +use std::sync::mpsc; use std::sync::{Arc, Mutex}; use std::time::Duration; use satrs::request::MessageMetadata; -use crate::acs::mgm_assembly; use crate::ccsds::pack_ccsds_tm_packet_for_now; use crate::eps::PowerSwitchHelper; -use crate::spi::SpiInterface; pub const NR_OF_DATA_AND_CFG_REGISTERS: usize = 14; @@ -28,6 +26,29 @@ pub const X_LOWBYTE_IDX: usize = 9; pub const Y_LOWBYTE_IDX: usize = 11; pub const Z_LOWBYTE_IDX: usize = 13; +#[derive(Debug, PartialEq, Eq, Clone, Copy)] +pub enum MgmId { + _0, + _1, +} + +impl MgmId { + pub const fn str(&self) -> &str { + match self { + MgmId::_0 => "MGM 0", + MgmId::_1 => "MGM 1", + } + } + + #[inline] + pub const fn component_id(&self) -> ComponentId { + match self { + MgmId::_0 => ComponentId::AcsMgm0, + MgmId::_1 => ComponentId::AcsMgm1, + } + } +} + #[derive(Default, Debug, PartialEq, Eq)] pub enum TransitionState { #[default] @@ -41,14 +62,26 @@ pub struct SpiDummyInterface { pub dummy_values: MgmLis3RawValues, } -impl SpiInterface for SpiDummyInterface { - type Error = (); - - fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { +impl SpiDummyInterface { + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) { rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.x.to_le_bytes()); rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.y.to_be_bytes()); rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.z.to_be_bytes()); - Ok(()) + } +} + +#[derive(Default)] +pub struct TestSpiInterface { + pub call_count: u32, + pub next_mgm_data: MgmLis3RawValues, +} + +impl TestSpiInterface { + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) { + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.next_mgm_data.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.next_mgm_data.y.to_le_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.next_mgm_data.z.to_le_bytes()); + self.call_count += 1; } } @@ -57,11 +90,9 @@ pub struct SpiSimInterface { pub sim_reply_rx: mpsc::Receiver, } -impl SpiInterface for SpiSimInterface { - type Error = (); - +impl SpiSimInterface { // Right now, we only support requesting sensor data and not configuration of the sensor. - fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) { let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; if let Err(e) = self .sim_request_tx @@ -84,22 +115,22 @@ impl SpiInterface for SpiSimInterface { log::warn!("MGM LIS3 SIM reply timeout: {e}"); } } - Ok(()) } } -pub enum SpiSimInterfaceWrapper { +pub enum SpiCommunication { Dummy(SpiDummyInterface), Sim(SpiSimInterface), + #[allow(dead_code)] + Test(TestSpiInterface), } -impl SpiInterface for SpiSimInterfaceWrapper { - type Error = (); - - fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { +impl SpiCommunication { + fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) { match self { - SpiSimInterfaceWrapper::Dummy(dummy) => dummy.transfer(tx, rx), - SpiSimInterfaceWrapper::Sim(sim_if) => sim_if.transfer(tx, rx), + SpiCommunication::Dummy(dummy) => dummy.transfer(tx, rx), + SpiCommunication::Sim(sim_if) => sim_if.transfer(tx, rx), + SpiCommunication::Test(test_if) => test_if.transfer(tx, rx), } } } @@ -110,67 +141,43 @@ pub struct BufWrapper { rx_buf: [u8; 32], } -pub struct ModeHelpers { - current: DeviceMode, - target: Option, - tc_id: Option, - transition_state: TransitionState, -} - -impl Default for ModeHelpers { - fn default() -> Self { - Self { - current: DeviceMode::Off, - target: Default::default(), - tc_id: Default::default(), - transition_state: Default::default(), - } - } -} - /// Helper component for communication with a parent component, which is usually as assembly. pub struct ModeLeafHelper { - pub request_rx: mpsc::Receiver, - pub report_tx: mpsc::SyncSender, + pub request_rx: mpsc::Receiver, + pub report_tx: mpsc::SyncSender, } /// Example MGM device handler strongly based on the LIS3MDL MEMS device. -pub struct MgmHandlerLis3Mdl { - id: ComponentId, - dev_str: &'static str, - tc_rx: mpsc::Receiver, - tm_tx: mpsc::SyncSender, +pub struct MgmHandlerLis3Mdl { + id: MgmId, switch_helper: PowerSwitchHelper, - pub com_interface: ComInterface, + tmtc_queues: TmtcQueues, + pub spi_com: SpiCommunication, shared_mgm_set: Arc>, buffers: BufWrapper, stamp_helper: TimestampHelper, hk_helper: HkHelperSingleSet, - mode_helpers: ModeHelpers, + mode_helpers: ModeHelper, mode_leaf_helper: ModeLeafHelper, } -impl MgmHandlerLis3Mdl { - #[allow(clippy::too_many_arguments)] +impl MgmHandlerLis3Mdl { pub fn new( - id: ComponentId, - dev_str: &'static str, - tc_rx: mpsc::Receiver, - tm_tx: mpsc::SyncSender, + id: MgmId, + tmtc_queues: TmtcQueues, switch_helper: PowerSwitchHelper, - com_interface: ComInterface, + spi_com: SpiCommunication, shared_mgm_set: Arc>, mode_leaf_helper: ModeLeafHelper, + mode_timeout: Duration ) -> Self { Self { id, - dev_str, - tc_rx, - tm_tx, + tmtc_queues, switch_helper, - com_interface, + spi_com, shared_mgm_set, - mode_helpers: ModeHelpers::default(), + mode_helpers: ModeHelper::new(DeviceMode::Off, mode_timeout), buffers: BufWrapper::default(), stamp_helper: TimestampHelper::default(), hk_helper: HkHelperSingleSet::new(false, Duration::from_millis(200)), @@ -186,9 +193,8 @@ impl MgmHandlerLis3Mdl { #[inline] pub fn switch_id(&self) -> SwitchId { match self.id { - ComponentId::AcsMgm0 => SwitchId::Mgm0, - ComponentId::AcsMgm1 => SwitchId::Mgm1, - _ => panic!("unexpected component id"), + MgmId::_0 => SwitchId::Mgm0, + MgmId::_1 => SwitchId::Mgm1, } } @@ -207,7 +213,7 @@ impl MgmHandlerLis3Mdl { // Poll sensor before checking and generating HK. if self.mode() == DeviceMode::Normal { - log::trace!("polling LIS3MDL sensor {}", self.dev_str); + log::trace!("polling LIS3MDL sensor {}", self.id.str()); self.poll_sensor(); } @@ -219,7 +225,7 @@ impl MgmHandlerLis3Mdl { pub fn handle_telecommands(&mut self) { loop { - match self.tc_rx.try_recv() { + 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) { @@ -236,10 +242,18 @@ impl MgmHandlerLis3Mdl { mgm::request::Request::Hk(hk_request) => { self.handle_hk_request(Some(tc_id), &hk_request) } - mgm::request::Request::Mode(device_mode) => { - self.mode_helpers.tc_id = Some(tc_id); - self.start_transition(device_mode, false); - } + mgm::request::Request::Mode(device_mode) => match device_mode { + ModeRequest::SetMode(device_mode) => { + self.mode_helpers.tc_commander = Some(tc_id); + self.start_transition(device_mode, false); + } + ModeRequest::ReadMode => self.send_telemetry( + Some(tc_id), + mgm::response::Response::Mode(ModeResponse::Mode( + self.mode(), + )), + ), + }, } } Err(e) => { @@ -261,10 +275,8 @@ impl MgmHandlerLis3Mdl { loop { match self.mode_leaf_helper.request_rx.try_recv() { Ok(request) => match request { - mgm_assembly::ModeRequest::SetMode(device_mode) => { - self.start_transition(device_mode, false) - } - mgm_assembly::ModeRequest::ReadMode => self.report_mode_to_parent(), + ModeRequest::SetMode(device_mode) => self.start_transition(device_mode, false), + ModeRequest::ReadMode => self.report_mode_to_parent(), }, Err(e) => match e { std::sync::mpsc::TryRecvError::Empty => break, @@ -281,9 +293,9 @@ impl MgmHandlerLis3Mdl { tc_id: Option, response: mgm::response::Response, ) { - match pack_ccsds_tm_packet_for_now(self.id, tc_id, &response) { + match pack_ccsds_tm_packet_for_now(self.id.component_id(), tc_id, &response) { Ok(packet) => { - if let Err(e) = self.tm_tx.send(packet) { + if let Err(e) = self.tmtc_queues.tm_tx.send(packet) { log::warn!("failed to send TM packet: {}", e); } } @@ -327,12 +339,10 @@ impl MgmHandlerLis3Mdl { pub fn poll_sensor(&mut self) { // Communicate with the device. This is actually how to read the data from the LIS3 device // SPI interface. - self.com_interface - .transfer( - &self.buffers.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1], - &mut self.buffers.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1], - ) - .expect("failed to transfer data"); + self.spi_com.transfer( + &self.buffers.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1], + &mut self.buffers.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1], + ); let x_raw = i16::from_le_bytes( self.buffers.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] .try_into() @@ -358,12 +368,11 @@ impl MgmHandlerLis3Mdl { } fn start_transition(&mut self, target_mode: DeviceMode, _forced: bool) { - log::info!("{}: transitioning to mode {:?}", self.dev_str, target_mode); + log::info!("{}: transitioning to mode {:?}", self.id.str(), target_mode); if target_mode == DeviceMode::Off { self.shared_mgm_set.lock().unwrap().valid = false; } - self.mode_helpers.transition_state = TransitionState::Idle; - self.mode_helpers.target = Some(target_mode); + self.mode_helpers.start(target_mode); } pub fn handle_mode_transition(&mut self) { @@ -373,7 +382,6 @@ impl MgmHandlerLis3Mdl { let target_mode = self.mode_helpers.target.unwrap(); if target_mode == DeviceMode::On || target_mode == DeviceMode::Normal { if self.mode_helpers.transition_state == TransitionState::Idle { - // TODO: Switch ID for MGM1.. let result = self .switch_helper .send_switch_on_cmd(MessageMetadata::new(0, self.id as u32), self.switch_id()); @@ -383,37 +391,59 @@ impl MgmHandlerLis3Mdl { } self.mode_helpers.transition_state = TransitionState::PowerSwitching; } - if self.mode_helpers.transition_state == TransitionState::PowerSwitching - && self.switch_helper.is_switch_on(self.switch_id()) - { - self.mode_helpers.transition_state = TransitionState::Done; + if self.mode_helpers.transition_state == TransitionState::PowerSwitching { + if self.switch_helper.is_switch_on(self.switch_id()) { + log::info!("switch is on"); + self.mode_helpers.transition_state = TransitionState::Done; + } else if self.mode_helpers.timed_out() { + self.handle_mode_transition_failure(); + } } if self.mode_helpers.transition_state == TransitionState::Done { - self.mode_helpers.current = self.mode_helpers.target.unwrap(); self.handle_mode_reached(); - self.mode_helpers.transition_state = TransitionState::Idle; } } } + // Should be called to complete a mode transition which failed. + fn handle_mode_transition_failure(&mut self) { + let tc_commander = self.mode_helpers.finish(false); + if tc_commander.is_some() { + self.send_telemetry( + tc_commander, + mgm::response::Response::Mode(ModeResponse::SetModeTimeout), + ); + } + self.mode_leaf_helper + .report_tx + .send(ModeResponse::SetModeTimeout) + .unwrap(); + } + + // Should be called to complete a mode transition successfully. fn handle_mode_reached(&mut self) { - self.mode_helpers.target = None; - log::info!( - "{} announcing mode: {:?}", - self.dev_str, - self.mode_helpers.current - ); - if let Some(requestor) = self.mode_helpers.tc_id { + let tc_commander = self.mode_helpers.finish(true); + self.announce_mode(); + if let Some(requestor) = tc_commander { self.send_mode_tm(requestor); } // Inform our parent about mode changes. self.report_mode_to_parent(); } + fn announce_mode(&self) { + log::info!( + "{} announcing mode: {:?}", + self.id.str(), + self.mode_helpers.current + ); + // TODO: Event? + } + fn report_mode_to_parent(&self) { self.mode_leaf_helper .report_tx - .send(mgm_assembly::ModeReport::Mode(self.mode_helpers.current)) + .send(ModeResponse::Mode(self.mode_helpers.current)) .unwrap(); } @@ -425,15 +455,16 @@ impl MgmHandlerLis3Mdl { #[cfg(test)] mod tests { use std::sync::{ - mpsc::{self, TryRecvError}, Arc, + mpsc::{self, TryRecvError}, }; use arbitrary_int::u11; use models::{ + Apid, ComponentId, TcHeader, + ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}, mgm::request::HkRequest, pcdu::{SwitchRequest, SwitchState, SwitchStateBinary}, - Apid, ComponentId, TcHeader, }; use satrs::{request::GenericMessage, spacepackets::SpacePacketHeader}; use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; @@ -468,36 +499,15 @@ mod tests { ) } - #[derive(Default)] - pub struct TestSpiInterface { - pub call_count: u32, - pub next_mgm_data: MgmLis3RawValues, - } - - impl SpiInterface for TestSpiInterface { - type Error = (); - - fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { - rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] - .copy_from_slice(&self.next_mgm_data.x.to_le_bytes()); - rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2] - .copy_from_slice(&self.next_mgm_data.y.to_le_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2] - .copy_from_slice(&self.next_mgm_data.z.to_le_bytes()); - self.call_count += 1; - Ok(()) - } - } - #[allow(dead_code)] pub struct MgmTestbench { - pub assembly_mode_request_tx: mpsc::SyncSender, - pub mode_report_rx: mpsc::Receiver, + pub assembly_mode_request_tx: mpsc::SyncSender, + pub mode_report_rx: mpsc::Receiver, pub shared_switch_set: SharedSwitchSet, pub tc_tx: mpsc::SyncSender, pub tm_rx: mpsc::Receiver, pub switch_rx: mpsc::Receiver>, - pub handler: MgmHandlerLis3Mdl, + pub handler: MgmHandlerLis3Mdl, } impl MgmTestbench { @@ -517,12 +527,10 @@ mod tests { let switch_map = SwitchSet::new(switch_map); let shared_switch_set = SharedSwitchSet::new(Mutex::new(switch_map)); let handler = MgmHandlerLis3Mdl::new( - ComponentId::AcsMgm0, - "TEST_MGM", - tc_rx, - tm_tx, + MgmId::_0, + TmtcQueues { tc_rx, tm_tx }, PowerSwitchHelper::new(switcher_tx, shared_switch_set.clone()), - TestSpiInterface::default(), + SpiCommunication::Test(TestSpiInterface::default()), shared_mgm_set, mode_leaf_helper, ); @@ -536,16 +544,25 @@ mod tests { tc_tx, } } + + pub fn test_spi_interface(&mut self) -> &mut TestSpiInterface { + match &mut self.handler.spi_com { + SpiCommunication::Dummy(_) | SpiCommunication::Sim(_) => { + panic!("unexpected SPI interface") + } + SpiCommunication::Test(test_spi_interface) => test_spi_interface, + } + } } #[test] fn test_basic_handler() { let mut testbench = MgmTestbench::new(); - assert_eq!(testbench.handler.com_interface.call_count, 0); + assert_eq!(testbench.test_spi_interface().call_count, 0); assert_eq!(testbench.handler.mode(), DeviceMode::Off); testbench.handler.periodic_operation(); // Handler is OFF, no changes expected. - assert_eq!(testbench.handler.com_interface.call_count, 0); + assert_eq!(testbench.test_spi_interface().call_count, 0); assert_eq!(testbench.handler.mode(), DeviceMode::Off); } @@ -556,7 +573,7 @@ mod tests { .tc_tx .send(create_request_tc( MgmSelect::_0, - mgm::request::Request::Mode(DeviceMode::Normal), + mgm::request::Request::Mode(ModeRequest::SetMode(DeviceMode::Normal)), )) .unwrap(); testbench.handler.periodic_operation(); @@ -587,7 +604,7 @@ mod tests { .expect("failed to deserialize mode reply"); matches!(response, models::mgm::response::Response::Ok); // The device should have been polled once. - assert_eq!(testbench.handler.com_interface.call_count, 1); + assert_eq!(testbench.test_spi_interface().call_count, 1); let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap(); assert!(mgm_set.x < 0.001); assert!(mgm_set.y < 0.001); @@ -605,12 +622,12 @@ mod tests { y: -1000, z: 1000, }; - testbench.handler.com_interface.next_mgm_data = raw_values; + testbench.test_spi_interface().next_mgm_data = raw_values; testbench .tc_tx .send(create_request_tc( MgmSelect::_0, - mgm::request::Request::Mode(DeviceMode::Normal), + mgm::request::Request::Mode(ModeRequest::SetMode(DeviceMode::Normal)), )) .unwrap(); testbench.handler.periodic_operation(); @@ -694,7 +711,7 @@ mod tests { .tc_tx .send(create_request_tc( MgmSelect::_0, - mgm::request::Request::Mode(DeviceMode::Normal), + mgm::request::Request::Mode(ModeRequest::SetMode(DeviceMode::Normal)), )) .unwrap(); // This simulates one cycle for the power switch to update. diff --git a/satrs-example/src/acs/mgm_assembly.rs b/satrs-example/src/acs/mgm_assembly.rs index 672f742..7d1683f 100644 --- a/satrs-example/src/acs/mgm_assembly.rs +++ b/satrs-example/src/acs/mgm_assembly.rs @@ -1,42 +1,179 @@ -// TODO: Program assembly. -// TODO: Remove dead_code lint as soon as assembly is done. -#![allow(dead_code)] +use std::{sync::mpsc, time::Duration}; -use std::sync::mpsc; +use models::{ + ComponentId, DeviceMode, + mgm_assembly::{AssemblyMode, request, response}, +}; +use satrs::spacepackets::CcsdsPacketIdAndPsc; +use satrs_example::{ModeHelper, TmtcQueues}; -use models::DeviceMode; +use crate::ccsds::pack_ccsds_tm_packet_for_now; -pub enum ModeRequest { - SetMode(DeviceMode), - ReadMode, -} - -pub enum ModeReport { - Mode(DeviceMode), +pub struct ParentQueueHelper { + pub request_rx: mpsc::Receiver, + pub report_tx: mpsc::SyncSender, } /// Helper component for communication with a parent component, which is usually as assembly. -pub struct QueueHelper { - pub request_tx_queues: [mpsc::SyncSender; 2], - pub report_rx_queues: [mpsc::Receiver; 2], +pub struct ChildrenQueueHelper { + pub request_tx_queues: [mpsc::SyncSender; 2], + pub report_rx_queues: [mpsc::Receiver; 2], } +#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] +pub enum TransitionState { + #[default] + Idle, + AwaitingReplies, +} + +#[derive(Debug, Default, Copy, Clone)] +pub struct MgmInfo { + reply_received: bool, + mode: Option, +} + +/// MGM assembly component. pub struct Assembly { - pub(crate) helper: QueueHelper, + 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. + mode_keeping_transition: bool, + tmtc_queues: TmtcQueues, + mgm_modes: [MgmInfo; 2], + parent_queues: ParentQueueHelper, + pub(crate) children_queues: ChildrenQueueHelper, } impl Assembly { - pub fn periodic_operation(&mut self) { - self.handle_mode_queue(); + pub const ID: ComponentId = ComponentId::AcsMgmAssembly; + + pub fn new( + parent_queues: ParentQueueHelper, + children_queues: ChildrenQueueHelper, + tmtc_queues: TmtcQueues, + mode_timeout: Duration, + ) -> Self { + Self { + mode_helper: ModeHelper::new(AssemblyMode::NoModeKeeping, mode_timeout), + mode_keeping_transition: false, + tmtc_queues, + mgm_modes: [MgmInfo::default(); 2], + parent_queues, + children_queues, + } } - pub fn handle_mode_queue(&mut self) { - for rx in &mut self.helper.report_rx_queues { + pub fn periodic_operation(&mut self) { + self.handle_telecommands(); + self.handle_parent_mode_queue(); + self.handle_children_mode_queues(); + + if self.mode_helper.transition_active() { + self.handle_mode_transition(); + } + } + + 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::mgm_assembly::request::Request::Ping => { + self.send_telemetry(Some(tc_id), response::Response::Ok) + } + models::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( + self.mode(), + )), + ), + } + } + }, + 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 send_telemetry( + &self, + tc_id: Option, + response: models::mgm_assembly::response::Response, + ) { + match 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); + } + } + } + + pub fn handle_parent_mode_queue(&mut self) { + loop { + match self.parent_queues.request_rx.try_recv() { + Ok(request) => match request { + request::ModeRequest::SetMode(assembly_mode) => match assembly_mode { + AssemblyMode::Device(_device_mode) => { + self.start_transition(false, assembly_mode, None); + } + AssemblyMode::NoModeKeeping => { + self.mode_helper.current = AssemblyMode::NoModeKeeping; + } + }, + request::ModeRequest::ReadMode => self + .parent_queues + .report_tx + .send(response::ModeReport::Mode(self.mode_helper.current)) + .unwrap(), + }, + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + log::warn!("packet sender disconnected") + } + }, + } + } + } + + pub fn handle_children_mode_queues(&mut self) { + let mut mode_report_received = false; + for (idx, rx) in self.children_queues.report_rx_queues.iter_mut().enumerate() { loop { match rx.try_recv() { - // TODO: Do something with the report. Ok(report) => match report { - ModeReport::Mode(_device_mode) => (), + models::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 => { + // Ignore, handle this with our own timeout. + log::warn!("MGM {} mode timeout", idx); + } }, Err(e) => match e { mpsc::TryRecvError::Empty => break, @@ -47,5 +184,451 @@ impl Assembly { } } } + if !mode_report_received { + return; + } + + // 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() + { + // 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)) + } else { + response::ModeReport::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 + && self + .mgm_modes + .iter() + .all(|info| info.mode != Some(device_mode)) + { + // Children lost mode. Try to command them back to the correct + // mode. + self.start_transition(true, self.mode_helper.current, None); + } + } + + pub fn handle_mode_transition(&mut self) { + if self.mode_helper.target.is_none() { + self.handle_mode_reached(true); + return; + } + let target = self.mode_helper.target.unwrap(); + let device_mode = match target { + AssemblyMode::Device(device_mode) => device_mode, + AssemblyMode::NoModeKeeping => { + self.handle_mode_reached(true); + return; + } + }; + if self.mode_helper.transition_state == TransitionState::Idle { + self.command_children(device_mode); + self.mode_helper.transition_state = TransitionState::AwaitingReplies; + } + if self.mode_helper.transition_state == TransitionState::AwaitingReplies + && self.mode_helper.timed_out() + { + let report = if self.mode_keeping_transition { + response::ModeReport::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) + } else { + response::ModeReport::SetModeTimeout(self.mgm_modes.map(|info| info.mode)) + }; + self.handle_mode_transition_failure(report); + } + } + + pub fn handle_mode_reached(&mut self, success: bool) { + let tc_commander = self.mode_helper.finish(success); + self.announce_mode(); + if tc_commander.is_some() { + self.send_telemetry(tc_commander, response::Response::Ok); + } + self.parent_queues + .report_tx + .send(response::ModeReport::Mode(self.mode_helper.current)) + .unwrap(); + } + + pub fn handle_mode_transition_failure(&mut self, report: response::ModeReport) { + if self.mode_helper.tc_commander.is_some() { + self.send_telemetry( + self.mode_helper.tc_commander, + response::Response::Mode(response::ModeReport::SetModeTimeout( + self.mgm_modes.map(|info| info.mode), + )), + ); + } + self.parent_queues.report_tx.send(report).unwrap(); + self.mode_helper.finish(false); + } + + pub fn command_children(&self, mode: DeviceMode) { + for tx in &self.children_queues.request_tx_queues { + tx.send(models::mgm::request::ModeRequest::SetMode(mode)) + .unwrap(); + } + } + + pub fn start_transition( + &mut self, + mode_keeping: bool, + target: AssemblyMode, + tc_id: Option, + ) { + self.mode_keeping_transition = mode_keeping; + self.mode_helper.tc_commander = tc_id; + self.mgm_modes + .iter_mut() + .for_each(|m| m.reply_received = false); + self.mode_helper.start(target); + } + + fn announce_mode(&self) { + // TODO: Event? + log::info!( + "{:?} announcing mode: {:?}", + Self::ID, + self.mode_helper.current + ); + } + + #[inline] + pub fn mode(&self) -> AssemblyMode { + self.mode_helper.current + } + + #[inline] + #[cfg(test)] + fn mode_transition_active(&self) -> bool { + self.mode_helper.transition_active() + } +} + +#[cfg(test)] +mod tests { + use std::sync::mpsc::TryRecvError; + + use arbitrary_int::u11; + use models::{ + Apid, Message, MessageType, TcHeader, + ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}, + mgm_assembly, + }; + use satrs::spacepackets::SpacePacketHeader; + + use super::*; + + 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], + tc_tx: mpsc::SyncSender, + tm_rx: mpsc::Receiver, + assembly: Assembly, + } + + impl Testbench { + pub fn new() -> Self { + let (subsystem_req_tx, subsystem_req_rx) = mpsc::sync_channel(5); + let (subsystem_report_tx, subsystem_report_rx) = mpsc::sync_channel(5); + + let (mgm_0_mode_request_tx, mgm_0_mode_request_rx) = mpsc::sync_channel(5); + let (mgm_1_mode_request_tx, mgm_1_mode_request_rx) = mpsc::sync_channel(5); + let (mgm_0_mode_report_tx, mgm_0_mode_report_rx) = mpsc::sync_channel(5); + let (mgm_1_mode_report_tx, mgm_1_mode_report_rx) = mpsc::sync_channel(5); + + let (tc_tx, tc_rx) = mpsc::sync_channel(5); + let (tm_tx, tm_rx) = mpsc::sync_channel(5); + + Self { + subsystem_req_tx, + subsystem_report_rx, + mgm_request_rx: [mgm_0_mode_request_rx, mgm_1_mode_request_rx], + mgm_report_tx: [mgm_0_mode_report_tx, mgm_1_mode_report_tx], + tc_tx, + tm_rx, + assembly: Assembly::new( + ParentQueueHelper { + request_rx: subsystem_req_rx, + report_tx: subsystem_report_tx, + }, + ChildrenQueueHelper { + request_tx_queues: [mgm_0_mode_request_tx, mgm_1_mode_request_tx], + report_rx_queues: [mgm_0_mode_report_rx, mgm_1_mode_report_rx], + }, + TmtcQueues { tc_rx, tm_tx }, + Duration::from_millis(20), + ), + } + } + + pub fn assert_all_queues_empty(&self) { + assert!( + matches!(self.tm_rx.try_recv().unwrap_err(), TryRecvError::Empty), + "TM queue not empty" + ); + assert!( + matches!( + self.subsystem_report_rx.try_recv().unwrap_err(), + TryRecvError::Empty + ), + "subsystem report queue not empty" + ); + for rx in self.mgm_request_rx.iter() { + assert!( + matches!(rx.try_recv().unwrap_err(), TryRecvError::Empty), + "mgm request queue not empty" + ) + } + } + } + + pub fn create_request_tc( + request: models::mgm_assembly::request::Request, + ) -> models::ccsds::CcsdsTcPacketOwned { + models::ccsds::CcsdsTcPacketOwned::new_with_request( + SpacePacketHeader::new_from_apid(u11::new(Apid::Acs as u16)), + TcHeader::new(Assembly::ID, request.message_type()), + request, + ) + } + + #[test] + fn basic_test() { + let mut tb = Testbench::new(); + tb.assert_all_queues_empty(); + tb.assembly.periodic_operation(); + tb.assert_all_queues_empty(); + assert_eq!(tb.assembly.mode(), AssemblyMode::NoModeKeeping); + } + + #[test] + fn test_tc_commanded_transition() { + let mut tb = Testbench::new(); + tb.tc_tx + .send(create_request_tc(mgm_assembly::request::Request::Mode( + request::ModeRequest::SetMode(AssemblyMode::Device(DeviceMode::Normal)), + ))) + .unwrap(); + tb.assembly.periodic_operation(); + assert!(tb.assembly.mode_transition_active()); + + for rx in tb.mgm_request_rx.iter() { + let request = rx.try_recv().unwrap(); + assert_eq!( + request, + models::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::Normal, + )) + .unwrap(); + } + + tb.assembly.periodic_operation(); + assert!(!tb.assembly.mode_transition_active()); + assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + + let response = tb.tm_rx.try_recv().unwrap(); + assert_eq!(response.tm_header.sender_id, Assembly::ID); + assert_eq!(response.tm_header.message_type, MessageType::Verification); + let response: response::Response = postcard::from_bytes(&response.payload).unwrap(); + assert_eq!(response, response::Response::Ok); + } + + #[test] + fn test_parent_commanded_transition() { + let mut tb = Testbench::new(); + tb.subsystem_req_tx + .send(request::ModeRequest::SetMode(AssemblyMode::Device( + DeviceMode::Normal, + ))) + .unwrap(); + tb.assembly.periodic_operation(); + assert!(tb.assembly.mode_transition_active()); + + for rx in tb.mgm_request_rx.iter() { + let request = rx.try_recv().unwrap(); + assert_eq!( + request, + models::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::Normal, + )) + .unwrap(); + } + + tb.assembly.periodic_operation(); + assert!(!tb.assembly.mode_transition_active()); + assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + + let report = tb.subsystem_report_rx.try_recv().unwrap(); + assert_eq!( + report, + response::ModeReport::Mode(AssemblyMode::Device(DeviceMode::Normal)) + ); + } + + #[test] + fn test_one_mgm_is_sufficient() { + let mut tb = Testbench::new(); + tb.subsystem_req_tx + .send(request::ModeRequest::SetMode(AssemblyMode::Device( + DeviceMode::Normal, + ))) + .unwrap(); + tb.assembly.periodic_operation(); + assert!(tb.assembly.mode_transition_active()); + + for rx in tb.mgm_request_rx.iter() { + let request = rx.try_recv().unwrap(); + assert_eq!( + request, + models::mgm::request::ModeRequest::SetMode(DeviceMode::Normal) + ); + } + + // One device is sufficient. + tb.mgm_report_tx[0] + .send(models::mgm::response::ModeResponse::Mode( + DeviceMode::Normal, + )) + .unwrap(); + tb.mgm_report_tx[1] + .send(models::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)); + + let report = tb.subsystem_report_rx.try_recv().unwrap(); + assert_eq!( + report, + response::ModeReport::Mode(AssemblyMode::Device(DeviceMode::Normal)) + ); + } + + #[test] + fn test_mode_commanding_fails() { + let mut tb = Testbench::new(); + tb.subsystem_req_tx + .send(request::ModeRequest::SetMode(AssemblyMode::Device( + DeviceMode::Normal, + ))) + .unwrap(); + tb.assembly.periodic_operation(); + assert!(tb.assembly.mode_transition_active()); + + for rx in tb.mgm_request_rx.iter() { + let request = rx.try_recv().unwrap(); + assert_eq!( + request, + models::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(); + } + + tb.assembly.periodic_operation(); + assert!(!tb.assembly.mode_transition_active()); + assert_eq!(tb.assembly.mode(), AssemblyMode::NoModeKeeping); + + let report = tb.subsystem_report_rx.try_recv().unwrap(); + assert_eq!( + report, + response::ModeReport::WrongMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) + ); + } + + #[test] + fn test_mode_keeping_fails() { + let mut tb = Testbench::new(); + tb.subsystem_req_tx + .send(request::ModeRequest::SetMode(AssemblyMode::Device( + DeviceMode::Normal, + ))) + .unwrap(); + tb.assembly.periodic_operation(); + assert!(tb.assembly.mode_transition_active()); + + for rx in tb.mgm_request_rx.iter() { + let request = rx.try_recv().unwrap(); + assert_eq!( + request, + models::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::Normal, + )) + .unwrap(); + } + + tb.assembly.periodic_operation(); + assert!(!tb.assembly.mode_transition_active()); + assert_eq!(tb.assembly.mode(), AssemblyMode::Device(DeviceMode::Normal)); + + let report = tb.subsystem_report_rx.try_recv().unwrap(); + assert_eq!( + report, + response::ModeReport::Mode(AssemblyMode::Device(DeviceMode::Normal)) + ); + + for tx in tb.mgm_report_tx.iter() { + tx.send(models::mgm::response::ModeResponse::Mode(DeviceMode::Off)) + .unwrap(); + } + // This should start mode keeping. + tb.assembly.periodic_operation(); + assert!(tb.assembly.mode_transition_active()); + + for rx in tb.mgm_request_rx.iter() { + let request = rx.try_recv().unwrap(); + assert_eq!( + request, + models::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(); + } + 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)]) + ); } } diff --git a/satrs-example/src/ccsds.rs b/satrs-example/src/ccsds.rs index 39b6b14..7122809 100644 --- a/satrs-example/src/ccsds.rs +++ b/satrs-example/src/ccsds.rs @@ -1,8 +1,8 @@ use arbitrary_int::u11; -use models::{ccsds::CcsdsTmPacketOwned, Apid, ComponentId, Message, TmHeader}; +use models::{Apid, ComponentId, Message, TmHeader, ccsds::CcsdsTmPacketOwned}; use satrs::spacepackets::{ - time::{cds::CdsTime, StdTimestampError}, CcsdsPacketIdAndPsc, SpHeader, + time::{StdTimestampError, cds::CdsTime}, }; use serde::Serialize; diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs index fb7f75b..1cf90bd 100644 --- a/satrs-example/src/config.rs +++ b/satrs-example/src/config.rs @@ -169,7 +169,7 @@ pub mod pool { pub mod tasks { pub const FREQ_MS_UDP_TMTC: u64 = 200; - pub const FREQ_MS_AOCS: u64 = 500; + pub const FREQ_MS_AOCS: u64 = 200; pub const FREQ_MS_CONTROLLER: u64 = 200; pub const SIM_CLIENT_IDLE_DELAY_MS: u64 = 5; } diff --git a/satrs-example/src/control.rs b/satrs-example/src/control.rs index f27835b..0e74795 100644 --- a/satrs-example/src/control.rs +++ b/satrs-example/src/control.rs @@ -1,6 +1,7 @@ use models::{ + ComponentId, ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}, - control, ComponentId, + control, }; use satrs::spacepackets::CcsdsPacketIdAndPsc; diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 5430019..9144490 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -1,24 +1,24 @@ use std::{ cell::RefCell, collections::{HashMap, VecDeque}, - sync::{mpsc, Arc, Mutex}, + sync::{Arc, Mutex, mpsc}, }; use derive_new::new; use models::{ + ComponentId, DeviceMode, ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}, pcdu::{ self, SwitchId, SwitchMapBinary, SwitchMapBinaryWrapper, SwitchRequest, SwitchState, SwitchStateBinary, SwitchesBitfield, }, - ComponentId, DeviceMode, }; use num_enum::{IntoPrimitive, TryFromPrimitive}; use satrs::{request::GenericMessage, spacepackets::CcsdsPacketIdAndPsc}; use satrs_example::TimestampHelper; use satrs_minisim::{ - eps::{PcduReply, PcduRequest}, SerializableSimMsgPayload, SimReply, SimRequest, + eps::{PcduReply, PcduRequest}, }; use serde::{Deserialize, Serialize}; use strum::IntoEnumIterator as _; @@ -532,8 +532,8 @@ mod tests { use arbitrary_int::u11; use models::{ - pcdu::{SwitchMapBinary, SwitchStateBinary}, Apid, TcHeader, + pcdu::{SwitchMapBinary, SwitchStateBinary}, }; use satrs::{ mode::{ModeReply, ModeRequest}, diff --git a/satrs-example/src/event_manager.rs b/satrs-example/src/event_manager.rs index a011ece..e5a129f 100644 --- a/satrs-example/src/event_manager.rs +++ b/satrs-example/src/event_manager.rs @@ -1,4 +1,4 @@ -use models::{ccsds::CcsdsTmPacketOwned, control, ComponentId, Event, Message}; +use models::{ComponentId, Event, Message, ccsds::CcsdsTmPacketOwned, control}; use crate::ccsds::pack_ccsds_tm_packet_for_now; diff --git a/satrs-example/src/interface/sim_client_udp.rs b/satrs-example/src/interface/sim_client_udp.rs index 3d8fa84..bec6db4 100644 --- a/satrs-example/src/interface/sim_client_udp.rs +++ b/satrs-example/src/interface/sim_client_udp.rs @@ -7,8 +7,8 @@ use std::{ use satrs::pus::HandlingStatus; use satrs_minisim::{ - udp::SIM_CTRL_PORT, SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimReply, - SimRequest, + SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimReply, SimRequest, + udp::SIM_CTRL_PORT, }; use satrs_minisim::{SimCtrlReply, SimCtrlRequest}; @@ -187,16 +187,17 @@ pub mod tests { collections::HashMap, net::{SocketAddr, UdpSocket}, sync::{ + Arc, atomic::{AtomicBool, Ordering}, - mpsc, Arc, + mpsc, }, time::Duration, }; use satrs_minisim::{ - eps::{PcduReply, PcduRequest}, SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, SimReply, SimRequest, + eps::{PcduReply, PcduRequest}, }; use super::SimClientUdp; diff --git a/satrs-example/src/interface/tcp.rs b/satrs-example/src/interface/tcp.rs index b1ce05d..40b05bf 100644 --- a/satrs-example/src/interface/tcp.rs +++ b/satrs-example/src/interface/tcp.rs @@ -129,15 +129,13 @@ impl TcpTask { } pub fn periodic_operation(&mut self) { - loop { - let result = self - .0 - .handle_all_connections(Some(Duration::from_millis(400))); - match result { - Ok(_conn_result) => (), - Err(e) => { - warn!("TCP server error: {e:?}"); - } + let result = self + .0 + .handle_all_connections(Some(Duration::from_millis(400))); + match result { + Ok(_conn_result) => (), + Err(e) => { + warn!("TCP server error: {e:?}"); } } } diff --git a/satrs-example/src/interface/udp.rs b/satrs-example/src/interface/udp.rs index 6a0395d..350a740 100644 --- a/satrs-example/src/interface/udp.rs +++ b/satrs-example/src/interface/udp.rs @@ -1,7 +1,7 @@ #![allow(dead_code)] use std::collections::VecDeque; use std::net::{SocketAddr, UdpSocket}; -use std::sync::{mpsc, Arc, Mutex}; +use std::sync::{Arc, Mutex, mpsc}; use log::warn; use models::ccsds::CcsdsTmPacketOwned; @@ -115,11 +115,11 @@ mod tests { use models::Apid; use satrs::spacepackets::ecss::{CreatorConfig, MessageTypeId}; use satrs::{ - spacepackets::{ - ecss::{tc::PusTcCreator, WritablePusPacket}, - SpHeader, - }, ComponentId, + spacepackets::{ + SpHeader, + ecss::{WritablePusPacket, tc::PusTcCreator}, + }, }; use satrs_example::config::OBSW_SERVER_ADDR; diff --git a/satrs-example/src/lib.rs b/satrs-example/src/lib.rs index 6967898..ee16549 100644 --- a/satrs-example/src/lib.rs +++ b/satrs-example/src/lib.rs @@ -1,9 +1,13 @@ extern crate alloc; -use std::time::{Duration, Instant}; +use std::{ + sync::mpsc, + time::{Duration, Instant}, +}; pub use models::ComponentId; -use satrs::spacepackets::time::cds::CdsTime; +use models::ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}; +use satrs::spacepackets::{CcsdsPacketIdAndPsc, time::cds::CdsTime}; pub mod config; @@ -90,3 +94,64 @@ impl HkHelperSingleSet { false } } + +pub struct TmtcQueues { + pub tc_rx: mpsc::Receiver, + pub tm_tx: mpsc::SyncSender, +} + +#[derive(Debug)] +pub struct ModeHelper { + pub current: Mode, + pub target: Option, + pub tc_commander: Option, + pub transition_start: Option, + pub timeout: Duration, + pub transition_state: TransitionState, +} + +impl ModeHelper { + pub fn new(init_mode: Mode, timeout: Duration) -> Self { + Self { + current: init_mode, + target: Default::default(), + tc_commander: Default::default(), + transition_start: None, + timeout, + transition_state: Default::default(), + } + } + + pub fn start(&mut self, target: Mode) { + self.target = Some(target); + self.transition_start = Some(Instant::now()); + self.transition_state = TransitionState::default(); + } + + #[inline] + pub fn transition_active(&self) -> bool { + self.target.is_some() + } + + pub fn timed_out(&self) -> bool { + if self.target.is_none() { + return false; + } + if let Some(transition_start) = self.transition_start { + return Instant::now() - transition_start >= self.timeout; + } + false + } + + pub fn finish(&mut self, success: bool) -> Option { + self.target?; + if success { + self.current = self.target.take().unwrap(); + } else { + self.target = None; + } + self.transition_state = Default::default(); + self.transition_start = None; + self.tc_commander.take() + } +} diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 94be517..922f8ba 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -1,13 +1,17 @@ use std::{ net::{IpAddr, SocketAddr}, - sync::{mpsc, Arc, Mutex}, + sync::{ + Arc, Mutex, + atomic::{AtomicBool, Ordering}, + mpsc, + }, thread, time::Duration, }; use eps::{ - pcdu::{PcduHandler, SerialInterfaceDummy, SerialInterfaceToSim, SerialSimInterfaceWrapper}, PowerSwitchHelper, + pcdu::{PcduHandler, SerialInterfaceDummy, SerialInterfaceToSim, SerialSimInterfaceWrapper}, }; use interface::{ sim_client_udp::create_sim_client, @@ -24,21 +28,19 @@ use satrs::{ request::{GenericMessage, MessageMetadata}, spacepackets::time::cds::CdsTime, }; -use satrs_example::config::{ - components::NO_SENDER, - tasks::{FREQ_MS_AOCS, FREQ_MS_CONTROLLER, FREQ_MS_UDP_TMTC, SIM_CLIENT_IDLE_DELAY_MS}, - OBSW_SERVER_ADDR, PACKET_ID_VALIDATOR, SERVER_PORT, +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}, + }, }; use tmtc::sender::TmTcSender; use tmtc::{tc_source::TcSourceTask, tm_sink::TmSink}; use crate::{ - acs::{ - mgm::{ - self, MgmHandlerLis3Mdl, SpiDummyInterface, SpiSimInterface, SpiSimInterfaceWrapper, - }, - mgm_assembly, - }, + acs::{mgm, mgm_assembly}, control::Controller, eps::pcdu::SwitchSet, event_manager::EventManager, @@ -53,12 +55,17 @@ mod eps; mod event_manager; mod interface; mod logger; -mod spi; mod tmtc; - fn main() { + static KILL_SIGNAL: AtomicBool = AtomicBool::new(false); + setup_logger().expect("setting up logging with fern failed"); println!("Runng OBSW example"); + ctrlc::set_handler(move || { + log::info!("Received Ctrl-C, shutting down"); + KILL_SIGNAL.store(true, Ordering::Relaxed); + }) + .expect("Error setting Ctrl-C handler"); let (tc_source_tx, tc_source_rx) = mpsc::sync_channel(50); let (tm_sink_tx, tm_sink_rx) = mpsc::sync_channel(50); @@ -72,10 +79,15 @@ 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 (pcdu_handler_tc_tx, pcdu_handler_tc_rx) = mpsc::sync_channel(30); let (controller_tc_tx, controller_tc_rx) = mpsc::sync_channel(10); - // These message handles need to go into the MGM assembly. + // 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); + + // 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); let (mgm_1_mode_request_tx, mgm_1_mode_request_rx) = mpsc::sync_channel(5); let (mgm_0_mode_report_tx, mgm_0_mode_report_rx) = mpsc::sync_channel(5); @@ -97,6 +109,7 @@ fn main() { tc_source.add_target(ComponentId::Controller, controller_tc_tx); 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); let tc_sender = TmTcSender::Normal(tc_source_tx.clone()); let udp_tm_handler = UdpTmHandlerWithChannel { @@ -147,26 +160,27 @@ fn main() { sim_client .add_reply_recipient(satrs_minisim::SimComponent::Mgm1Lis3Mdl, mgm_1_sim_reply_tx); ( - SpiSimInterfaceWrapper::Sim(SpiSimInterface { + mgm::SpiCommunication::Sim(mgm::SpiSimInterface { sim_request_tx: sim_request_tx.clone(), sim_reply_rx: mgm_0_sim_reply_rx, }), - SpiSimInterfaceWrapper::Sim(SpiSimInterface { + mgm::SpiCommunication::Sim(mgm::SpiSimInterface { sim_request_tx: sim_request_tx.clone(), sim_reply_rx: mgm_1_sim_reply_rx, }), ) } else { ( - SpiSimInterfaceWrapper::Dummy(SpiDummyInterface::default()), - SpiSimInterfaceWrapper::Dummy(SpiDummyInterface::default()), + mgm::SpiCommunication::Dummy(mgm::SpiDummyInterface::default()), + mgm::SpiCommunication::Dummy(mgm::SpiDummyInterface::default()), ) }; - let mut mgm_0_handler = MgmHandlerLis3Mdl::new( - ComponentId::AcsMgm0, - "MGM_0", - mgm_0_handler_tc_rx, - tm_sink_tx.clone(), + let mut mgm_0_handler = mgm::MgmHandlerLis3Mdl::new( + mgm::MgmId::_0, + TmtcQueues { + tc_rx: mgm_0_handler_tc_rx, + tm_tx: tm_sink_tx.clone(), + }, switch_helper.clone(), mgm_0_spi_interface, shared_mgm_0_set, @@ -174,12 +188,14 @@ fn main() { request_rx: mgm_0_mode_request_rx, report_tx: mgm_0_mode_report_tx, }, + Duration::from_millis(1000) ); - let mut mgm_1_handler = MgmHandlerLis3Mdl::new( - ComponentId::AcsMgm1, - "MGM_1", - mgm_1_handler_tc_rx, - tm_sink_tx.clone(), + let mut mgm_1_handler = mgm::MgmHandlerLis3Mdl::new( + mgm::MgmId::_1, + TmtcQueues { + tc_rx: mgm_1_handler_tc_rx, + tm_tx: tm_sink_tx.clone(), + }, switch_helper.clone(), mgm_1_spi_interface, shared_mgm_1_set, @@ -187,13 +203,23 @@ fn main() { request_rx: mgm_1_mode_request_rx, report_tx: mgm_1_mode_report_tx, }, + Duration::from_millis(1000) ); - let mut mgm_assembly = mgm_assembly::Assembly { - helper: mgm_assembly::QueueHelper { + let mut mgm_assembly = mgm_assembly::Assembly::new( + mgm_assembly::ParentQueueHelper { + request_rx: mgm_assembly_request_rx, + report_tx: mgm_assembly_report_tx, + }, + mgm_assembly::ChildrenQueueHelper { request_tx_queues: [mgm_0_mode_request_tx, mgm_1_mode_request_tx], report_rx_queues: [mgm_0_mode_report_rx, mgm_1_mode_report_rx], }, - }; + TmtcQueues { + tc_rx: mgm_assembly_tc_rx, + tm_tx: tm_sink_tx.clone(), + }, + Duration::from_millis(2000), + ); 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); @@ -230,6 +256,9 @@ fn main() { .spawn(move || { info!("Running UDP server on port {SERVER_PORT}"); loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } udp_tmtc_server.periodic_operation(); tc_source.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_UDP_TMTC)); @@ -243,6 +272,9 @@ fn main() { .spawn(move || { info!("Running TCP server on port {SERVER_PORT}"); loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } tcp_server.periodic_operation(); } }) @@ -251,8 +283,13 @@ fn main() { info!("Starting TM funnel task"); let jh_tm_funnel = thread::Builder::new() .name("TM SINK".to_string()) - .spawn(move || loop { - tm_sink.operation(); + .spawn(move || { + loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } + tm_sink.operation(); + } }) .unwrap(); @@ -262,9 +299,14 @@ fn main() { opt_jh_sim_client = Some( thread::Builder::new() .name("SIM ADAPTER".to_string()) - .spawn(move || loop { - if sim_client.operation() == HandlingStatus::Empty { - std::thread::sleep(Duration::from_millis(SIM_CLIENT_IDLE_DELAY_MS)); + .spawn(move || { + loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } + if sim_client.operation() == HandlingStatus::Empty { + std::thread::sleep(Duration::from_millis(SIM_CLIENT_IDLE_DELAY_MS)); + } } }) .unwrap(), @@ -274,39 +316,54 @@ fn main() { info!("Starting AOCS thread"); let jh_aocs = thread::Builder::new() .name("AOCS".to_string()) - .spawn(move || loop { - mgm_0_handler.periodic_operation(); - mgm_1_handler.periodic_operation(); - mgm_assembly.periodic_operation(); - thread::sleep(Duration::from_millis(FREQ_MS_AOCS)); + .spawn(move || { + loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } + mgm_0_handler.periodic_operation(); + mgm_1_handler.periodic_operation(); + mgm_assembly.periodic_operation(); + thread::sleep(Duration::from_millis(FREQ_MS_AOCS)); + } }) .unwrap(); info!("Starting EPS thread"); let jh_eps = thread::Builder::new() .name("EPS".to_string()) - .spawn(move || loop { - // TODO: We should introduce something like a fixed timeslot helper to allow a more - // declarative API. It would also be very useful for the AOCS task. - // - // TODO: The fixed timeslot handler exists.. use it. - // TODO: Why not just use sync code in the PCDU handler, and fully delay there? - pcdu_handler.periodic_operation(crate::eps::pcdu::OpCode::RegularOp); - thread::sleep(Duration::from_millis(50)); - pcdu_handler.periodic_operation(crate::eps::pcdu::OpCode::PollAndRecvReplies); - thread::sleep(Duration::from_millis(50)); - pcdu_handler.periodic_operation(crate::eps::pcdu::OpCode::PollAndRecvReplies); - thread::sleep(Duration::from_millis(300)); + .spawn(move || { + loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } + // TODO: We should introduce something like a fixed timeslot helper to allow a more + // declarative API. It would also be very useful for the AOCS task. + // + // TODO: The fixed timeslot handler exists.. use it. + // TODO: Why not just use sync code in the PCDU handler, and fully delay there? + pcdu_handler.periodic_operation(crate::eps::pcdu::OpCode::RegularOp); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(crate::eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(crate::eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(300)); + } }) .unwrap(); info!("Starting controller thread"); let jh_controller_thread = thread::Builder::new() .name("CTRL".to_string()) - .spawn(move || loop { - controller.periodic_operation(); - event_manager.periodic_operation(); - thread::sleep(Duration::from_millis(FREQ_MS_CONTROLLER)); + .spawn(move || { + loop { + if KILL_SIGNAL.load(Ordering::Relaxed) { + break; + } + controller.periodic_operation(); + event_manager.periodic_operation(); + thread::sleep(Duration::from_millis(FREQ_MS_CONTROLLER)); + } }) .unwrap(); diff --git a/satrs-example/src/spi.rs b/satrs-example/src/spi.rs deleted file mode 100644 index 165e6da..0000000 --- a/satrs-example/src/spi.rs +++ /dev/null @@ -1,6 +0,0 @@ -use core::fmt::Debug; - -pub trait SpiInterface { - type Error: Debug; - fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>; -} diff --git a/satrs-example/src/tmtc/sender.rs b/satrs-example/src/tmtc/sender.rs index 71fd17b..26f84e4 100644 --- a/satrs-example/src/tmtc/sender.rs +++ b/satrs-example/src/tmtc/sender.rs @@ -1,9 +1,9 @@ use std::{cell::RefCell, collections::VecDeque, sync::mpsc}; use satrs::{ + ComponentId, queue::GenericSendError, tmtc::{PacketAsVec, PacketHandler}, - ComponentId, }; #[derive(Default, Debug, Clone)] diff --git a/satrs-example/src/tmtc/tc_source.rs b/satrs-example/src/tmtc/tc_source.rs index 6a26a40..a20af6a 100644 --- a/satrs-example/src/tmtc/tc_source.rs +++ b/satrs-example/src/tmtc/tc_source.rs @@ -1,4 +1,4 @@ -use models::{ccsds::CcsdsTcPacketOwned, ComponentId, TcHeader}; +use models::{ComponentId, TcHeader, ccsds::CcsdsTcPacketOwned}; use satrs::{ pus::HandlingStatus, spacepackets::{CcsdsPacketReader, ChecksumType}, diff --git a/satrs-example/src/tmtc/tm_sink.rs b/satrs-example/src/tmtc/tm_sink.rs index f5092de..0cb2724 100644 --- a/satrs-example/src/tmtc/tm_sink.rs +++ b/satrs-example/src/tmtc/tm_sink.rs @@ -46,7 +46,7 @@ impl TmSink { } pub fn operation(&mut self) { - if let Ok(mut tm) = self.tm_funnel_rx.recv() { + if let Ok(mut tm) = self.tm_funnel_rx.try_recv() { tm.sp_header .set_seq_count(self.seq_counter_map.get_and_increment(tm.sp_header.apid())); self.sync_tm_tcp_source.add_tm(&tm.to_vec()); diff --git a/satrs/src/dev_mgmt.rs b/satrs/src/dev_mgmt.rs deleted file mode 100644 index bf4c9e5..0000000 --- a/satrs/src/dev_mgmt.rs +++ /dev/null @@ -1,448 +0,0 @@ -use crate::{ - ComponentId, - mode::{ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender}, - mode_tree::{ModeStoreProvider, ModeStoreVec}, - queue::{GenericSendError, GenericTargetedMessagingError}, - request::{GenericMessage, RequestId}, -}; -use core::fmt::Debug; - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub struct ActiveModeCommandContext { - pub target_mode: ModeAndSubmode, - pub active_request_id: RequestId, -} - -#[derive(Debug, Default, PartialEq, Eq)] -pub enum DevManagerHelperResult { - #[default] - Idle, - Busy, - ModeCommandingDone(ActiveModeCommandContext), -} - -#[derive(Debug)] -pub enum DevManagerHelperError { - ChildNotInStore, -} - -pub trait DevManagerUserHook: Debug { - fn send_mode_cmd_to_child( - &self, - request_id: RequestId, - target_id: ComponentId, - mode: ModeAndSubmode, - forced: bool, - children_mode_store: &mut ModeStoreVec, - mode_req_sender: &impl ModeRequestSender, - ) -> Result<(), GenericSendError>; - - fn send_mode_cmds_to_children( - &self, - request_id: RequestId, - commanded_parent_mode: ModeAndSubmode, - forced: bool, - children_mode_store: &mut ModeStoreVec, - mode_req_sender: &impl ModeRequestSender, - ) -> Result<(), GenericSendError>; -} - -#[derive(Debug, Default)] -pub struct TransparentDevManagerHook {} - -impl DevManagerUserHook for TransparentDevManagerHook { - fn send_mode_cmds_to_children( - &self, - request_id: RequestId, - commanded_parent_mode: ModeAndSubmode, - forced: bool, - children_mode_store: &mut ModeStoreVec, - mode_req_sender: &impl ModeRequestSender, - ) -> Result<(), GenericSendError> { - for child in children_mode_store { - mode_req_sender.send_mode_request( - request_id, - child.id(), - ModeRequest::SetMode { - mode_and_submode: commanded_parent_mode, - forced, - }, - )?; - child.awaiting_reply = true; - } - Ok(()) - } - - fn send_mode_cmd_to_child( - &self, - request_id: RequestId, - target_id: ComponentId, - mode: ModeAndSubmode, - forced: bool, - children_mode_store: &mut ModeStoreVec, - mode_req_sender: &impl ModeRequestSender, - ) -> Result<(), GenericSendError> { - let mut_val = children_mode_store - .get_mut(target_id) - .ok_or(GenericSendError::TargetDoesNotExist(target_id))?; - mut_val.awaiting_reply = true; - mode_req_sender.send_mode_request( - request_id, - target_id, - ModeRequest::SetMode { - mode_and_submode: mode, - forced, - }, - )?; - Ok(()) - } -} - -#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] -pub enum DevManagerCommandingState { - #[default] - Idle, - AwaitingReplies(ActiveModeCommandContext), -} - -impl DevManagerCommandingState { - fn new_active_cmd(mode_and_submode: ModeAndSubmode, active_request_id: RequestId) -> Self { - DevManagerCommandingState::AwaitingReplies(ActiveModeCommandContext { - target_mode: mode_and_submode, - active_request_id, - }) - } -} - -/// A generic helper for manager components which manage child components in a mode tree. -/// -/// Mode commands are usually forwarded to all children components transparently. -/// For example, this could be used in an Assembly component which manages multiple redundant -/// child components. It can also be used inside a manager component which only manages one device. -#[derive(Debug, Default)] -pub struct DevManagerCommandingHelper { - /// The IDs, modes and reply awaition status of all children are tracked in this data - /// structure. - pub children_mode_store: ModeStoreVec, - pub user_hook: UserHook, - pub state: DevManagerCommandingState, -} - -impl DevManagerCommandingHelper { - pub fn new(user_hook: UserHook) -> Self { - Self { - children_mode_store: Default::default(), - user_hook, - state: Default::default(), - } - } - - pub fn send_mode_cmd_to_one_child( - &mut self, - request_id: RequestId, - target_id: ComponentId, - mode_and_submode: ModeAndSubmode, - forced: bool, - mode_req_sender: &impl ModeRequestSender, - ) -> Result<(), GenericSendError> { - self.state = DevManagerCommandingState::new_active_cmd(mode_and_submode, request_id); - self.user_hook.send_mode_cmd_to_child( - request_id, - target_id, - mode_and_submode, - forced, - &mut self.children_mode_store, - mode_req_sender, - )?; - Ok(()) - } - - pub fn send_mode_cmd_to_all_children( - &mut self, - request_id: RequestId, - mode_and_submode: ModeAndSubmode, - forced: bool, - mode_req_sender: &impl ModeRequestSender, - ) -> Result<(), GenericSendError> { - self.state = DevManagerCommandingState::new_active_cmd(mode_and_submode, request_id); - self.user_hook.send_mode_cmds_to_children( - request_id, - mode_and_submode, - forced, - &mut self.children_mode_store, - mode_req_sender, - )?; - Ok(()) - } - - pub fn target_mode(&self) -> Option { - match self.state { - DevManagerCommandingState::Idle => None, - DevManagerCommandingState::AwaitingReplies(context) => Some(context.target_mode), - } - } - - pub fn state(&self) -> DevManagerCommandingState { - self.state - } - - pub fn send_announce_mode_cmd_to_children( - &self, - request_id: RequestId, - mode_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.iter() { - mode_req_sender.send_mode_request(request_id, child.id(), request)?; - } - Ok(()) - } - - pub fn add_mode_child(&mut self, target_id: ComponentId, mode: ModeAndSubmode) { - self.children_mode_store.add_component(target_id, mode); - } - - /// Helper method which counts the number of children which have a certain mode. - pub fn count_number_of_children_with_mode(&self, mode_and_submode: ModeAndSubmode) -> usize { - let mut children_in_target_mode = 0; - for child in &self.children_mode_store { - if child.mode_and_submode() == mode_and_submode { - children_in_target_mode += 1; - } - } - children_in_target_mode - } - - pub fn handle_mode_reply( - &mut self, - mode_reply: &GenericMessage, - ) -> Result { - let context = match self.state { - DevManagerCommandingState::Idle => return Ok(DevManagerHelperResult::Idle), - DevManagerCommandingState::AwaitingReplies(active_mode_command_context) => { - Some(active_mode_command_context) - } - }; - if !self - .children_mode_store - .has_component(mode_reply.sender_id()) - { - return Err(DevManagerHelperError::ChildNotInStore); - } - let mut generic_mode_reply_handler = |mode_and_submode: Option| { - // 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 let DevManagerCommandingState::AwaitingReplies { .. } = self.state { - handle_awaition = true; - } - let still_awating_replies = self.children_mode_store.mode_reply_handler( - mode_reply.sender_id(), - mode_and_submode, - handle_awaition, - ); - // It is okay to unwrap: If awaition should be handled, the returned value should - // always be some valid value. - if handle_awaition && !still_awating_replies.unwrap() { - self.state = DevManagerCommandingState::Idle; - return Ok(DevManagerHelperResult::ModeCommandingDone(context.unwrap())); - } - Ok(DevManagerHelperResult::Busy) - }; - match mode_reply.message { - ModeReply::ModeInfo(mode_and_submode) | ModeReply::ModeReply(mode_and_submode) => { - generic_mode_reply_handler(Some(mode_and_submode)) - } - ModeReply::CantReachMode(_result_u16) => generic_mode_reply_handler(None), - ModeReply::WrongMode { - expected: _, - reached, - } => generic_mode_reply_handler(Some(reached)), - } - } -} - -#[cfg(test)] -mod tests { - use crate::{ - mode::{UNKNOWN_MODE, tests::ModeReqSenderMock}, - request::MessageMetadata, - }; - - use super::*; - - pub enum ExampleId { - Id1 = 1, - Id2 = 2, - } - - pub enum ExampleMode { - Mode1 = 1, - Mode2 = 2, - } - - #[test] - fn test_basic() { - let assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); - assert_eq!(assy_helper.state(), DevManagerCommandingState::Idle); - } - - #[test] - fn test_mode_announce() { - let mut assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); - let mode_req_sender = ModeReqSenderMock::default(); - assy_helper.add_mode_child(ExampleId::Id1 as ComponentId, UNKNOWN_MODE); - assy_helper.add_mode_child(ExampleId::Id2 as ComponentId, UNKNOWN_MODE); - assy_helper - .send_announce_mode_cmd_to_children(1, &mode_req_sender, false) - .unwrap(); - assert_eq!(mode_req_sender.requests.borrow().len(), 2); - let mut req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id1 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!(req.request, ModeRequest::AnnounceMode); - req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id2 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!(req.request, ModeRequest::AnnounceMode); - } - - #[test] - fn test_mode_announce_recursive() { - let mut assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); - let mode_req_sender = ModeReqSenderMock::default(); - assy_helper.add_mode_child(ExampleId::Id1 as ComponentId, UNKNOWN_MODE); - assy_helper.add_mode_child(ExampleId::Id2 as ComponentId, UNKNOWN_MODE); - assy_helper - .send_announce_mode_cmd_to_children(1, &mode_req_sender, true) - .unwrap(); - assert_eq!(mode_req_sender.requests.borrow().len(), 2); - let mut req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id1 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); - req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id2 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!(req.request, ModeRequest::AnnounceModeRecursive); - } - - #[test] - fn test_mode_commanding_one_child() { - let mut dev_mgmt_helper = - DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); - let mode_req_sender = ModeReqSenderMock::default(); - dev_mgmt_helper.add_mode_child(ExampleId::Id1 as ComponentId, UNKNOWN_MODE); - let expected_mode = ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0); - dev_mgmt_helper - .send_mode_cmd_to_one_child( - 1, - ExampleId::Id1 as ComponentId, - expected_mode, - false, - &mode_req_sender, - ) - .unwrap(); - assert_eq!(mode_req_sender.requests.borrow().len(), 1); - let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id1 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!( - req.request, - ModeRequest::SetMode { - mode_and_submode: expected_mode, - forced: false - } - ); - matches!( - dev_mgmt_helper.state(), - DevManagerCommandingState::AwaitingReplies { .. } - ); - if let DevManagerCommandingState::AwaitingReplies(ctx) = dev_mgmt_helper.state() { - assert_eq!(ctx.target_mode, expected_mode); - assert_eq!(ctx.active_request_id, 1); - } - let reply = GenericMessage::new( - MessageMetadata::new(1, ExampleId::Id1 as ComponentId), - ModeReply::ModeReply(expected_mode), - ); - if let DevManagerHelperResult::ModeCommandingDone(ActiveModeCommandContext { - target_mode, - active_request_id, - }) = dev_mgmt_helper.handle_mode_reply(&reply).unwrap() - { - assert_eq!(target_mode, expected_mode); - assert_eq!(active_request_id, 1); - } - matches!(dev_mgmt_helper.state(), DevManagerCommandingState::Idle); - } - - #[test] - fn test_mode_commanding_multi_child() { - let mut dev_mgmt_helper = - DevManagerCommandingHelper::new(TransparentDevManagerHook::default()); - let mode_req_sender = ModeReqSenderMock::default(); - dev_mgmt_helper.add_mode_child(ExampleId::Id1 as ComponentId, UNKNOWN_MODE); - dev_mgmt_helper.add_mode_child(ExampleId::Id2 as ComponentId, UNKNOWN_MODE); - let expected_mode = ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0); - dev_mgmt_helper - .send_mode_cmd_to_all_children(1, expected_mode, false, &mode_req_sender) - .unwrap(); - assert_eq!(mode_req_sender.requests.borrow().len(), 2); - let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id1 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!( - req.request, - ModeRequest::SetMode { - mode_and_submode: expected_mode, - forced: false - } - ); - let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap(); - assert_eq!(req.target_id, ExampleId::Id2 as ComponentId); - assert_eq!(req.request_id, 1); - assert_eq!( - req.request, - ModeRequest::SetMode { - mode_and_submode: expected_mode, - forced: false - } - ); - matches!( - dev_mgmt_helper.state(), - DevManagerCommandingState::AwaitingReplies { .. } - ); - if let DevManagerCommandingState::AwaitingReplies(ctx) = dev_mgmt_helper.state() { - assert_eq!(ctx.target_mode, expected_mode); - assert_eq!(ctx.active_request_id, 1); - } - - let reply = GenericMessage::new( - MessageMetadata::new(1, ExampleId::Id1 as ComponentId), - ModeReply::ModeReply(expected_mode), - ); - assert_eq!( - dev_mgmt_helper.handle_mode_reply(&reply).unwrap(), - DevManagerHelperResult::Busy - ); - let reply = GenericMessage::new( - MessageMetadata::new(1, ExampleId::Id2 as ComponentId), - ModeReply::ModeReply(expected_mode), - ); - if let DevManagerHelperResult::ModeCommandingDone(ActiveModeCommandContext { - target_mode, - active_request_id, - }) = dev_mgmt_helper.handle_mode_reply(&reply).unwrap() - { - assert_eq!(target_mode, expected_mode); - assert_eq!(active_request_id, 1); - } - matches!(dev_mgmt_helper.state(), DevManagerCommandingState::Idle); - } -} diff --git a/satrs/src/lib.rs b/satrs/src/lib.rs index 2d39285..fed4d14 100644 --- a/satrs/src/lib.rs +++ b/satrs/src/lib.rs @@ -22,8 +22,6 @@ extern crate std; pub mod action; pub mod ccsds; -#[cfg(feature = "alloc")] -pub mod dev_mgmt; pub mod encoding; #[cfg(feature = "std")] pub mod executable; diff --git a/satrs/tests/mode_tree.rs b/satrs/tests/mode_tree.rs index a0b0327..680d980 100644 --- a/satrs/tests/mode_tree.rs +++ b/satrs/tests/mode_tree.rs @@ -1,3 +1,4 @@ +/* use core::cell::Cell; use num_enum::TryFromPrimitive; use satrs::dev_mgmt::{ @@ -1608,3 +1609,4 @@ fn command_safe_mode() { expected_req_id_not_ctrl, ); } +*/