From 6a4417c95417c89a6d6dbe08cc7d0e69a3c3d741 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 May 2025 16:18:31 +0200 Subject: [PATCH] added and scheduled MGM assembly --- satrs-example/src/acs/assembly.rs | 174 +++++++++++++++++++++++++++++ satrs-example/src/acs/mgm.rs | 5 +- satrs-example/src/ids.rs | 6 +- satrs-example/src/interface/udp.rs | 5 +- satrs-example/src/main.rs | 33 +++++- satrs/src/request.rs | 2 +- 6 files changed, 215 insertions(+), 10 deletions(-) diff --git a/satrs-example/src/acs/assembly.rs b/satrs-example/src/acs/assembly.rs index 09e2227..7aad154 100644 --- a/satrs-example/src/acs/assembly.rs +++ b/satrs-example/src/acs/assembly.rs @@ -1 +1,175 @@ // TODO: Write the assembly +// + +use std::sync::mpsc; + +use satrs::{ + dev_mgmt::{DevManagerCommandingHelper, DevManagerHelperResult, TransparentDevManagerHook}, + mode::{ + ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeReplyReceiver as _, + ModeReplySender as _, ModeRequest, ModeRequestHandler, ModeRequestReceiver as _, + ModeRequestorAndHandlerMpscBounded, UNKNOWN_MODE, + }, + mode_tree::{ModeChild, ModeNode, ModeParent}, + queue::GenericTargetedMessagingError, + request::{GenericMessage, MessageMetadata}, + ComponentId, +}; +use satrs_example::{ids, DeviceMode}; + +pub type RequestSenderType = mpsc::SyncSender>; +pub type ReplySenderType = mpsc::SyncSender>; + +// TODO: Needs to perform same functions as the integration test assembly, but also needs +// to track mode changes and health changes of children. +pub struct MgmAssembly { + pub mode_node: ModeRequestorAndHandlerMpscBounded, + pub mode_requestor_info: Option, + pub mode_and_submode: ModeAndSubmode, + pub commanding_helper: DevManagerCommandingHelper, +} + +impl MgmAssembly { + pub fn new(mode_node: ModeRequestorAndHandlerMpscBounded) -> Self { + Self { + mode_node, + mode_requestor_info: None, + mode_and_submode: UNKNOWN_MODE, + commanding_helper: DevManagerCommandingHelper::new(TransparentDevManagerHook::default()), + } + } + + pub const fn id() -> ComponentId { + ids::acs::MGM_ASSEMBLY.raw() + } + + pub fn periodic_operation(&mut self) { + self.check_mode_requests().expect("mode messaging error"); + self.check_mode_replies().expect("mode messaging error"); + // TODO: perform target keeping, check whether children are in correct mode. + } + + pub fn check_mode_requests(&mut self) -> Result<(), GenericTargetedMessagingError> { + while let Some(request) = self.mode_node.try_recv_mode_request()? { + self.handle_mode_request(request).unwrap(); + } + Ok(()) + } + + pub fn check_mode_replies(&mut self) -> Result<(), ModeError> { + while let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? { + match self.commanding_helper.handle_mode_reply(&reply_and_id) { + Ok(result) => { + if let DevManagerHelperResult::ModeCommandingDone(context) = result { + // Complete the mode command. + self.mode_and_submode = context.target_mode; + self.handle_mode_reached(self.mode_requestor_info)?; + } + } + Err(err) => match err { + satrs::dev_mgmt::DevManagerHelperError::ChildNotInStore => todo!(), + }, + } + } + Ok(()) + } +} + +impl ModeNode for MgmAssembly { + fn id(&self) -> ComponentId { + Self::id() + } +} +impl ModeParent for MgmAssembly { + type Sender = RequestSenderType; + + fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) { + self.mode_node.add_request_target(id, request_sender); + self.commanding_helper.add_mode_child(id, UNKNOWN_MODE); + } +} + +impl ModeChild for MgmAssembly { + type Sender = ReplySenderType; + + fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) { + self.mode_node.add_reply_target(id, reply_sender); + } +} + +impl ModeProvider for MgmAssembly { + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +impl ModeRequestHandler for MgmAssembly { + type Error = ModeError; + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + forced: bool, + ) -> Result<(), Self::Error> { + // Always accept forced commands and commands to mode OFF. + if self.commanding_helper.target_mode().is_some() + && !forced + && mode_and_submode.mode() != DeviceMode::Off as u32 + { + return Err(ModeError::Busy); + } + self.mode_requestor_info = Some(requestor); + self.commanding_helper.send_mode_cmd_to_all_children( + requestor.request_id(), + mode_and_submode, + forced, + &self.mode_node, + )?; + Ok(()) + } + + fn announce_mode(&self, requestor_info: Option, recursive: bool) { + println!( + "TestAssembly: Announcing mode (recursively: {}): {:?}", + recursive, self.mode_and_submode + ); + let request_id = requestor_info.map_or(0, |info| info.request_id()); + self.commanding_helper + .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive) + .expect("sending mode request failed"); + // TODO: Send announce event. + log::info!( + "MGM assembly announcing mode: {:?}", + self.mode_and_submode() + ); + } + + fn handle_mode_reached( + &mut self, + mode_requestor: Option, + ) -> Result<(), Self::Error> { + if let Some(requestor) = mode_requestor { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?; + } + self.announce_mode(mode_requestor, false); + Ok(()) + } + + fn handle_mode_info( + &mut self, + requestor_info: MessageMetadata, + info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + // TODO: Perform mode keeping. + Ok(()) + } + + fn send_mode_reply( + &self, + requestor: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + self.mode_node.send_mode_reply(requestor, reply)?; + Ok(()) + } +} diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 8c728fc..efeffd2 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -400,6 +400,7 @@ impl< } fn announce_mode(&self, _requestor_info: Option, _recursive: bool) { + // TODO: Event log::info!( "{} announcing mode: {:?}", self.dev_str, @@ -492,7 +493,7 @@ mod tests { tmtc::PacketAsVec, ComponentId, }; - use satrs_example::ids::{acs::ASSEMBLY, Apid}; + use satrs_example::ids::{acs::MGM_ASSEMBLY, Apid}; use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest}; @@ -593,7 +594,7 @@ mod tests { shared_mgm_set, ); handler.add_mode_parent(PUS_MODE.into(), reply_tx_to_pus); - handler.add_mode_parent(ASSEMBLY.into(), reply_tx_to_parent); + handler.add_mode_parent(MGM_ASSEMBLY.into(), reply_tx_to_parent); Self { mode_request_tx: request_tx, mode_reply_rx_to_pus: reply_rx_to_pus, diff --git a/satrs-example/src/ids.rs b/satrs-example/src/ids.rs index 8da7094..d7bd284 100644 --- a/satrs-example/src/ids.rs +++ b/satrs-example/src/ids.rs @@ -15,15 +15,15 @@ pub mod acs { #[derive(Debug, Copy, Clone, PartialEq, Eq)] pub enum Id { Subsystem = 1, - Assembly = 2, + MgmAssembly = 2, Mgm0 = 3, Mgm1 = 4, } pub const SUBSYSTEM: super::UniqueApidTargetId = super::UniqueApidTargetId::new(super::Apid::Acs as u16, Id::Subsystem as u32); - pub const ASSEMBLY: super::UniqueApidTargetId = - super::UniqueApidTargetId::new(super::Apid::Acs as u16, Id::Assembly as u32); + pub const MGM_ASSEMBLY: super::UniqueApidTargetId = + super::UniqueApidTargetId::new(super::Apid::Acs as u16, Id::MgmAssembly as u32); pub const MGM0: super::UniqueApidTargetId = super::UniqueApidTargetId::new(super::Apid::Acs as u16, Id::Mgm0 as u32); pub const MGM1: super::UniqueApidTargetId = diff --git a/satrs-example/src/interface/udp.rs b/satrs-example/src/interface/udp.rs index bf0a71c..f9c3c78 100644 --- a/satrs-example/src/interface/udp.rs +++ b/satrs-example/src/interface/udp.rs @@ -119,7 +119,8 @@ mod tests { }, ComponentId, }; - use satrs_example::config::{components, OBSW_SERVER_ADDR}; + use satrs_example::config::OBSW_SERVER_ADDR; + use satrs_example::ids; use crate::tmtc::sender::{MockSender, TmTcSender}; @@ -175,7 +176,7 @@ mod tests { udp_tc_server, tm_handler, }; - let sph = SpHeader::new_for_unseg_tc(components::Apid::GenericPus as u16, 0, 0); + let sph = SpHeader::new_for_unseg_tc(ids::Apid::GenericPus as u16, 0, 0); let ping_tc = PusTcCreator::new_simple(sph, 17, 1, &[], true) .to_vec() .unwrap(); diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 3f6e165..fc51d20 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -5,7 +5,10 @@ use std::{ time::Duration, }; -use acs::mgm::{MgmHandlerLis3Mdl, SpiDummyInterface, SpiSimInterface, SpiSimInterfaceWrapper}; +use acs::{ + assembly::MgmAssembly, + mgm::{MgmHandlerLis3Mdl, SpiDummyInterface, SpiSimInterface, SpiSimInterfaceWrapper}, +}; use eps::{ pcdu::{PcduHandler, SerialInterfaceDummy, SerialInterfaceToSim, SerialSimInterfaceWrapper}, PowerSwitchHelper, @@ -31,7 +34,10 @@ use pus::{ use requests::GenericRequestRouter; use satrs::{ hal::std::{tcp_server::ServerConfig, udp_server::UdpTcServer}, - mode::{Mode, ModeAndSubmode, ModeRequest, ModeRequestHandlerMpscBounded}, + mode::{ + Mode, ModeAndSubmode, ModeRequest, ModeRequestHandlerMpscBounded, + ModeRequestorAndHandlerMpscBounded, + }, mode_tree::connect_mode_nodes, pus::{event_man::EventRequestWithToken, EcssTcInMemConverter, HandlingStatus}, request::{GenericMessage, MessageMetadata}, @@ -303,6 +309,15 @@ fn main() { let (switch_request_tx, switch_request_rx) = mpsc::sync_channel(20); let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone()); + let (mgm_assy_mode_req_tx, mgm_assy_mode_req_rx) = mpsc::sync_channel(20); + let (mgm_assy_mode_reply_tx, mgm_assy_mode_reply_rx) = mpsc::sync_channel(20); + let mgm_assembly_mode_node = ModeRequestorAndHandlerMpscBounded::new( + MgmAssembly::id(), + mgm_assy_mode_req_rx, + mgm_assy_mode_reply_rx, + ); + let mut mgm_assembly = MgmAssembly::new(mgm_assembly_mode_node); + let shared_mgm_0_set = Arc::default(); let shared_mgm_1_set = Arc::default(); let mgm_0_mode_node = ModeRequestHandlerMpscBounded::new(MGM0.into(), mgm_0_handler_mode_rx); @@ -364,6 +379,19 @@ fn main() { &mut mgm_1_handler, pus_mode_reply_tx.clone(), ); + // Connect assembly to device handlers. + connect_mode_nodes( + &mut mgm_assembly, + mgm_assy_mode_req_tx.clone(), + &mut mgm_1_handler, + mgm_assy_mode_reply_tx.clone(), + ); + connect_mode_nodes( + &mut mgm_assembly, + mgm_assy_mode_req_tx, + &mut mgm_1_handler, + mgm_assy_mode_reply_tx, + ); 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); @@ -455,6 +483,7 @@ fn main() { let jh_aocs = thread::Builder::new() .name("sat-rs aocs".to_string()) .spawn(move || loop { + mgm_assembly.periodic_operation(); mgm_0_handler.periodic_operation(); mgm_1_handler.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_AOCS)); diff --git a/satrs/src/request.rs b/satrs/src/request.rs index 180ce00..3d5b80d 100644 --- a/satrs/src/request.rs +++ b/satrs/src/request.rs @@ -39,7 +39,7 @@ impl UniqueApidTargetId { } } - pub fn raw(&self) -> ComponentId { + pub const fn raw(&self) -> ComponentId { ((self.apid as u64) << 32) | (self.unique_id as u64) }