diff --git a/satrs-example/models/src/lib.rs b/satrs-example/models/src/lib.rs index 4bdc2fc..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( @@ -165,6 +168,19 @@ pub enum DeviceMode { 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_assembly.rs b/satrs-example/models/src/mgm_assembly.rs new file mode 100644 index 0000000..9287e2d --- /dev/null +++ b/satrs-example/models/src/mgm_assembly.rs @@ -0,0 +1,91 @@ +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, 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(AssemblyMode), + } + + 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(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] + pub enum ModeCommandFailure { + Timeout, + } + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] + pub enum Response { + Ok, + ModeFailure(ModeCommandFailure), + } + + impl Response { + fn message_type(&self) -> crate::MessageType { + match self { + Response::Ok => crate::MessageType::Verification, + Response::ModeFailure(_mode_failure) => 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 7288641..d80c892 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -56,11 +56,13 @@ pub enum TransitionState { Done, } +#[derive(Debug, Clone, Copy)] pub enum ModeRequest { SetMode(DeviceMode), ReadMode, } +#[derive(Debug, Clone, Copy)] pub enum ModeReport { /// New mode has been set. Mode(DeviceMode), @@ -406,6 +408,7 @@ impl MgmHandlerLis3Mdl { } } + // Should be called to complete a mode transition which failed. fn handle_mode_transition_failure(&mut self) { self.mode_helpers.finish(false); if let Some(requestor) = self.mode_helpers.tc_id { @@ -420,18 +423,24 @@ impl MgmHandlerLis3Mdl { .unwrap(); } + // Should be called to complete a mode transition successfully. fn handle_mode_reached(&mut self) { - self.mode_helpers.finish(true); - log::info!( - "{} announcing mode: {:?}", - self.id.str(), - self.mode_helpers.current - ); + self.announce_mode(); if let Some(requestor) = self.mode_helpers.tc_id { self.send_mode_tm(requestor); } // Inform our parent about mode changes. self.report_mode_to_parent(); + self.mode_helpers.finish(true); + } + + fn announce_mode(&self) { + log::info!( + "{} announcing mode: {:?}", + self.id.str(), + self.mode_helpers.current + ); + // TODO: Event? } fn report_mode_to_parent(&self) { diff --git a/satrs-example/src/acs/mgm_assembly.rs b/satrs-example/src/acs/mgm_assembly.rs index 538abdf..d8de7d5 100644 --- a/satrs-example/src/acs/mgm_assembly.rs +++ b/satrs-example/src/acs/mgm_assembly.rs @@ -1,24 +1,32 @@ -// TODO: Program assembly. -// TODO: Remove dead_code lint as soon as assembly is done. #![allow(dead_code)] - use std::{sync::mpsc, time::Duration}; -use models::DeviceMode; +use models::{ + ComponentId, DeviceMode, + mgm_assembly::{ + AssemblyMode, + response::{self, ModeCommandFailure}, + }, +}; +use satrs::spacepackets::CcsdsPacketIdAndPsc; use satrs_example::{ModeHelper, TmtcQueues}; +use crate::ccsds::pack_ccsds_tm_packet_for_now; + +#[derive(Debug, Copy, Clone)] pub enum ModeRequest { SetMode(AssemblyMode), ReadMode, } +#[derive(Debug, Copy, Clone)] pub enum ModeReport { /// Mode of the assembly. Mode(AssemblyMode), /// Failure setting the children mode. - SetModeRetryLimitReached([DeviceMode; 2]), - /// An assembly can not keep its mode. - CanNotKeepMode([DeviceMode; 2]), + SetModeTimeout([Option; 2]), + /// An assembly tried modekeeping but can not keep its mode. + CanNotKeepMode([Option; 2]), } pub struct ParentQueueHelper { @@ -36,18 +44,13 @@ pub struct ChildrenQueueHelper { pub enum TransitionState { #[default] Idle, - CommandingChildren { - step: usize, - }, + AwaitingReplies, } -#[derive(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, +#[derive(Debug, Default, Copy, Clone)] +pub struct MgmInfo { + reply_received: bool, + mode: Option, } /// MGM assembly component. @@ -58,30 +61,32 @@ pub struct Assembly { /// mode keeping. mode_keeping_transition: bool, tmtc_queues: TmtcQueues, - mgm_modes: [DeviceMode; 2], + mgm_modes: [MgmInfo; 2], parent_queues: ParentQueueHelper, pub(crate) children_queues: ChildrenQueueHelper, } impl Assembly { - const RETRIES: usize = 3; + 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, Duration::from_millis(200)), + mode_helper: ModeHelper::new(AssemblyMode::NoModeKeeping, mode_timeout), mode_keeping_transition: false, tmtc_queues, - mgm_modes: [DeviceMode::Off; 2], + mgm_modes: [MgmInfo::default(); 2], parent_queues, children_queues, } } pub fn periodic_operation(&mut self) { + self.handle_telecommands(); self.handle_parent_mode_queue(); self.handle_children_mode_queues(); @@ -90,35 +95,49 @@ impl Assembly { } } - pub fn handle_mode_transition(&mut self) { - if self.mode_helper.transition_state == TransitionState::Idle { - self.mode_helper.transition_state = TransitionState::CommandingChildren { step: 0 }; - } - if let TransitionState::CommandingChildren { step } = self.mode_helper.transition_state - && self.mode_helper.timed_out() - { - if step >= Self::RETRIES { - let report = if self.mode_keeping_transition { - ModeReport::CanNotKeepMode(self.mgm_modes) - } else { - ModeReport::SetModeRetryLimitReached(self.mgm_modes) - }; - self.parent_queues.report_tx.send(report).unwrap(); - self.mode_helper.finish(false); + 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(assembly_mode) => { + self.start_transition(false, assembly_mode, Some(tc_id)) + } + }, + 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"), + }, } - if let AssemblyMode::Device(device_mode) = self.mode_helper.target.unwrap() { - self.command_children(device_mode); - } else { - self.mode_helper.finish(true); - } - self.mode_helper.transition_state = - TransitionState::CommandingChildren { step: step + 1 } } } - pub fn command_children(&self, mode: DeviceMode) { - for tx in &self.children_queues.request_tx_queues { - tx.send(super::mgm::ModeRequest::SetMode(mode)).unwrap(); + 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); + } } } @@ -127,9 +146,8 @@ impl Assembly { match self.parent_queues.request_rx.try_recv() { Ok(request) => match request { ModeRequest::SetMode(assembly_mode) => match assembly_mode { - AssemblyMode::Device(device_mode) => { - self.mode_keeping_transition = false; - self.mode_helper.start(AssemblyMode::Device(device_mode)); + AssemblyMode::Device(_device_mode) => { + self.start_transition(false, assembly_mode, None); } AssemblyMode::NoModeKeeping => { self.mode_helper.current = AssemblyMode::NoModeKeeping @@ -152,32 +170,15 @@ impl Assembly { } 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() { Ok(report) => match report { super::mgm::ModeReport::Mode(device_mode) => { - self.mgm_modes[idx] = device_mode; - - // Transition is active, check for completion. - // If at least one child reached the correct mode, we are done. - if self.mode_helper.transition_active() - && let AssemblyMode::Device(device_mode) = - self.mode_helper.target.unwrap() - && self.mgm_modes.contains(&device_mode) - { - self.mode_helper.finish(true); - } - - // Mode keeping active: Check children modes. - if let AssemblyMode::Device(device_mode) = self.mode_helper.current - && self.mgm_modes.iter().all(|m| *m != device_mode) - { - self.mode_keeping_transition = true; - // Children lost mode. Try to command them back to the correct - // mode. - self.mode_helper.start(self.mode_helper.current); - } + self.mgm_modes[idx].mode = Some(device_mode); + self.mgm_modes[idx].reply_received = true; + mode_report_received = true; } super::mgm::ModeReport::SetModeTimeout => { // Ignore, handle this with our own timeout. @@ -193,5 +194,193 @@ impl Assembly { } } } + if !mode_report_received { + return; + } + + // Transition is active, check for completion. + // If at least one child reached the correct mode, we are done. + if self.mode_helper.transition_active() + && let AssemblyMode::Device(device_mode) = self.mode_helper.target.unwrap() + && self.mgm_modes.iter().all(|i| i.reply_received) + && self.mgm_modes.iter().any(|i| i.mode == Some(device_mode)) + { + self.handle_mode_reached(); + } + + // 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(); + 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(); + 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() + { + self.handle_mode_transition_failure(); + } + } + + pub fn handle_mode_reached(&mut self) { + self.announce_mode(); + if self.mode_helper.tc_id.is_some() { + self.send_telemetry(self.mode_helper.tc_id, response::Response::Ok); + } + self.parent_queues + .report_tx + .send(ModeReport::Mode(self.mode_helper.current)) + .unwrap(); + self.mode_helper.finish(true); + } + + pub fn handle_mode_transition_failure(&mut self) { + let report = if self.mode_keeping_transition { + ModeReport::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) + } else { + ModeReport::SetModeTimeout(self.mgm_modes.map(|info| info.mode)) + }; + if self.mode_helper.tc_id.is_some() { + self.send_telemetry( + self.mode_helper.tc_id, + response::Response::ModeFailure(ModeCommandFailure::Timeout), + ); + } + 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(super::mgm::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_id = 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 + ); + } +} + +#[cfg(test)] +mod tests { + use std::sync::mpsc::TryRecvError; + + use models::ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned}; + + 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" + ) + } + } + } + + #[test] + fn basic_test() { + let mut tb = Testbench::new(); + tb.assert_all_queues_empty(); + tb.assembly.periodic_operation(); } } diff --git a/satrs-example/src/lib.rs b/satrs-example/src/lib.rs index 97cf771..ca1f1a1 100644 --- a/satrs-example/src/lib.rs +++ b/satrs-example/src/lib.rs @@ -152,6 +152,7 @@ impl ModeHelper