diff --git a/satrs-example/client/src/main.rs b/satrs-example/client/src/main.rs index 178ead2..e98eaf3 100644 --- a/satrs-example/client/src/main.rs +++ b/satrs-example/client/src/main.rs @@ -213,9 +213,7 @@ fn main() -> anyhow::Result<()> { models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Off) } AssemblyModeSelect::Normal => { - models::acs::mgm_assembly::Mode::Device( - models::DeviceMode::Normal, - ) + models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Normal) } }; diff --git a/satrs-example/models/src/acs/mgm_assembly.rs b/satrs-example/models/src/acs/mgm_assembly.rs index f96fd06..261c041 100644 --- a/satrs-example/models/src/acs/mgm_assembly.rs +++ b/satrs-example/models/src/acs/mgm_assembly.rs @@ -75,7 +75,7 @@ pub mod response { } #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] - pub enum ModeReport { + pub enum ModeResponse { /// Mode of the assembly. Mode(super::Mode), /// Timeout failure setting the children modes. @@ -89,7 +89,7 @@ pub mod response { #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug, PartialEq, Eq)] pub enum Response { Ok, - Mode(ModeReport), + Mode(ModeResponse), } impl Response { diff --git a/satrs-example/models/src/acs/subsystem.rs b/satrs-example/models/src/acs/subsystem.rs index 2bfab26..cbcb52e 100644 --- a/satrs-example/models/src/acs/subsystem.rs +++ b/satrs-example/models/src/acs/subsystem.rs @@ -4,6 +4,13 @@ pub enum Mode { Safe, } +#[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy, PartialEq, Eq)] +pub enum ModeChild { + MgmAssembly, + Mgt, + Controller, +} + impl Mode { /// Each subsystem mode has a fallback mode to allow clean transitions. pub const fn fallback_mode(&self) -> Self { @@ -13,3 +20,69 @@ impl Mode { } } } + +pub mod request { + use crate::Message; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeRequest { + SetMode(super::Mode), + ReadMode, + } + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] + pub enum Request { + Ping, + Mode(ModeRequest), + } + + impl Request { + fn message_type(&self) -> crate::MessageType { + match self { + Request::Ping => crate::MessageType::Verification, + Request::Mode(_mode) => crate::MessageType::Mode, + } + } + } + + impl Message for Request { + fn message_type(&self) -> crate::MessageType { + self.message_type() + } + } +} + +pub mod response { + use crate::Message; + + #[derive(Debug, Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)] + pub enum ModeResponse { + /// Mode of the subsystem. + Mode(super::Mode), + /// Command timeout when commanding a child. + CommandTimeout(super::ModeChild), + /// Can not keep mode because a child changed mode unexpectedly. + CanNotKeepMode(super::ModeChild), + } + + #[derive(serde::Serialize, serde::Deserialize, Clone, Copy, Debug)] + pub enum Response { + Ok, + Mode(ModeResponse), + } + + impl Response { + fn message_type(&self) -> crate::MessageType { + match self { + Response::Ok => crate::MessageType::Verification, + Response::Mode(_mode_report) => crate::MessageType::Mode, + } + } + } + + impl Message for Response { + fn message_type(&self) -> crate::MessageType { + self.message_type() + } + } +} diff --git a/satrs-example/src/acs/mgm_assembly.rs b/satrs-example/src/acs/mgm_assembly.rs index ebea489..0d1b204 100644 --- a/satrs-example/src/acs/mgm_assembly.rs +++ b/satrs-example/src/acs/mgm_assembly.rs @@ -11,7 +11,7 @@ use crate::ccsds::pack_ccsds_tm_packet_for_now; pub struct ParentQueueHelper { pub request_rx: mpsc::Receiver, - pub report_tx: mpsc::SyncSender, + pub report_tx: mpsc::SyncSender, } /// Helper component for communication with a parent component, which is usually as assembly. @@ -94,7 +94,7 @@ impl Assembly { } request::ModeRequest::ReadMode => self.send_telemetry( Some(tc_id), - response::Response::Mode(response::ModeReport::Mode( + response::Response::Mode(response::ModeResponse::Mode( self.mode(), )), ), @@ -146,7 +146,7 @@ impl Assembly { request::ModeRequest::ReadMode => self .parent_queues .report_tx - .send(response::ModeReport::Mode(self.mode_helper.current)) + .send(response::ModeResponse::Mode(self.mode_helper.current)) .unwrap(), }, Err(e) => match e { @@ -198,9 +198,9 @@ impl Assembly { self.handle_mode_reached(true); } else { let report = if self.mode_keeping_transition { - response::ModeReport::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) } else { - response::ModeReport::WrongMode(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::WrongMode(self.mgm_modes.map(|info| info.mode)) }; self.handle_mode_transition_failure(report); } @@ -240,9 +240,9 @@ impl Assembly { && self.mode_helper.timed_out() { let report = if self.mode_keeping_transition { - response::ModeReport::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::CanNotKeepMode(self.mgm_modes.map(|info| info.mode)) } else { - response::ModeReport::SetModeTimeout(self.mgm_modes.map(|info| info.mode)) + response::ModeResponse::SetModeTimeout(self.mgm_modes.map(|info| info.mode)) }; self.handle_mode_transition_failure(report); } @@ -256,15 +256,15 @@ impl Assembly { } self.parent_queues .report_tx - .send(response::ModeReport::Mode(self.mode_helper.current)) + .send(response::ModeResponse::Mode(self.mode_helper.current)) .unwrap(); } - pub fn handle_mode_transition_failure(&mut self, report: response::ModeReport) { + pub fn handle_mode_transition_failure(&mut self, report: response::ModeResponse) { if self.mode_helper.tc_commander.is_some() { self.send_telemetry( self.mode_helper.tc_commander, - response::Response::Mode(response::ModeReport::SetModeTimeout( + response::Response::Mode(response::ModeResponse::SetModeTimeout( self.mgm_modes.map(|info| info.mode), )), ); @@ -331,7 +331,7 @@ mod tests { pub struct Testbench { subsystem_req_tx: mpsc::SyncSender, - subsystem_report_rx: mpsc::Receiver, + subsystem_report_rx: mpsc::Receiver, mgm_request_rx: [mpsc::Receiver; 2], mgm_report_tx: [mpsc::SyncSender; 2], tc_tx: mpsc::SyncSender, @@ -486,7 +486,7 @@ mod tests { let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::Mode(Mode::Device(DeviceMode::Normal)) + response::ModeResponse::Mode(Mode::Device(DeviceMode::Normal)) ); } @@ -528,7 +528,7 @@ mod tests { let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::Mode(Mode::Device(DeviceMode::Normal)) + response::ModeResponse::Mode(Mode::Device(DeviceMode::Normal)) ); } @@ -566,7 +566,7 @@ mod tests { let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::WrongMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) + response::ModeResponse::WrongMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) ); } @@ -604,7 +604,7 @@ mod tests { let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::Mode(Mode::Device(DeviceMode::Normal)) + response::ModeResponse::Mode(Mode::Device(DeviceMode::Normal)) ); for tx in tb.mgm_report_tx.iter() { @@ -636,7 +636,7 @@ mod tests { let report = tb.subsystem_report_rx.try_recv().unwrap(); assert_eq!( report, - response::ModeReport::CanNotKeepMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) + response::ModeResponse::CanNotKeepMode([Some(DeviceMode::Off), Some(DeviceMode::Off)]) ); } } diff --git a/satrs-example/src/acs/subsystem.rs b/satrs-example/src/acs/subsystem.rs index 07169e8..f8caa16 100644 --- a/satrs-example/src/acs/subsystem.rs +++ b/satrs-example/src/acs/subsystem.rs @@ -1,12 +1,27 @@ -use std::sync::mpsc::SyncSender; +use std::{ + sync::mpsc::{self, Receiver, SyncSender}, + time::Duration, +}; -use models::ComponentId; +use models::{ + ComponentId, + acs::subsystem::{Mode, response}, +}; +use satrs::spacepackets::CcsdsPacketIdAndPsc; +use satrs_example::{ModeHelper, TmtcQueues}; #[derive(Debug)] pub struct TransitionInfo { check_mode_reached: bool, } +#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] +pub enum TransitionState { + #[default] + Idle, + AwaitingReplies, +} + #[derive(Debug)] pub struct ChildModes { mgm_assembly_mode: models::acs::mgm_assembly::Mode, @@ -79,24 +94,26 @@ const SAFE_SEQUENCE: [TransitionCommands; 2] = [ #[derive(Debug)] pub struct ModeRequestSenders { - mode_request_ctrl: SyncSender, - mode_request_assy: SyncSender, - mode_request_mgt: SyncSender, + pub mode_request_ctrl: SyncSender, + pub mode_request_assy: SyncSender, + pub mode_request_mgt: SyncSender, } #[derive(Debug)] pub struct ModeReportReceivers { - mode_response_ctrl: SyncSender, - mode_response_assy: SyncSender, - mode_response_mgt: SyncSender, + pub mode_response_ctrl: Receiver, + pub mode_response_assy: Receiver, + pub mode_response_mgt: Receiver, } #[derive(Debug)] pub struct Subsystem { - current_mode: models::acs::subsystem::Mode, + mode_helper: ModeHelper, + transition_step: usize, current_child_modes: Option, mode_request_senders: ModeRequestSenders, mode_report_receivers: ModeReportReceivers, + tmtc_queues: TmtcQueues, } impl Subsystem { @@ -105,12 +122,122 @@ impl Subsystem { pub fn new( mode_request_senders: ModeRequestSenders, mode_report_receivers: ModeReportReceivers, + tmtc_queues: TmtcQueues, ) -> Self { Self { - current_mode: models::acs::subsystem::Mode::Off, + mode_helper: ModeHelper::new( + models::acs::subsystem::Mode::Off, + Duration::from_millis(2000), + ), current_child_modes: None, mode_request_senders, mode_report_receivers, + tmtc_queues, + transition_step: 0, + } + } + + pub fn periodic_operation(&mut self) { + self.handle_telecommands(); + } + + pub fn handle_telecommands(&mut self) { + loop { + match self.tmtc_queues.tc_rx.try_recv() { + Ok(packet) => { + let tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&packet.sp_header); + match postcard::from_bytes::( + &packet.payload, + ) { + Ok(request) => match request { + models::acs::subsystem::request::Request::Ping => { + self.send_telemetry(Some(tc_id), response::Response::Ok) + } + models::acs::subsystem::request::Request::Mode(mode_request) => { + self.handle_mode_request(mode_request); + } + }, + Err(e) => { + log::warn!("failed to deserialize request: {}", e); + } + } + } + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => log::warn!("packet sender disconnected"), + }, + } + } + } + + pub fn transition_sequence_for_mode(mode: Mode) -> &'static [TransitionCommands] { + match mode { + Mode::Off => &OFF_SEQUENCE, + Mode::Safe => &SAFE_SEQUENCE, + } + } + + pub fn handle_mode_request( + &mut self, + mode_request: models::acs::subsystem::request::ModeRequest, + ) { + match mode_request { + models::acs::subsystem::request::ModeRequest::SetMode(target_mode) => { + self.mode_helper.start(target_mode); + match target_mode { + models::acs::subsystem::Mode::Off => { + // TODO: We need to start executing the transition table here.. + /* + self.mode_request_senders + .mode_request_assy + .send(models::acs::mgm_assembly::request::ModeRequest::SetMode( + models::acs::mgm_assembly::Mode::Device(models::DeviceMode::Off), + )) + .expect("failed to send mode request to MGM assembly"); + self.mode_request_senders + .mode_request_mgt + .send(models::acs::mgt::request::ModeRequest::SetMode( + models::acs::mgt::Mode::Normal, + )) + .expect("failed to send mode request to MGM assembly"); + self.mode_request_senders + .mode_request_ctrl + .send(models::acs::ctrl::request::ModeRequest::SetMode( + models::acs::ctrl::Mode::Passive, + )) + .expect("failed to send mode request to MGM assembly"); + */ + } + models::acs::subsystem::Mode::Safe => { + // TODO: Perform SAFE sequence. + } + } + } + models::acs::subsystem::request::ModeRequest::ReadMode => { + self.send_telemetry( + None, + response::Response::Mode(response::ModeResponse::Mode( + self.mode_helper.current, + )), + ); + } + } + } + + pub fn send_telemetry( + &self, + tc_id: Option, + response: models::acs::subsystem::response::Response, + ) { + match crate::ccsds::pack_ccsds_tm_packet_for_now(Self::ID, tc_id, &response) { + Ok(packet) => { + if let Err(e) = self.tmtc_queues.tm_tx.send(packet) { + log::warn!("failed to send TM packet: {}", e); + } + } + Err(e) => { + log::warn!("failed to pack TM packet: {}", e); + } } } } diff --git a/satrs-example/src/lib.rs b/satrs-example/src/lib.rs index ee16549..00f376e 100644 --- a/satrs-example/src/lib.rs +++ b/satrs-example/src/lib.rs @@ -95,6 +95,7 @@ impl HkHelperSingleSet { } } +#[derive(Debug)] pub struct TmtcQueues { pub tc_rx: mpsc::Receiver, pub tm_tx: mpsc::SyncSender, diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index bdef7f4..b10b9d3 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -40,7 +40,7 @@ use tmtc::sender::TmTcSender; use tmtc::{tc_source::TcSourceTask, tm_sink::TmSink}; use crate::{ - acs::{mgm, mgm_assembly}, + acs::{mgm, mgm_assembly, subsystem}, control::Controller, eps::pcdu::SwitchSet, event_manager::EventManager, @@ -80,12 +80,19 @@ fn main() { let (mgm_0_handler_tc_tx, mgm_0_handler_tc_rx) = mpsc::sync_channel(10); let (mgm_1_handler_tc_tx, mgm_1_handler_tc_rx) = mpsc::sync_channel(10); let (mgm_assembly_tc_tx, mgm_assembly_tc_rx) = mpsc::sync_channel(10); + let (acs_subsystem_tc_tx, acs_subsystem_tc_rx) = mpsc::sync_channel(10); let (pcdu_handler_tc_tx, pcdu_handler_tc_rx) = mpsc::sync_channel(30); let (controller_tc_tx, controller_tc_rx) = mpsc::sync_channel(10); + let (mgt_request_tx, _mgt_request_rx) = mpsc::sync_channel(5); + let (_mgt_report_tx, mgt_report_rx) = mpsc::sync_channel(5); + + let (acs_ctrl_request_tx, _acs_ctrl_request_rx) = mpsc::sync_channel(5); + let (_acs_ctrl_response_tx, acs_ctrl_response_rx) = mpsc::sync_channel(5); + // These message handles need to go into the MGM assembly and ACS subsystem. - let (_mgm_assembly_request_tx, mgm_assembly_request_rx) = mpsc::sync_channel(5); - let (mgm_assembly_report_tx, _mgm_assembly_report_rx) = mpsc::sync_channel(5); + let (mgm_assembly_request_tx, mgm_assembly_request_rx) = mpsc::sync_channel(5); + let (mgm_assembly_report_tx, mgm_assembly_report_rx) = mpsc::sync_channel(5); // These message handles need to go into the MGM assembly and MGM devices. let (mgm_0_mode_request_tx, mgm_0_mode_request_rx) = mpsc::sync_channel(5); @@ -110,6 +117,7 @@ fn main() { tc_source.add_target(ComponentId::AcsMgm0, mgm_0_handler_tc_tx); tc_source.add_target(ComponentId::AcsMgm1, mgm_1_handler_tc_tx); tc_source.add_target(ComponentId::AcsMgmAssembly, mgm_assembly_tc_tx); + tc_source.add_target(ComponentId::AcsSubsystem, acs_subsystem_tc_tx); let tc_sender = TmTcSender::Normal(tc_source_tx.clone()); let udp_tm_handler = UdpTmHandlerWithChannel { @@ -221,6 +229,23 @@ fn main() { Duration::from_millis(2000), ); + let mut acs_subsystem = subsystem::Subsystem::new( + subsystem::ModeRequestSenders { + mode_request_ctrl: acs_ctrl_request_tx, + mode_request_assy: mgm_assembly_request_tx, + mode_request_mgt: mgt_request_tx, + }, + subsystem::ModeReportReceivers { + mode_response_ctrl: acs_ctrl_response_rx, + mode_response_assy: mgm_assembly_report_rx, + mode_response_mgt: mgt_report_rx, + }, + TmtcQueues { + tc_rx: acs_subsystem_tc_rx, + tm_tx: tm_sink_tx.clone(), + }, + ); + let pcdu_serial_interface = if let Some(sim_client) = opt_sim_client.as_mut() { sim_client.add_reply_recipient(satrs_minisim::SimComponent::Pcdu, pcdu_sim_reply_tx); SerialSimInterfaceWrapper::Sim(SerialInterfaceToSim::new( @@ -324,6 +349,7 @@ fn main() { mgm_0_handler.periodic_operation(); mgm_1_handler.periodic_operation(); mgm_assembly.periodic_operation(); + acs_subsystem.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_AOCS)); } })