From c20163b10a8fd0c2f7cb48731839160ec57fb7e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 May 2024 20:38:45 +0200 Subject: [PATCH 01/23] start integrating sim in example --- satrs-example/Cargo.toml | 3 + satrs-example/src/acs/mgm.rs | 98 +++++++-- satrs-example/src/config.rs | 1 - satrs-example/src/interface/mod.rs | 1 + satrs-example/src/interface/sim_client_udp.rs | 172 +++++++++++++++ satrs-example/src/lib.rs | 6 +- satrs-example/src/main.rs | 102 ++++++--- satrs-example/src/pus/mod.rs | 6 +- satrs-minisim/src/acs.rs | 80 ++++--- satrs-minisim/src/controller.rs | 29 +-- satrs-minisim/src/eps.rs | 6 +- satrs-minisim/src/lib.rs | 201 +++++++++--------- satrs-minisim/src/main.rs | 10 +- 13 files changed, 510 insertions(+), 205 deletions(-) create mode 100644 satrs-example/src/interface/sim_client_udp.rs diff --git a/satrs-example/Cargo.toml b/satrs-example/Cargo.toml index b22904b..3749cff 100644 --- a/satrs-example/Cargo.toml +++ b/satrs-example/Cargo.toml @@ -27,6 +27,9 @@ serde_json = "1" path = "../satrs" features = ["test_util"] +[dependencies.satrs-minisim] +path = "../satrs-minisim" + [dependencies.satrs-mib] version = "0.1.1" path = "../satrs-mib" diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index d50bc6d..2445dd6 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -4,7 +4,10 @@ use satrs::queue::{GenericSendError, GenericTargetedMessagingError}; use satrs::spacepackets::ecss::hk; use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; use satrs::spacepackets::SpHeader; -use satrs_example::{DeviceMode, TimeStampHelper}; +use satrs_example::{DeviceMode, TimestampHelper}; +use satrs_minisim::acs::lis3mdl::{FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR}; +use satrs_minisim::acs::MgmRequestLis3Mdl; +use satrs_minisim::{SimReply, SimRequest}; use std::sync::mpsc::{self}; use std::sync::{Arc, Mutex}; @@ -20,9 +23,12 @@ use crate::requests::CompositeRequest; use serde::{Deserialize, Serialize}; -const GAUSS_TO_MICROTESLA_FACTOR: f32 = 100.0; -// This is the selected resoltion for the STM LIS3MDL device for the 4 Gauss sensitivity setting. -const FIELD_LSB_PER_GAUSS_4_SENS: f32 = 1.0 / 6842.0; +pub const NR_OF_DATA_AND_CFG_REGISTERS: usize = 14; + +// Register adresses to access various bytes from the raw reply. +pub const X_LOWBYTE_IDX: usize = 9; +pub const Y_LOWBYTE_IDX: usize = 11; +pub const Z_LOWBYTE_IDX: usize = 13; pub trait SpiInterface { type Error; @@ -40,13 +46,53 @@ impl SpiInterface for SpiDummyInterface { type Error = (); fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { - rx[0..2].copy_from_slice(&self.dummy_val_0.to_be_bytes()); - rx[2..4].copy_from_slice(&self.dummy_val_1.to_be_bytes()); - rx[4..6].copy_from_slice(&self.dummy_val_2.to_be_bytes()); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_0.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_1.to_be_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_2.to_be_bytes()); Ok(()) } } +pub struct SpiSimInterface { + pub sim_request_tx: mpsc::Sender, + pub sim_reply_rx: mpsc::Receiver, +} + +impl SpiInterface for SpiSimInterface { + type Error = (); + + fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; + self.sim_request_tx + .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) + .expect("failed to send request"); + self.sim_reply_rx.recv().expect("reply timeout"); + /* + let mgm_req_json = serde_json::to_string(&mgm_sensor_request)?; + self.udp_socket + .send_to(mgm_req_json.as_bytes(), self.sim_addr) + .unwrap(); + */ + Ok(()) + } +} + +pub enum SpiSimInterfaceWrapper { + Dummy(SpiDummyInterface), + Sim(SpiSimInterface), +} + +impl SpiInterface for SpiSimInterfaceWrapper { + type Error = (); + + fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + match self { + SpiSimInterfaceWrapper::Dummy(dummy) => dummy.transfer(tx, rx), + SpiSimInterfaceWrapper::Sim(sim_if) => sim_if.transfer(tx, rx), + } + } +} + #[derive(Default, Debug, Copy, Clone, Serialize, Deserialize)] pub struct MgmData { pub valid: bool, @@ -76,13 +122,13 @@ pub struct MgmHandlerLis3Mdl #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] mode_and_submode: ModeAndSubmode, #[new(default)] - tx_buf: [u8; 12], + tx_buf: [u8; 32], #[new(default)] - rx_buf: [u8; 12], + rx_buf: [u8; 32], #[new(default)] tm_buf: [u8; 16], #[new(default)] - stamp_helper: TimeStampHelper, + stamp_helper: TimestampHelper, } impl MgmHandlerLis3Mdl { @@ -94,17 +140,35 @@ impl MgmHandlerLis3Mdl>); + +pub fn create_sim_client(sim_request_rx: mpsc::Receiver) -> Option { + match SimClientUdp::new( + SocketAddr::V4(SocketAddrV4::new(Ipv4Addr::LOCALHOST, SIM_CTRL_PORT)), + sim_request_rx, + ) { + Ok(sim_client) => { + log::info!("simulator client connection success"); + return Some(sim_client); + } + Err(e) => match e { + SimClientCreationResult::Io(e) => { + log::warn!("creating SIM client failed with io error {}", e); + } + SimClientCreationResult::Timeout => { + log::warn!("timeout when attempting connection to SIM client"); + } + SimClientCreationResult::InvalidPingReply(reply) => { + log::warn!( + "invalid ping reply when attempting connection to SIM client: {}", + reply + ); + } + }, + } + None +} + +#[derive(thiserror::Error, Debug)] +pub enum SimClientCreationResult { + #[error("io error: {0}")] + Io(#[from] std::io::Error), + #[error("timeout when trying to connect to sim UDP server")] + Timeout, + #[error("invalid ping reply when trying connection to UDP sim server")] + InvalidPingReply(#[from] serde_json::Error), +} + +pub struct SimClientUdp { + udp_client: UdpSocket, + simulator_addr: SocketAddr, + sim_request_rx: mpsc::Receiver, + reply_map: SimReplyMap, + reply_buf: [u8; 4096], +} + +impl SimClientUdp { + pub fn new( + simulator_addr: SocketAddr, + sim_request_rx: mpsc::Receiver, + ) -> Result { + let mut reply_buf: [u8; 4096] = [0; 4096]; + let udp_client = UdpSocket::bind("127.0.0.1:0")?; + udp_client.set_read_timeout(Some(Duration::from_millis(100)))?; + let sim_req = SimRequest::new_with_epoch_time(SimCtrlRequest::Ping); + let sim_req_json = serde_json::to_string(&sim_req).expect("failed to serialize SimRequest"); + udp_client.send_to(sim_req_json.as_bytes(), simulator_addr)?; + match udp_client.recv(&mut reply_buf) { + Ok(reply_len) => { + let sim_reply: SimCtrlReply = serde_json::from_slice(&reply_buf[0..reply_len])?; + udp_client.set_read_timeout(None)?; + match sim_reply { + SimCtrlReply::Pong => Ok(Self { + udp_client, + simulator_addr, + sim_request_rx, + reply_map: SimReplyMap(HashMap::new()), + reply_buf, + }), + SimCtrlReply::InvalidRequest(_) => { + panic!("received invalid request reply from UDP sim server") + } + } + } + Err(e) => { + if e.kind() == std::io::ErrorKind::TimedOut + || e.kind() == std::io::ErrorKind::WouldBlock + { + Err(SimClientCreationResult::Timeout) + } else { + Err(SimClientCreationResult::Io(e)) + } + } + } + } + + pub fn operation(&mut self) { + let mut no_sim_requests_handled = true; + let mut no_data_from_udp_server_received = true; + loop { + match self.sim_request_rx.try_recv() { + Ok(request) => { + let request_json = + serde_json::to_string(&request).expect("failed to serialize SimRequest"); + if let Err(e) = self + .udp_client + .send_to(request_json.as_bytes(), self.simulator_addr) + { + log::error!("error sending data to UDP SIM server: {}", e); + break; + } else { + no_sim_requests_handled = false; + } + } + Err(e) => match e { + mpsc::TryRecvError::Empty => { + break; + } + mpsc::TryRecvError::Disconnected => { + log::warn!("SIM request sender disconnected"); + break; + } + }, + } + } + loop { + match self.udp_client.recv(&mut self.reply_buf) { + Ok(recvd_bytes) => { + no_data_from_udp_server_received = false; + let sim_reply_result: serde_json::Result = + serde_json::from_slice(&self.reply_buf[0..recvd_bytes]); + match sim_reply_result { + Ok(sim_reply) => { + if let Some(sender) = self.reply_map.0.get(&sim_reply.component()) { + sender.send(sim_reply).expect("failed to send SIM reply"); + } else { + log::warn!( + "no recipient for SIM reply from component {:?}", + sim_reply.component() + ); + } + } + Err(e) => { + log::warn!("failed to deserialize SIM reply: {}", e); + } + } + } + Err(e) => { + if e.kind() == std::io::ErrorKind::WouldBlock + || e.kind() == std::io::ErrorKind::TimedOut + { + break; + } + log::error!("error receiving data from UDP SIM server: {}", e); + break; + } + } + } + if no_sim_requests_handled && no_data_from_udp_server_received { + std::thread::sleep(Duration::from_millis(5)); + } + } + + pub fn add_reply_recipient( + &mut self, + component: SimComponent, + reply_sender: mpsc::Sender, + ) { + self.reply_map.0.insert(component, reply_sender); + } +} diff --git a/satrs-example/src/lib.rs b/satrs-example/src/lib.rs index a224fe5..889bdc5 100644 --- a/satrs-example/src/lib.rs +++ b/satrs-example/src/lib.rs @@ -9,12 +9,12 @@ pub enum DeviceMode { Normal = 2, } -pub struct TimeStampHelper { +pub struct TimestampHelper { stamper: CdsTime, time_stamp: [u8; 7], } -impl TimeStampHelper { +impl TimestampHelper { pub fn stamp(&self) -> &[u8] { &self.time_stamp } @@ -29,7 +29,7 @@ impl TimeStampHelper { } } -impl Default for TimeStampHelper { +impl Default for TimestampHelper { fn default() -> Self { Self { stamper: CdsTime::now_with_u16_days().expect("creating time stamper failed"), diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 02138c5..bd7ba0b 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -19,12 +19,14 @@ use satrs::hal::std::udp_server::UdpTcServer; use satrs::request::GenericMessage; use satrs::tmtc::{PacketSenderWithSharedPool, SharedPacketPool}; use satrs_example::config::pool::{create_sched_tc_pool, create_static_pools}; -use satrs_example::config::tasks::{ - FREQ_MS_AOCS, FREQ_MS_EVENT_HANDLING, FREQ_MS_PUS_STACK, FREQ_MS_UDP_TMTC, -}; +use satrs_example::config::tasks::{FREQ_MS_AOCS, FREQ_MS_PUS_STACK, FREQ_MS_UDP_TMTC}; use satrs_example::config::{OBSW_SERVER_ADDR, PACKET_ID_VALIDATOR, SERVER_PORT}; -use crate::acs::mgm::{MgmHandlerLis3Mdl, MpscModeLeafInterface, SpiDummyInterface}; +use crate::acs::mgm::{ + MgmHandlerLis3Mdl, MpscModeLeafInterface, SpiDummyInterface, SpiSimInterface, + SpiSimInterfaceWrapper, +}; +use crate::interface::sim_client_udp::create_sim_client; use crate::interface::tcp::{SyncTcpTmSource, TcpTask}; use crate::interface::udp::{StaticUdpTmHandler, UdpTmtcServer}; use crate::logger::setup_logger; @@ -60,6 +62,10 @@ fn static_tmtc_pool_main() { let tm_sink_tx_sender = PacketSenderWithSharedPool::new(tm_sink_tx.clone(), shared_tm_pool_wrapper.clone()); + let (sim_request_tx, sim_request_rx) = mpsc::channel(); + let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); + let mut opt_sim_client = create_sim_client(sim_request_rx); + let (mgm_handler_composite_tx, mgm_handler_composite_rx) = mpsc::channel::>(); let (mgm_handler_mode_tx, mgm_handler_mode_rx) = mpsc::channel::>(); @@ -197,13 +203,22 @@ fn static_tmtc_pool_main() { let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = mpsc::channel(); - let dummy_spi_interface = SpiDummyInterface::default(); let shared_mgm_set = Arc::default(); let mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, reply_tx_to_pus: pus_mode_reply_tx, reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx, }; + + let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() { + sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx); + SpiSimInterfaceWrapper::Sim(SpiSimInterface { + sim_request_tx: sim_request_tx.clone(), + sim_reply_rx: mgm_sim_reply_rx, + }) + } else { + SpiSimInterfaceWrapper::Dummy(SpiDummyInterface::default()) + }; let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", @@ -211,7 +226,7 @@ fn static_tmtc_pool_main() { mgm_handler_composite_rx, pus_hk_reply_tx, tm_sink_tx, - dummy_spi_interface, + mgm_spi_interface, shared_mgm_set, ); @@ -247,14 +262,18 @@ fn static_tmtc_pool_main() { }) .unwrap(); - info!("Starting event handling task"); - let jh_event_handling = thread::Builder::new() - .name("sat-rs events".to_string()) - .spawn(move || loop { - event_handler.periodic_operation(); - thread::sleep(Duration::from_millis(FREQ_MS_EVENT_HANDLING)); - }) - .unwrap(); + let mut opt_jh_sim_client = None; + if let Some(mut sim_client) = opt_sim_client { + info!("Starting UDP sim client task"); + opt_jh_sim_client = Some( + thread::Builder::new() + .name("sat-rs sim adapter".to_string()) + .spawn(move || loop { + sim_client.operation(); + }) + .unwrap(), + ); + } info!("Starting AOCS thread"); let jh_aocs = thread::Builder::new() @@ -269,6 +288,7 @@ fn static_tmtc_pool_main() { let jh_pus_handler = thread::Builder::new() .name("sat-rs pus".to_string()) .spawn(move || loop { + event_handler.periodic_operation(); pus_stack.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_PUS_STACK)); }) @@ -283,9 +303,11 @@ fn static_tmtc_pool_main() { jh_tm_funnel .join() .expect("Joining TM Funnel thread failed"); - jh_event_handling - .join() - .expect("Joining Event Manager thread failed"); + if let Some(jh_sim_client) = opt_jh_sim_client { + jh_sim_client + .join() + .expect("Joining SIM client thread failed"); + } jh_aocs.join().expect("Joining AOCS thread failed"); jh_pus_handler .join() @@ -298,6 +320,10 @@ fn dyn_tmtc_pool_main() { let (tm_funnel_tx, tm_funnel_rx) = mpsc::channel(); let (tm_server_tx, tm_server_rx) = mpsc::channel(); + let (sim_request_tx, sim_request_rx) = mpsc::channel(); + let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); + let mut opt_sim_client = create_sim_client(sim_request_rx); + // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let (mgm_handler_composite_tx, mgm_handler_composite_rx) = mpsc::channel::>(); @@ -414,13 +440,22 @@ fn dyn_tmtc_pool_main() { let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = mpsc::channel(); - let dummy_spi_interface = SpiDummyInterface::default(); let shared_mgm_set = Arc::default(); let mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, reply_tx_to_pus: pus_mode_reply_tx, reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx, }; + + let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() { + sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx); + SpiSimInterfaceWrapper::Sim(SpiSimInterface { + sim_request_tx: sim_request_tx.clone(), + sim_reply_rx: mgm_sim_reply_rx, + }) + } else { + SpiSimInterfaceWrapper::Dummy(SpiDummyInterface::default()) + }; let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", @@ -428,7 +463,7 @@ fn dyn_tmtc_pool_main() { mgm_handler_composite_rx, pus_hk_reply_tx, tm_funnel_tx, - dummy_spi_interface, + mgm_spi_interface, shared_mgm_set, ); @@ -464,14 +499,18 @@ fn dyn_tmtc_pool_main() { }) .unwrap(); - info!("Starting event handling task"); - let jh_event_handling = thread::Builder::new() - .name("sat-rs events".to_string()) - .spawn(move || loop { - event_handler.periodic_operation(); - thread::sleep(Duration::from_millis(FREQ_MS_EVENT_HANDLING)); - }) - .unwrap(); + let mut opt_jh_sim_client = None; + if let Some(mut sim_client) = opt_sim_client { + info!("Starting UDP sim client task"); + opt_jh_sim_client = Some( + thread::Builder::new() + .name("sat-rs sim adapter".to_string()) + .spawn(move || loop { + sim_client.operation(); + }) + .unwrap(), + ); + } info!("Starting AOCS thread"); let jh_aocs = thread::Builder::new() @@ -487,6 +526,7 @@ fn dyn_tmtc_pool_main() { .name("sat-rs pus".to_string()) .spawn(move || loop { pus_stack.periodic_operation(); + event_handler.periodic_operation(); thread::sleep(Duration::from_millis(FREQ_MS_PUS_STACK)); }) .unwrap(); @@ -500,9 +540,11 @@ fn dyn_tmtc_pool_main() { jh_tm_funnel .join() .expect("Joining TM Funnel thread failed"); - jh_event_handling - .join() - .expect("Joining Event Manager thread failed"); + if let Some(jh_sim_client) = opt_jh_sim_client { + jh_sim_client + .join() + .expect("Joining SIM client thread failed"); + } jh_aocs.join().expect("Joining AOCS thread failed"); jh_pus_handler .join() diff --git a/satrs-example/src/pus/mod.rs b/satrs-example/src/pus/mod.rs index f305308..b2e7f8a 100644 --- a/satrs-example/src/pus/mod.rs +++ b/satrs-example/src/pus/mod.rs @@ -19,7 +19,7 @@ use satrs::tmtc::{PacketAsVec, PacketInPool}; use satrs::ComponentId; use satrs_example::config::components::PUS_ROUTING_SERVICE; use satrs_example::config::{tmtc_err, CustomPusServiceId}; -use satrs_example::TimeStampHelper; +use satrs_example::TimestampHelper; use std::fmt::Debug; use std::sync::mpsc::{self, Sender}; @@ -53,7 +53,7 @@ pub struct PusTcDistributor { pub tm_sender: TmSender, pub verif_reporter: VerificationReporter, pub pus_router: PusTcMpscRouter, - stamp_helper: TimeStampHelper, + stamp_helper: TimestampHelper, } impl PusTcDistributor { @@ -66,7 +66,7 @@ impl PusTcDistributor { PUS_ROUTING_SERVICE.apid, ), pus_router, - stamp_helper: TimeStampHelper::default(), + stamp_helper: TimestampHelper::default(), } } diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 77b8cb0..e85ebf3 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -6,14 +6,17 @@ use asynchronix::{ }; use satrs::power::SwitchStateBinary; use satrs_minisim::{ - acs::{MgmReply, MgmSensorValues, MgtDipole, MgtHkSet, MgtReply, MGT_GEN_MAGNETIC_FIELD}, + acs::{ + lis3mdl::MgmLis3MdlReply, MgmReplyCommon, MgmReplyProvider, MgmSensorValuesMicroTesla, + MgtDipole, MgtHkSet, MgtReply, MGT_GEN_MAGNETIC_FIELD, + }, SimReply, }; use crate::time::current_millis; // Earth magnetic field varies between -30 uT and 30 uT -const AMPLITUDE_MGM: f32 = 0.03; +const AMPLITUDE_MGM_UT: f32 = 30.0; // Lets start with a simple frequency here. const FREQUENCY_MGM: f32 = 1.0; const PHASE_X: f32 = 0.0; @@ -31,30 +34,34 @@ const PHASE_Z: f32 = 0.2; /// 2. It would sample the magnetic field at a high fixed rate. This might not be possible for /// a general purpose OS, but self self-sampling at a relatively high rate (20-40 ms) might /// stil lbe possible. -pub struct MagnetometerModel { +pub struct MagnetometerModel { pub switch_state: SwitchStateBinary, pub periodicity: Duration, - pub external_mag_field: Option, + pub external_mag_field: Option, pub reply_sender: mpsc::Sender, + pub phatom: std::marker::PhantomData, } -impl MagnetometerModel { - pub fn new(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { +impl MagnetometerModel { + pub fn new_for_lis3mdl(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { Self { switch_state: SwitchStateBinary::Off, periodicity, external_mag_field: None, reply_sender, + phatom: std::marker::PhantomData, } } +} +impl MagnetometerModel { pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { self.switch_state = switch_state; } pub async fn send_sensor_values(&mut self, _: (), scheduler: &Scheduler) { self.reply_sender - .send(SimReply::new(MgmReply { + .send(ReplyProvider::create_mgm_reply(MgmReplyCommon { switch_state: self.switch_state, sensor_values: self.calculate_current_mgm_tuple(current_millis(scheduler.time())), })) @@ -63,23 +70,23 @@ impl MagnetometerModel { // Devices like magnetorquers generate a strong magnetic field which overrides the default // model for the measured magnetic field. - pub async fn apply_external_magnetic_field(&mut self, field: MgmSensorValues) { + pub async fn apply_external_magnetic_field(&mut self, field: MgmSensorValuesMicroTesla) { self.external_mag_field = Some(field); } - fn calculate_current_mgm_tuple(&self, time_ms: u64) -> MgmSensorValues { + fn calculate_current_mgm_tuple(&self, time_ms: u64) -> MgmSensorValuesMicroTesla { if SwitchStateBinary::On == self.switch_state { if let Some(ext_field) = self.external_mag_field { return ext_field; } let base_sin_val = 2.0 * PI * FREQUENCY_MGM * (time_ms as f32 / 1000.0); - return MgmSensorValues { - x: AMPLITUDE_MGM * (base_sin_val + PHASE_X).sin(), - y: AMPLITUDE_MGM * (base_sin_val + PHASE_Y).sin(), - z: AMPLITUDE_MGM * (base_sin_val + PHASE_Z).sin(), + return MgmSensorValuesMicroTesla { + x: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_X).sin(), + y: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Y).sin(), + z: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Z).sin(), }; } - MgmSensorValues { + MgmSensorValuesMicroTesla { x: 0.0, y: 0.0, z: 0.0, @@ -87,13 +94,13 @@ impl MagnetometerModel { } } -impl Model for MagnetometerModel {} +impl Model for MagnetometerModel {} pub struct MagnetorquerModel { switch_state: SwitchStateBinary, torquing: bool, torque_dipole: MgtDipole, - pub gen_magnetic_field: Output, + pub gen_magnetic_field: Output, reply_sender: mpsc::Sender, } @@ -153,7 +160,7 @@ impl MagnetorquerModel { .unwrap(); } - fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValues { + fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValuesMicroTesla { // Simplified model: Just returns some fixed magnetic field for now. // Later, we could make this more fancy by incorporating the commanded dipole. MGT_GEN_MAGNETIC_FIELD @@ -179,7 +186,10 @@ pub mod tests { use satrs::power::SwitchStateBinary; use satrs_minisim::{ - acs::{MgmReply, MgmRequest, MgtDipole, MgtHkSet, MgtReply, MgtRequest}, + acs::{ + lis3mdl::{self, MgmLis3MdlReply}, + MgmRequest, MgtDipole, MgtHkSet, MgtReply, MgtRequest, + }, eps::PcduSwitch, SerializableSimMsgPayload, SimMessageProvider, SimRequest, SimTarget, }; @@ -198,13 +208,13 @@ pub mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Mgm); - let reply = MgmReply::from_sim_message(&sim_reply) + assert_eq!(sim_reply.component(), SimTarget::MgmLis3Mdl); + let reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); - assert_eq!(reply.switch_state, SwitchStateBinary::Off); - assert_eq!(reply.sensor_values.x, 0.0); - assert_eq!(reply.sensor_values.y, 0.0); - assert_eq!(reply.sensor_values.z, 0.0); + assert_eq!(reply.common.switch_state, SwitchStateBinary::Off); + assert_eq!(reply.common.sensor_values.x, 0.0); + assert_eq!(reply.common.sensor_values.y, 0.0); + assert_eq!(reply.common.sensor_values.z, 0.0); } #[test] @@ -221,8 +231,8 @@ pub mod tests { let mut sim_reply_res = sim_testbench.try_receive_next_reply(); assert!(sim_reply_res.is_some()); let mut sim_reply = sim_reply_res.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Mgm); - let first_reply = MgmReply::from_sim_message(&sim_reply) + assert_eq!(sim_reply.component(), SimTarget::MgmLis3Mdl); + let first_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); sim_testbench.step_by(Duration::from_millis(50)); @@ -236,8 +246,24 @@ pub mod tests { assert!(sim_reply_res.is_some()); sim_reply = sim_reply_res.unwrap(); - let second_reply = MgmReply::from_sim_message(&sim_reply) + let second_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); + let x_conv_back = second_reply.raw.x as f32 + * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS + * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; + let y_conv_back = second_reply.raw.y as f32 + * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS + * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; + let z_conv_back = second_reply.raw.z as f32 + * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS + * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; + let diff_x = (second_reply.common.sensor_values.x - x_conv_back).abs(); + assert!(diff_x < 0.01, "diff x too large: {}", diff_x); + let diff_y = (second_reply.common.sensor_values.y - y_conv_back).abs(); + assert!(diff_y < 0.01, "diff y too large: {}", diff_y); + let diff_z = (second_reply.common.sensor_values.z - z_conv_back).abs(); + assert!(diff_z < 0.01, "diff z too large: {}", diff_z); + // assert_eq!(second_reply.raw_reply, SwitchStateBinary::On); // Check that the values are changing. assert!(first_reply != second_reply); } diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs index 9255932..c3196af 100644 --- a/satrs-minisim/src/controller.rs +++ b/satrs-minisim/src/controller.rs @@ -5,10 +5,10 @@ use asynchronix::{ time::{Clock, MonotonicTime, SystemClock}, }; use satrs_minisim::{ - acs::{MgmRequest, MgtRequest}, + acs::{lis3mdl::MgmLis3MdlReply, MgmRequestLis3Mdl, MgtRequest}, eps::PcduRequest, - SerializableSimMsgPayload, SimCtrlReply, SimCtrlRequest, SimMessageProvider, SimReply, - SimRequest, SimRequestError, SimTarget, + SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, + SimReply, SimRequest, SimRequestError, }; use crate::{ @@ -22,7 +22,7 @@ pub struct SimController { pub request_receiver: mpsc::Receiver, pub reply_sender: mpsc::Sender, pub simulation: Simulation, - pub mgm_addr: Address, + pub mgm_addr: Address>, pub pcdu_addr: Address, pub mgt_addr: Address, } @@ -33,7 +33,7 @@ impl SimController { request_receiver: mpsc::Receiver, reply_sender: mpsc::Sender, simulation: Simulation, - mgm_addr: Address, + mgm_addr: Address>, pcdu_addr: Address, mgt_addr: Address, ) -> Self { @@ -70,11 +70,11 @@ impl SimController { if request.timestamp < old_timestamp { log::warn!("stale data with timestamp {:?} received", request.timestamp); } - if let Err(e) = match request.target() { - SimTarget::SimCtrl => self.handle_ctrl_request(&request), - SimTarget::Mgm => self.handle_mgm_request(&request), - SimTarget::Mgt => self.handle_mgt_request(&request), - SimTarget::Pcdu => self.handle_pcdu_request(&request), + if let Err(e) = match request.component() { + SimComponent::SimCtrl => self.handle_ctrl_request(&request), + SimComponent::MgmLis3Mdl => self.handle_mgm_request(&request), + SimComponent::Mgt => self.handle_mgt_request(&request), + SimComponent::Pcdu => self.handle_pcdu_request(&request), } { self.handle_invalid_request_with_valid_target(e, &request) } @@ -100,10 +100,11 @@ impl SimController { } Ok(()) } + fn handle_mgm_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { - let mgm_request = MgmRequest::from_sim_message(request)?; + let mgm_request = MgmRequestLis3Mdl::from_sim_message(request)?; match mgm_request { - MgmRequest::RequestSensorData => { + MgmRequestLis3Mdl::RequestSensorData => { self.simulation.send_event( MagnetometerModel::send_sensor_values, (), @@ -156,7 +157,7 @@ impl SimController { ) { log::warn!( "received invalid {:?} request: {:?}", - request.target(), + request.component(), error ); self.reply_sender @@ -183,7 +184,7 @@ mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::SimCtrl); + assert_eq!(sim_reply.component(), SimComponent::SimCtrl); let reply = SimCtrlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); assert_eq!(reply, SimCtrlReply::Pong); diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs index ebbeb4e..07c5c4e 100644 --- a/satrs-minisim/src/eps.rs +++ b/satrs-minisim/src/eps.rs @@ -76,7 +76,7 @@ pub(crate) mod tests { use std::time::Duration; use satrs_minisim::{ - eps::PcduRequest, SerializableSimMsgPayload, SimMessageProvider, SimRequest, SimTarget, + eps::PcduRequest, SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, }; use crate::test_helpers::SimTestbench; @@ -122,7 +122,7 @@ pub(crate) mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Pcdu); + assert_eq!(sim_reply.component(), SimComponent::Pcdu); let pcdu_reply = PcduReply::from_sim_message(&sim_reply) .expect("failed to deserialize PCDU switch info"); match pcdu_reply { @@ -157,7 +157,7 @@ pub(crate) mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.target(), SimTarget::Pcdu); + assert_eq!(sim_reply.component(), SimComponent::Pcdu); let pcdu_reply = PcduReply::from_sim_message(&sim_reply) .expect("failed to deserialize PCDU switch info"); match pcdu_reply { diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 2762326..92c73c3 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -1,19 +1,17 @@ use asynchronix::time::MonotonicTime; use serde::{de::DeserializeOwned, Deserialize, Serialize}; -pub const SIM_CTRL_UDP_PORT: u16 = 7303; - -#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] -pub enum SimTarget { +#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash)] +pub enum SimComponent { SimCtrl, - Mgm, + MgmLis3Mdl, Mgt, Pcdu, } #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct SimMessage { - pub target: SimTarget, + pub target: SimComponent, pub payload: String, } @@ -37,10 +35,10 @@ pub enum SimMessageType { pub trait SerializableSimMsgPayload: Serialize + DeserializeOwned + Sized { - const TARGET: SimTarget; + const TARGET: SimComponent; fn from_sim_message(sim_message: &P) -> Result> { - if sim_message.target() == Self::TARGET { + if sim_message.component() == Self::TARGET { return Ok(serde_json::from_str(sim_message.payload())?); } Err(SimMessageError::TargetRequestMissmatch(sim_message.clone())) @@ -49,7 +47,7 @@ pub trait SerializableSimMsgPayload: pub trait SimMessageProvider: Serialize + DeserializeOwned + Clone + Sized { fn msg_type(&self) -> SimMessageType; - fn target(&self) -> SimTarget; + fn component(&self) -> SimComponent; fn payload(&self) -> &String; fn from_raw_data(data: &[u8]) -> serde_json::Result { serde_json::from_slice(data) @@ -78,7 +76,7 @@ impl SimRequest { } impl SimMessageProvider for SimRequest { - fn target(&self) -> SimTarget { + fn component(&self) -> SimComponent { self.inner.target } fn payload(&self) -> &String { @@ -109,7 +107,7 @@ impl SimReply { } impl SimMessageProvider for SimReply { - fn target(&self) -> SimTarget { + fn component(&self) -> SimComponent { self.inner.target } fn payload(&self) -> &String { @@ -126,7 +124,7 @@ pub enum SimCtrlRequest { } impl SerializableSimMsgPayload for SimCtrlRequest { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } pub type SimReplyError = SimMessageError; @@ -151,7 +149,7 @@ pub enum SimCtrlReply { } impl SerializableSimMsgPayload for SimCtrlReply { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } impl From for SimCtrlReply { @@ -184,7 +182,7 @@ pub mod eps { } impl SerializableSimMsgPayload for PcduRequest { - const TARGET: SimTarget = SimTarget::Pcdu; + const TARGET: SimComponent = SimComponent::Pcdu; } #[derive(Debug, Clone, Serialize, Deserialize)] @@ -193,7 +191,7 @@ pub mod eps { } impl SerializableSimMsgPayload for PcduReply { - const TARGET: SimTarget = SimTarget::Pcdu; + const TARGET: SimComponent = SimComponent::Pcdu; } } @@ -204,41 +202,104 @@ pub mod acs { use super::*; + pub trait MgmReplyProvider: Send + 'static { + fn create_mgm_reply(common: MgmReplyCommon) -> SimReply; + } + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] - pub enum MgmRequest { + pub enum MgmRequestLis3Mdl { RequestSensorData, } - impl SerializableSimMsgPayload for MgmRequest { - const TARGET: SimTarget = SimTarget::Mgm; + impl SerializableSimMsgPayload for MgmRequestLis3Mdl { + const TARGET: SimComponent = SimComponent::MgmLis3Mdl; } // Normally, small magnetometers generate their output as a signed 16 bit raw format or something // similar which needs to be converted to a signed float value with physical units. We will - // simplify this now and generate the signed float values directly. + // simplify this now and generate the signed float values directly. The unit is micro tesla. #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmSensorValues { + pub struct MgmSensorValuesMicroTesla { pub x: f32, pub y: f32, pub z: f32, } #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmReply { + pub struct MgmReplyCommon { pub switch_state: SwitchStateBinary, - pub sensor_values: MgmSensorValues, + pub sensor_values: MgmSensorValuesMicroTesla, } - impl SerializableSimMsgPayload for MgmReply { - const TARGET: SimTarget = SimTarget::Mgm; - } - - pub const MGT_GEN_MAGNETIC_FIELD: MgmSensorValues = MgmSensorValues { - x: 0.03, - y: -0.03, - z: 0.03, + pub const MGT_GEN_MAGNETIC_FIELD: MgmSensorValuesMicroTesla = MgmSensorValuesMicroTesla { + x: 30.0, + y: -30.0, + z: 30.0, }; + pub mod lis3mdl { + use super::*; + + // Field data register scaling + pub const GAUSS_TO_MICROTESLA_FACTOR: u32 = 100; + pub const FIELD_LSB_PER_GAUSS_4_SENS: f32 = 1.0 / 6842.0; + pub const FIELD_LSB_PER_GAUSS_8_SENS: f32 = 1.0 / 3421.0; + pub const FIELD_LSB_PER_GAUSS_12_SENS: f32 = 1.0 / 2281.0; + pub const FIELD_LSB_PER_GAUSS_16_SENS: f32 = 1.0 / 1711.0; + + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgmLis3RawValues { + pub x: i16, + pub y: i16, + pub z: i16, + } + + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgmLis3MdlReply { + pub common: MgmReplyCommon, + // Raw sensor values which are transmitted by the LIS3 device in little-endian + // order. + pub raw: MgmLis3RawValues, + } + + impl MgmLis3MdlReply { + pub fn new(common: MgmReplyCommon) -> Self { + let mut raw_reply: [u8; 7] = [0; 7]; + let raw_x: i16 = (common.sensor_values.x + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_y: i16 = (common.sensor_values.y + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_z: i16 = (common.sensor_values.z + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + // The first byte is a dummy byte. + raw_reply[1..3].copy_from_slice(&raw_x.to_be_bytes()); + raw_reply[3..5].copy_from_slice(&raw_y.to_be_bytes()); + raw_reply[5..7].copy_from_slice(&raw_z.to_be_bytes()); + Self { + common, + raw: MgmLis3RawValues { + x: raw_x, + y: raw_y, + z: raw_z, + }, + } + } + } + + impl SerializableSimMsgPayload for MgmLis3MdlReply { + const TARGET: SimComponent = SimComponent::MgmLis3Mdl; + } + + impl MgmReplyProvider for MgmLis3MdlReply { + fn create_mgm_reply(common: MgmReplyCommon) -> SimReply { + SimReply::new(Self::new(common)) + } + } + } + // Simple model using i16 values. #[derive(Default, Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct MgtDipole { @@ -262,7 +323,7 @@ pub mod acs { } impl SerializableSimMsgPayload for MgtRequest { - const TARGET: SimTarget = SimTarget::Mgt; + const TARGET: SimComponent = SimComponent::Mgt; } #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -279,78 +340,12 @@ pub mod acs { } impl SerializableSimMsgPayload for MgtReply { - const TARGET: SimTarget = SimTarget::Mgm; + const TARGET: SimComponent = SimComponent::MgmLis3Mdl; } } pub mod udp { - use std::{ - net::{SocketAddr, UdpSocket}, - time::Duration, - }; - - use thiserror::Error; - - use crate::{SimReply, SimRequest}; - - #[derive(Error, Debug)] - pub enum ReceptionError { - #[error("IO error: {0}")] - Io(#[from] std::io::Error), - #[error("Serde JSON error: {0}")] - SerdeJson(#[from] serde_json::Error), - } - - pub struct SimUdpClient { - socket: UdpSocket, - pub reply_buf: [u8; 4096], - } - - impl SimUdpClient { - pub fn new( - server_addr: &SocketAddr, - non_blocking: bool, - read_timeot_ms: Option, - ) -> std::io::Result { - let socket = UdpSocket::bind("127.0.0.1:0")?; - socket.set_nonblocking(non_blocking)?; - socket - .connect(server_addr) - .expect("could not connect to server addr"); - if let Some(read_timeout) = read_timeot_ms { - // Set a read timeout so the test does not hang on failures. - socket.set_read_timeout(Some(Duration::from_millis(read_timeout)))?; - } - Ok(Self { - socket, - reply_buf: [0; 4096], - }) - } - - pub fn set_nonblocking(&self, non_blocking: bool) -> std::io::Result<()> { - self.socket.set_nonblocking(non_blocking) - } - - pub fn set_read_timeout(&self, read_timeout_ms: u64) -> std::io::Result<()> { - self.socket - .set_read_timeout(Some(Duration::from_millis(read_timeout_ms))) - } - - pub fn send_request(&self, sim_request: &SimRequest) -> std::io::Result { - self.socket.send( - &serde_json::to_vec(sim_request).expect("conversion of request to vector failed"), - ) - } - - pub fn recv_raw(&mut self) -> std::io::Result { - self.socket.recv(&mut self.reply_buf) - } - - pub fn recv_sim_reply(&mut self) -> Result { - let read_len = self.recv_raw()?; - Ok(serde_json::from_slice(&self.reply_buf[0..read_len])?) - } - } + pub const SIM_CTRL_PORT: u16 = 7303; } #[cfg(test)] @@ -363,7 +358,7 @@ pub mod tests { } impl SerializableSimMsgPayload for DummyRequest { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -372,13 +367,13 @@ pub mod tests { } impl SerializableSimMsgPayload for DummyReply { - const TARGET: SimTarget = SimTarget::SimCtrl; + const TARGET: SimComponent = SimComponent::SimCtrl; } #[test] fn test_basic_request() { let sim_request = SimRequest::new_with_epoch_time(DummyRequest::Ping); - assert_eq!(sim_request.target(), SimTarget::SimCtrl); + assert_eq!(sim_request.component(), SimComponent::SimCtrl); assert_eq!(sim_request.msg_type(), SimMessageType::Request); let dummy_request = DummyRequest::from_sim_message(&sim_request).expect("deserialization failed"); @@ -388,7 +383,7 @@ pub mod tests { #[test] fn test_basic_reply() { let sim_reply = SimReply::new(DummyReply::Pong); - assert_eq!(sim_reply.target(), SimTarget::SimCtrl); + assert_eq!(sim_reply.component(), SimComponent::SimCtrl); assert_eq!(sim_reply.msg_type(), SimMessageType::Reply); let dummy_request = DummyReply::from_sim_message(&sim_reply).expect("deserialization failed"); diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index 59701f7..4b6c240 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -3,7 +3,8 @@ use asynchronix::simulation::{Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, SystemClock}; use controller::SimController; use eps::PcduModel; -use satrs_minisim::{SimReply, SimRequest, SIM_CTRL_UDP_PORT}; +use satrs_minisim::udp::SIM_CTRL_PORT; +use satrs_minisim::{SimReply, SimRequest}; use std::sync::mpsc; use std::thread; use std::time::{Duration, SystemTime}; @@ -30,7 +31,8 @@ fn create_sim_controller( request_receiver: mpsc::Receiver, ) -> SimController { // Instantiate models and their mailboxes. - let mgm_model = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); + let mgm_model = + MagnetometerModel::new_for_lis3mdl(Duration::from_millis(50), reply_sender.clone()); let mgm_mailbox = Mailbox::new(); let mgm_addr = mgm_mailbox.address(); @@ -112,9 +114,9 @@ fn main() { }); let mut udp_server = - SimUdpServer::new(SIM_CTRL_UDP_PORT, request_sender, reply_receiver, 200, None) + SimUdpServer::new(SIM_CTRL_PORT, request_sender, reply_receiver, 200, None) .expect("could not create UDP request server"); - log::info!("starting UDP server on port {}", SIM_CTRL_UDP_PORT); + log::info!("starting UDP server on port {}", SIM_CTRL_PORT); // This thread manages the simulator UDP server. let udp_tc_thread = thread::spawn(move || { udp_server.run(); From fe4126f7e21a6e27183612e4de6d79f2ad36def9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 May 2024 20:55:56 +0200 Subject: [PATCH 02/23] first connection success --- satrs-example/src/acs/mgm.rs | 10 ++---- satrs-example/src/interface/sim_client_udp.rs | 35 +++++++++---------- satrs-minisim/src/controller.rs | 17 +++++++++ satrs-minisim/src/lib.rs | 2 +- 4 files changed, 38 insertions(+), 26 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 2445dd6..3f7edf3 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -61,18 +61,14 @@ pub struct SpiSimInterface { impl SpiInterface for SpiSimInterface { type Error = (); - fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + // 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> { let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; self.sim_request_tx .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) .expect("failed to send request"); self.sim_reply_rx.recv().expect("reply timeout"); - /* - let mgm_req_json = serde_json::to_string(&mgm_sensor_request)?; - self.udp_socket - .send_to(mgm_req_json.as_bytes(), self.sim_addr) - .unwrap(); - */ + // TODO: Write the sensor data to the raw buffer. Ok(()) } } diff --git a/satrs-example/src/interface/sim_client_udp.rs b/satrs-example/src/interface/sim_client_udp.rs index fe596dc..0da3c3e 100644 --- a/satrs-example/src/interface/sim_client_udp.rs +++ b/satrs-example/src/interface/sim_client_udp.rs @@ -5,7 +5,10 @@ use std::{ time::Duration, }; -use satrs_minisim::{udp::SIM_CTRL_PORT, SimComponent, SimMessageProvider, SimReply, SimRequest}; +use satrs_minisim::{ + udp::SIM_CTRL_PORT, SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimReply, + SimRequest, +}; use satrs_minisim::{SimCtrlReply, SimCtrlRequest}; struct SimReplyMap(pub HashMap>); @@ -19,20 +22,9 @@ pub fn create_sim_client(sim_request_rx: mpsc::Receiver) -> Option match e { - SimClientCreationResult::Io(e) => { - log::warn!("creating SIM client failed with io error {}", e); - } - SimClientCreationResult::Timeout => { - log::warn!("timeout when attempting connection to SIM client"); - } - SimClientCreationResult::InvalidPingReply(reply) => { - log::warn!( - "invalid ping reply when attempting connection to SIM client: {}", - reply - ); - } - }, + Err(e) => { + log::warn!("sim client creation error: {}", e); + } } None } @@ -44,7 +36,9 @@ pub enum SimClientCreationResult { #[error("timeout when trying to connect to sim UDP server")] Timeout, #[error("invalid ping reply when trying connection to UDP sim server")] - InvalidPingReply(#[from] serde_json::Error), + InvalidReplyJsonError(#[from] serde_json::Error), + #[error("invalid sim reply, not pong reply as expected: {0:?}")] + ReplyIsNotPong(SimReply), } pub struct SimClientUdp { @@ -68,9 +62,14 @@ impl SimClientUdp { udp_client.send_to(sim_req_json.as_bytes(), simulator_addr)?; match udp_client.recv(&mut reply_buf) { Ok(reply_len) => { - let sim_reply: SimCtrlReply = serde_json::from_slice(&reply_buf[0..reply_len])?; + let sim_reply: SimReply = serde_json::from_slice(&reply_buf[0..reply_len])?; + if sim_reply.component() != SimComponent::SimCtrl { + return Err(SimClientCreationResult::ReplyIsNotPong(sim_reply)); + } udp_client.set_read_timeout(None)?; - match sim_reply { + let sim_ctrl_reply = + SimCtrlReply::from_sim_message(&sim_reply).expect("invalid SIM reply"); + match sim_ctrl_reply { SimCtrlReply::Pong => Ok(Self { udp_client, simulator_addr, diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs index c3196af..09d2772 100644 --- a/satrs-minisim/src/controller.rs +++ b/satrs-minisim/src/controller.rs @@ -16,6 +16,11 @@ use crate::{ eps::PcduModel, }; +const SIM_CTRL_REQ_WIRETAPPING: bool = true; +const MGM_REQ_WIRETAPPING: bool = true; +const PCDU_REQ_WIRETAPPING: bool = true; +const MGT_REQ_WIRETAPPING: bool = true; + // The simulation controller processes requests and drives the simulation. pub struct SimController { pub sys_clock: SystemClock, @@ -91,6 +96,9 @@ impl SimController { fn handle_ctrl_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let sim_ctrl_request = SimCtrlRequest::from_sim_message(request)?; + if SIM_CTRL_REQ_WIRETAPPING { + log::info!("received sim ctrl request: {:?}", sim_ctrl_request); + } match sim_ctrl_request { SimCtrlRequest::Ping => { self.reply_sender @@ -103,6 +111,9 @@ impl SimController { fn handle_mgm_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let mgm_request = MgmRequestLis3Mdl::from_sim_message(request)?; + if MGM_REQ_WIRETAPPING { + log::info!("received MGM request: {:?}", mgm_request); + } match mgm_request { MgmRequestLis3Mdl::RequestSensorData => { self.simulation.send_event( @@ -117,6 +128,9 @@ impl SimController { fn handle_pcdu_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let pcdu_request = PcduRequest::from_sim_message(request)?; + if PCDU_REQ_WIRETAPPING { + log::info!("received PCDU request: {:?}", pcdu_request); + } match pcdu_request { PcduRequest::RequestSwitchInfo => { self.simulation @@ -135,6 +149,9 @@ impl SimController { fn handle_mgt_request(&mut self, request: &SimRequest) -> Result<(), SimRequestError> { let mgt_request = MgtRequest::from_sim_message(request)?; + if MGT_REQ_WIRETAPPING { + log::info!("received MGT request: {:?}", mgt_request); + } match mgt_request { MgtRequest::ApplyTorque { duration, dipole } => self.simulation.send_event( MagnetorquerModel::apply_torque, diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 92c73c3..1f0f1b6 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -89,7 +89,7 @@ impl SimMessageProvider for SimRequest { } /// A generic simulation reply type. Right now, the payload data is expected to be -/// JSON, which might be changed inthe future. +/// JSON, which might be changed in the future. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct SimReply { inner: SimMessage, From b86c2eb1d1bc13390145657080fb2d2b85e33e62 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 May 2024 21:02:16 +0200 Subject: [PATCH 03/23] added some test stubs --- satrs-example/src/acs/mgm.rs | 5 +++++ satrs-example/src/config.rs | 1 + satrs-example/src/interface/sim_client_udp.rs | 13 +++++++++++-- satrs-example/src/main.rs | 13 ++++++++++--- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 3f7edf3..81bb1f7 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -340,3 +340,8 @@ impl ModeRequestHandler Ok(()) } } + +#[cfg(test)] +mod tests { + // TODO: Add some basic tests for the modes of the device. +} diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs index deacd12..9e3ec6f 100644 --- a/satrs-example/src/config.rs +++ b/satrs-example/src/config.rs @@ -226,4 +226,5 @@ pub mod tasks { pub const FREQ_MS_UDP_TMTC: u64 = 200; pub const FREQ_MS_AOCS: u64 = 500; pub const FREQ_MS_PUS_STACK: u64 = 200; + pub const SIM_CLIENT_IDLE_DELAY_MS: u64 = 5; } diff --git a/satrs-example/src/interface/sim_client_udp.rs b/satrs-example/src/interface/sim_client_udp.rs index 0da3c3e..d16adfd 100644 --- a/satrs-example/src/interface/sim_client_udp.rs +++ b/satrs-example/src/interface/sim_client_udp.rs @@ -5,6 +5,7 @@ use std::{ time::Duration, }; +use satrs::pus::HandlingStatus; use satrs_minisim::{ udp::SIM_CTRL_PORT, SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimReply, SimRequest, @@ -94,7 +95,7 @@ impl SimClientUdp { } } - pub fn operation(&mut self) { + pub fn operation(&mut self) -> HandlingStatus { let mut no_sim_requests_handled = true; let mut no_data_from_udp_server_received = true; loop { @@ -157,8 +158,9 @@ impl SimClientUdp { } } if no_sim_requests_handled && no_data_from_udp_server_received { - std::thread::sleep(Duration::from_millis(5)); + return HandlingStatus::Empty; } + HandlingStatus::HandledOne } pub fn add_reply_recipient( @@ -169,3 +171,10 @@ impl SimClientUdp { self.reply_map.0.insert(component, reply_sender); } } + +#[cfg(test)] +pub mod tests { + // TODO: Write some basic tests which verify that the ping/pong handling/check for the + // constructor works as expected. + fn test_basic() {} +} diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index bd7ba0b..a0babb8 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -16,10 +16,13 @@ use log::info; use pus::test::create_test_service_dynamic; use satrs::hal::std::tcp_server::ServerConfig; use satrs::hal::std::udp_server::UdpTcServer; +use satrs::pus::HandlingStatus; use satrs::request::GenericMessage; use satrs::tmtc::{PacketSenderWithSharedPool, SharedPacketPool}; use satrs_example::config::pool::{create_sched_tc_pool, create_static_pools}; -use satrs_example::config::tasks::{FREQ_MS_AOCS, FREQ_MS_PUS_STACK, FREQ_MS_UDP_TMTC}; +use satrs_example::config::tasks::{ + FREQ_MS_AOCS, FREQ_MS_PUS_STACK, FREQ_MS_UDP_TMTC, SIM_CLIENT_IDLE_DELAY_MS, +}; use satrs_example::config::{OBSW_SERVER_ADDR, PACKET_ID_VALIDATOR, SERVER_PORT}; use crate::acs::mgm::{ @@ -269,7 +272,9 @@ fn static_tmtc_pool_main() { thread::Builder::new() .name("sat-rs sim adapter".to_string()) .spawn(move || loop { - sim_client.operation(); + if sim_client.operation() == HandlingStatus::Empty { + std::thread::sleep(Duration::from_millis(SIM_CLIENT_IDLE_DELAY_MS)); + } }) .unwrap(), ); @@ -506,7 +511,9 @@ fn dyn_tmtc_pool_main() { thread::Builder::new() .name("sat-rs sim adapter".to_string()) .spawn(move || loop { - sim_client.operation(); + if sim_client.operation() == HandlingStatus::Empty { + std::thread::sleep(Duration::from_millis(SIM_CLIENT_IDLE_DELAY_MS)); + } }) .unwrap(), ); From 4a8db6b26a63c9fa5e5a8c366f488f47cfbb9002 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 May 2024 21:08:41 +0200 Subject: [PATCH 04/23] fix tests --- satrs-minisim/src/acs.rs | 14 +++++----- satrs-minisim/src/udp.rs | 55 +++++++++++++++++++++++++++++++++++++--- 2 files changed, 59 insertions(+), 10 deletions(-) diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index e85ebf3..0e4b0bd 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -188,10 +188,10 @@ pub mod tests { use satrs_minisim::{ acs::{ lis3mdl::{self, MgmLis3MdlReply}, - MgmRequest, MgtDipole, MgtHkSet, MgtReply, MgtRequest, + MgmRequestLis3Mdl, MgtDipole, MgtHkSet, MgtReply, MgtRequest, }, eps::PcduSwitch, - SerializableSimMsgPayload, SimMessageProvider, SimRequest, SimTarget, + SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, }; use crate::{eps::tests::switch_device_on, test_helpers::SimTestbench}; @@ -199,7 +199,7 @@ pub mod tests { #[test] fn test_basic_mgm_request() { let mut sim_testbench = SimTestbench::new(); - let request = SimRequest::new_with_epoch_time(MgmRequest::RequestSensorData); + let request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); sim_testbench .send_request(request) .expect("sending MGM request failed"); @@ -208,7 +208,7 @@ pub mod tests { let sim_reply = sim_testbench.try_receive_next_reply(); assert!(sim_reply.is_some()); let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.component(), SimTarget::MgmLis3Mdl); + assert_eq!(sim_reply.component(), SimComponent::MgmLis3Mdl); let reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); assert_eq!(reply.common.switch_state, SwitchStateBinary::Off); @@ -222,7 +222,7 @@ pub mod tests { let mut sim_testbench = SimTestbench::new(); switch_device_on(&mut sim_testbench, PcduSwitch::Mgm); - let mut request = SimRequest::new_with_epoch_time(MgmRequest::RequestSensorData); + let mut request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); sim_testbench .send_request(request) .expect("sending MGM request failed"); @@ -231,12 +231,12 @@ pub mod tests { let mut sim_reply_res = sim_testbench.try_receive_next_reply(); assert!(sim_reply_res.is_some()); let mut sim_reply = sim_reply_res.unwrap(); - assert_eq!(sim_reply.component(), SimTarget::MgmLis3Mdl); + assert_eq!(sim_reply.component(), SimComponent::MgmLis3Mdl); let first_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) .expect("failed to deserialize MGM sensor values"); sim_testbench.step_by(Duration::from_millis(50)); - request = SimRequest::new_with_epoch_time(MgmRequest::RequestSensorData); + request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); sim_testbench .send_request(request) .expect("sending MGM request failed"); diff --git a/satrs-minisim/src/udp.rs b/satrs-minisim/src/udp.rs index ad50672..7d1d56c 100644 --- a/satrs-minisim/src/udp.rs +++ b/satrs-minisim/src/udp.rs @@ -150,6 +150,7 @@ impl SimUdpServer { mod tests { use std::{ io::ErrorKind, + net::{SocketAddr, UdpSocket}, sync::{ atomic::{AtomicBool, Ordering}, mpsc, Arc, @@ -159,7 +160,6 @@ mod tests { use satrs_minisim::{ eps::{PcduReply, PcduRequest}, - udp::{ReceptionError, SimUdpClient}, SimCtrlReply, SimCtrlRequest, SimReply, SimRequest, }; @@ -171,8 +171,57 @@ mod tests { // Wait time to ensure even possibly laggy systems like CI servers can run the tests. const SERVER_WAIT_TIME_MS: u64 = 50; + #[derive(thiserror::Error, Debug)] + pub enum ReceptionError { + #[error("IO error: {0}")] + Io(#[from] std::io::Error), + #[error("Serde JSON error: {0}")] + SerdeJson(#[from] serde_json::Error), + } + + pub struct SimUdpTestClient { + socket: UdpSocket, + pub reply_buf: [u8; 4096], + } + + impl SimUdpTestClient { + pub fn new( + server_addr: &SocketAddr, + non_blocking: bool, + read_timeot_ms: Option, + ) -> std::io::Result { + let socket = UdpSocket::bind("127.0.0.1:0")?; + socket.set_nonblocking(non_blocking)?; + socket + .connect(server_addr) + .expect("could not connect to server addr"); + if let Some(read_timeout) = read_timeot_ms { + // Set a read timeout so the test does not hang on failures. + socket.set_read_timeout(Some(Duration::from_millis(read_timeout)))?; + } + Ok(Self { + socket, + reply_buf: [0; 4096], + }) + } + + pub fn send_request(&self, sim_request: &SimRequest) -> std::io::Result { + self.socket.send( + &serde_json::to_vec(sim_request).expect("conversion of request to vector failed"), + ) + } + + pub fn recv_raw(&mut self) -> std::io::Result { + self.socket.recv(&mut self.reply_buf) + } + + pub fn recv_sim_reply(&mut self) -> Result { + let read_len = self.recv_raw()?; + Ok(serde_json::from_slice(&self.reply_buf[0..read_len])?) + } + } struct UdpTestbench { - client: SimUdpClient, + client: SimUdpTestClient, stop_signal: Arc, request_receiver: mpsc::Receiver, reply_sender: mpsc::Sender, @@ -197,7 +246,7 @@ mod tests { let server_addr = server.server_addr()?; Ok(( Self { - client: SimUdpClient::new( + client: SimUdpTestClient::new( &server_addr, client_non_blocking, client_read_timeout_ms, From 783388aa6ff2f32fbe541fd9856edb208d4a18b5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 May 2024 11:07:08 +0200 Subject: [PATCH 05/23] pytmtc as regular package now --- satrs-example/README.md | 5 +- satrs-example/pytmtc/.gitignore | 134 ++++++++++++++++++ satrs-example/pytmtc/main.py | 8 +- satrs-example/pytmtc/pyproject.toml | 27 ++++ .../pytmtc/{pus_tm.py => pytmtc/__init__.py} | 0 satrs-example/pytmtc/{ => pytmtc}/common.py | 0 satrs-example/pytmtc/{ => pytmtc}/pus_tc.py | 32 ++--- satrs-example/pytmtc/pytmtc/pus_tm.py | 0 satrs-example/pytmtc/requirements.txt | 3 +- satrs-example/pytmtc/tc_definitions.py | 38 ----- 10 files changed, 185 insertions(+), 62 deletions(-) create mode 100644 satrs-example/pytmtc/pyproject.toml rename satrs-example/pytmtc/{pus_tm.py => pytmtc/__init__.py} (100%) rename satrs-example/pytmtc/{ => pytmtc}/common.py (100%) rename satrs-example/pytmtc/{ => pytmtc}/pus_tc.py (99%) create mode 100644 satrs-example/pytmtc/pytmtc/pus_tm.py delete mode 100644 satrs-example/pytmtc/tc_definitions.py diff --git a/satrs-example/README.md b/satrs-example/README.md index 3447a0d..0503f62 100644 --- a/satrs-example/README.md +++ b/satrs-example/README.md @@ -48,16 +48,17 @@ It is recommended to use a virtual environment to do this. To set up one in the you can use `python3 -m venv venv` on Unix systems or `py -m venv venv` on Windows systems. After doing this, you can check the [venv tutorial](https://docs.python.org/3/tutorial/venv.html) on how to activate the environment and then use the following command to install the required -dependency: +dependency interactively: ```sh -pip install -r requirements.txt +pip install -e . ``` Alternatively, if you would like to use the GUI functionality provided by `tmtccmd`, you can also install it manually with ```sh +pip install -e . pip install tmtccmd[gui] ``` diff --git a/satrs-example/pytmtc/.gitignore b/satrs-example/pytmtc/.gitignore index d994678..008bdd0 100644 --- a/satrs-example/pytmtc/.gitignore +++ b/satrs-example/pytmtc/.gitignore @@ -1,3 +1,4 @@ +/tmtc_conf.json __pycache__ /venv @@ -7,3 +8,136 @@ __pycache__ /seqcnt.txt /.tmtc-history.txt + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +.python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# PyCharm +.idea diff --git a/satrs-example/pytmtc/main.py b/satrs-example/pytmtc/main.py index a90a011..bf7223a 100755 --- a/satrs-example/pytmtc/main.py +++ b/satrs-example/pytmtc/main.py @@ -46,8 +46,8 @@ from spacepackets.seqcount import FileSeqCountProvider, PusFileSeqCountProvider from tmtccmd.util.obj_id import ObjectIdDictT -import pus_tc -from common import Apid, EventU32 +from pytmtc.pus_tc import pack_pus_telecommands, create_cmd_definition_tree +from pytmtc.common import Apid, EventU32 _LOGGER = logging.getLogger() @@ -76,7 +76,7 @@ class SatRsConfigHook(HookBase): def get_command_definitions(self) -> CmdTreeNode: """This function should return the root node of the command definition tree.""" - return pus_tc.create_cmd_definition_tree() + return create_cmd_definition_tree() def get_cmd_history(self) -> Optional[History]: """Optionlly return a history class for the past command paths which will be used @@ -213,7 +213,7 @@ class TcHandler(TcHandlerBase): if info.proc_type == TcProcedureType.TREE_COMMANDING: def_proc = info.to_tree_commanding_procedure() assert def_proc.cmd_path is not None - pus_tc.pack_pus_telecommands(q, def_proc.cmd_path) + pack_pus_telecommands(q, def_proc.cmd_path) def main(): diff --git a/satrs-example/pytmtc/pyproject.toml b/satrs-example/pytmtc/pyproject.toml new file mode 100644 index 0000000..fcb4a32 --- /dev/null +++ b/satrs-example/pytmtc/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "pytmtc" +description = "Python TMTC client for OPS-SAT" +readme = "README.md" +version = "0.1.0" +requires-python = ">=3.8" +authors = [ + {name = "Robin Mueller", email = "robin.mueller.m@gmail.com"}, +] +dependencies = [ + "tmtccmd~=8.0", + "pydantic~=2.7" +] + +[tool.setuptools.packages] +find = {} + +[tool.ruff] +extend-exclude = ["archive"] +[tool.ruff.lint] +ignore = ["E501"] +[tool.ruff.lint.extend-per-file-ignores] +"__init__.py" = ["F401"] diff --git a/satrs-example/pytmtc/pus_tm.py b/satrs-example/pytmtc/pytmtc/__init__.py similarity index 100% rename from satrs-example/pytmtc/pus_tm.py rename to satrs-example/pytmtc/pytmtc/__init__.py diff --git a/satrs-example/pytmtc/common.py b/satrs-example/pytmtc/pytmtc/common.py similarity index 100% rename from satrs-example/pytmtc/common.py rename to satrs-example/pytmtc/pytmtc/common.py diff --git a/satrs-example/pytmtc/pus_tc.py b/satrs-example/pytmtc/pytmtc/pus_tc.py similarity index 99% rename from satrs-example/pytmtc/pus_tc.py rename to satrs-example/pytmtc/pytmtc/pus_tc.py index b0febdc..d81dd28 100644 --- a/satrs-example/pytmtc/pus_tc.py +++ b/satrs-example/pytmtc/pytmtc/pus_tc.py @@ -10,26 +10,11 @@ from tmtccmd.tmtc import DefaultPusQueueHelper from tmtccmd.pus.s11_tc_sched import create_time_tagged_cmd from tmtccmd.pus.s200_fsfw_mode import Subservice as ModeSubservice -from common import AcsId, Apid +from pytmtc.common import AcsId, Apid _LOGGER = logging.getLogger(__name__) -def create_set_mode_cmd( - apid: int, unique_id: int, mode: int, submode: int -) -> PusTelecommand: - app_data = bytearray() - app_data.extend(struct.pack("!I", unique_id)) - app_data.extend(struct.pack("!I", mode)) - app_data.extend(struct.pack("!H", submode)) - return PusTelecommand( - service=200, - subservice=ModeSubservice.TC_MODE_COMMAND, - apid=apid, - app_data=app_data, - ) - - def create_cmd_definition_tree() -> CmdTreeNode: root_node = CmdTreeNode.root_node() @@ -77,6 +62,21 @@ def create_cmd_definition_tree() -> CmdTreeNode: return root_node +def create_set_mode_cmd( + apid: int, unique_id: int, mode: int, submode: int +) -> PusTelecommand: + app_data = bytearray() + app_data.extend(struct.pack("!I", unique_id)) + app_data.extend(struct.pack("!I", mode)) + app_data.extend(struct.pack("!H", submode)) + return PusTelecommand( + service=200, + subservice=ModeSubservice.TC_MODE_COMMAND, + apid=apid, + app_data=app_data, + ) + + def pack_pus_telecommands(q: DefaultPusQueueHelper, cmd_path: str): # It should always be at least the root path "/", so we split of the empty portion left of it. cmd_path_list = cmd_path.split("/")[1:] diff --git a/satrs-example/pytmtc/pytmtc/pus_tm.py b/satrs-example/pytmtc/pytmtc/pus_tm.py new file mode 100644 index 0000000..e69de29 diff --git a/satrs-example/pytmtc/requirements.txt b/satrs-example/pytmtc/requirements.txt index 325615c..9c558e3 100644 --- a/satrs-example/pytmtc/requirements.txt +++ b/satrs-example/pytmtc/requirements.txt @@ -1,2 +1 @@ -tmtccmd == 8.0.0rc2 -# -e git+https://github.com/robamu-org/tmtccmd@97e5e51101a08b21472b3ddecc2063359f7e307a#egg=tmtccmd +. diff --git a/satrs-example/pytmtc/tc_definitions.py b/satrs-example/pytmtc/tc_definitions.py deleted file mode 100644 index 74fbff8..0000000 --- a/satrs-example/pytmtc/tc_definitions.py +++ /dev/null @@ -1,38 +0,0 @@ -from tmtccmd.config import OpCodeEntry, TmtcDefinitionWrapper, CoreServiceList -from tmtccmd.config.globals import get_default_tmtc_defs - -from common import HkOpCodes - - -def tc_definitions() -> TmtcDefinitionWrapper: - defs = get_default_tmtc_defs() - srv_5 = OpCodeEntry() - srv_5.add("0", "Event Test") - defs.add_service( - name=CoreServiceList.SERVICE_5.value, - info="PUS Service 5 Event", - op_code_entry=srv_5, - ) - srv_17 = OpCodeEntry() - srv_17.add("ping", "Ping Test") - srv_17.add("trigger_event", "Trigger Event") - defs.add_service( - name=CoreServiceList.SERVICE_17_ALT, - info="PUS Service 17 Test", - op_code_entry=srv_17, - ) - srv_3 = OpCodeEntry() - srv_3.add(HkOpCodes.GENERATE_ONE_SHOT, "Generate AOCS one shot HK") - defs.add_service( - name=CoreServiceList.SERVICE_3, - info="PUS Service 3 Housekeeping", - op_code_entry=srv_3, - ) - srv_11 = OpCodeEntry() - srv_11.add("0", "Scheduled TC Test") - defs.add_service( - name=CoreServiceList.SERVICE_11, - info="PUS Service 11 TC Scheduling", - op_code_entry=srv_11, - ) - return defs From d1476eb7708afb043e2c14af08e41bf4b124ba3e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 May 2024 11:41:11 +0200 Subject: [PATCH 06/23] added basic tests for pytmtc app --- satrs-example/pytmtc/main.py | 199 +-------------------- satrs-example/pytmtc/pytmtc/config.py | 47 +++++ satrs-example/pytmtc/pytmtc/pus_tc.py | 55 +++++- satrs-example/pytmtc/pytmtc/pus_tm.py | 98 ++++++++++ satrs-example/pytmtc/tests/__init__.py | 0 satrs-example/pytmtc/tests/test_tc_mods.py | 48 +++++ 6 files changed, 255 insertions(+), 192 deletions(-) create mode 100644 satrs-example/pytmtc/pytmtc/config.py create mode 100644 satrs-example/pytmtc/tests/__init__.py create mode 100644 satrs-example/pytmtc/tests/test_tc_mods.py diff --git a/satrs-example/pytmtc/main.py b/satrs-example/pytmtc/main.py index bf7223a..d8a55b8 100755 --- a/satrs-example/pytmtc/main.py +++ b/satrs-example/pytmtc/main.py @@ -3,27 +3,17 @@ import logging import sys import time -from typing import Any, Optional -from prompt_toolkit.history import History -from prompt_toolkit.history import FileHistory -from spacepackets.ccsds import PacketId, PacketType import tmtccmd -from spacepackets.ecss import PusTelemetry, PusVerificator -from spacepackets.ecss.pus_17_test import Service17Tm -from spacepackets.ecss.pus_1_verification import UnpackParams, Service1Tm -from spacepackets.ccsds.time import CdsShortTimestamp +from spacepackets.ecss import PusVerificator -from tmtccmd import TcHandlerBase, ProcedureParamsWrapper +from tmtccmd import ProcedureParamsWrapper from tmtccmd.core.base import BackendRequest from tmtccmd.pus import VerificationWrapper -from tmtccmd.tmtc import CcsdsTmHandler, GenericApidHandlerBase -from tmtccmd.com import ComInterface +from tmtccmd.tmtc import CcsdsTmHandler from tmtccmd.config import ( - CmdTreeNode, default_json_path, SetupParams, - HookBase, params_to_procedure_conversion, ) from tmtccmd.config import PreArgsParsingWrapper, SetupWrapper @@ -33,193 +23,20 @@ from tmtccmd.logging.pus import ( RawTmtcTimedLogWrapper, TimedLogWhen, ) -from tmtccmd.tmtc import ( - TcQueueEntryType, - ProcedureWrapper, - TcProcedureType, - FeedWrapper, - SendCbParams, - DefaultPusQueueHelper, - QueueWrapper, -) -from spacepackets.seqcount import FileSeqCountProvider, PusFileSeqCountProvider -from tmtccmd.util.obj_id import ObjectIdDictT +from spacepackets.seqcount import PusFileSeqCountProvider -from pytmtc.pus_tc import pack_pus_telecommands, create_cmd_definition_tree -from pytmtc.common import Apid, EventU32 +from pytmtc.config import SatrsConfigHook +from pytmtc.pus_tc import TcHandler +from pytmtc.pus_tm import PusHandler _LOGGER = logging.getLogger() -class SatRsConfigHook(HookBase): - def __init__(self, json_cfg_path: str): - super().__init__(json_cfg_path=json_cfg_path) - - def get_communication_interface(self, com_if_key: str) -> Optional[ComInterface]: - from tmtccmd.config.com import ( - create_com_interface_default, - create_com_interface_cfg_default, - ) - - assert self.cfg_path is not None - packet_id_list = [] - for apid in Apid: - packet_id_list.append(PacketId(PacketType.TM, True, apid)) - cfg = create_com_interface_cfg_default( - com_if_key=com_if_key, - json_cfg_path=self.cfg_path, - space_packet_ids=packet_id_list, - ) - assert cfg is not None - return create_com_interface_default(cfg) - - def get_command_definitions(self) -> CmdTreeNode: - """This function should return the root node of the command definition tree.""" - return create_cmd_definition_tree() - - def get_cmd_history(self) -> Optional[History]: - """Optionlly return a history class for the past command paths which will be used - when prompting a command path from the user in CLI mode.""" - return FileHistory(".tmtc-history.txt") - - def get_object_ids(self) -> ObjectIdDictT: - from tmtccmd.config.objects import get_core_object_ids - - return get_core_object_ids() - - -class PusHandler(GenericApidHandlerBase): - def __init__( - self, - file_logger: logging.Logger, - verif_wrapper: VerificationWrapper, - raw_logger: RawTmtcTimedLogWrapper, - ): - super().__init__(None) - self.file_logger = file_logger - self.raw_logger = raw_logger - self.verif_wrapper = verif_wrapper - - def handle_tm(self, apid: int, packet: bytes, _user_args: Any): - try: - pus_tm = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - except ValueError as e: - _LOGGER.warning("Could not generate PUS TM object from raw data") - _LOGGER.warning(f"Raw Packet: [{packet.hex(sep=',')}], REPR: {packet!r}") - raise e - service = pus_tm.service - if service == 1: - tm_packet = Service1Tm.unpack( - data=packet, params=UnpackParams(CdsShortTimestamp.TIMESTAMP_SIZE, 1, 2) - ) - res = self.verif_wrapper.add_tm(tm_packet) - if res is None: - _LOGGER.info( - f"Received Verification TM[{tm_packet.service}, {tm_packet.subservice}] " - f"with Request ID {tm_packet.tc_req_id.as_u32():#08x}" - ) - _LOGGER.warning( - f"No matching telecommand found for {tm_packet.tc_req_id}" - ) - else: - self.verif_wrapper.log_to_console(tm_packet, res) - self.verif_wrapper.log_to_file(tm_packet, res) - elif service == 3: - _LOGGER.info("No handling for HK packets implemented") - _LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]") - pus_tm = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - if pus_tm.subservice == 25: - if len(pus_tm.source_data) < 8: - raise ValueError("No addressable ID in HK packet") - json_str = pus_tm.source_data[8:] - _LOGGER.info(json_str) - elif service == 5: - tm_packet = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - src_data = tm_packet.source_data - event_u32 = EventU32.unpack(src_data) - _LOGGER.info( - f"Received event packet. Source APID: {Apid(tm_packet.apid)!r}, Event: {event_u32}" - ) - if event_u32.group_id == 0 and event_u32.unique_id == 0: - _LOGGER.info("Received test event") - elif service == 17: - tm_packet = Service17Tm.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - if tm_packet.subservice == 2: - self.file_logger.info("Received Ping Reply TM[17,2]") - _LOGGER.info("Received Ping Reply TM[17,2]") - else: - self.file_logger.info( - f"Received Test Packet with unknown subservice {tm_packet.subservice}" - ) - _LOGGER.info( - f"Received Test Packet with unknown subservice {tm_packet.subservice}" - ) - else: - _LOGGER.info( - f"The service {service} is not implemented in Telemetry Factory" - ) - tm_packet = PusTelemetry.unpack( - packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE - ) - self.raw_logger.log_tm(pus_tm) - - -class TcHandler(TcHandlerBase): - def __init__( - self, - seq_count_provider: FileSeqCountProvider, - verif_wrapper: VerificationWrapper, - ): - super(TcHandler, self).__init__() - self.seq_count_provider = seq_count_provider - self.verif_wrapper = verif_wrapper - self.queue_helper = DefaultPusQueueHelper( - queue_wrapper=QueueWrapper.empty(), - tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE, - seq_cnt_provider=seq_count_provider, - pus_verificator=self.verif_wrapper.pus_verificator, - default_pus_apid=None, - ) - - def send_cb(self, send_params: SendCbParams): - entry_helper = send_params.entry - if entry_helper.is_tc: - if entry_helper.entry_type == TcQueueEntryType.PUS_TC: - pus_tc_wrapper = entry_helper.to_pus_tc_entry() - raw_tc = pus_tc_wrapper.pus_tc.pack() - _LOGGER.info(f"Sending {pus_tc_wrapper.pus_tc}") - send_params.com_if.send(raw_tc) - elif entry_helper.entry_type == TcQueueEntryType.LOG: - log_entry = entry_helper.to_log_entry() - _LOGGER.info(log_entry.log_str) - - def queue_finished_cb(self, info: ProcedureWrapper): - if info.proc_type == TcProcedureType.TREE_COMMANDING: - def_proc = info.to_tree_commanding_procedure() - _LOGGER.info(f"Queue handling finished for command {def_proc.cmd_path}") - - def feed_cb(self, info: ProcedureWrapper, wrapper: FeedWrapper): - q = self.queue_helper - q.queue_wrapper = wrapper.queue_wrapper - if info.proc_type == TcProcedureType.TREE_COMMANDING: - def_proc = info.to_tree_commanding_procedure() - assert def_proc.cmd_path is not None - pack_pus_telecommands(q, def_proc.cmd_path) - - def main(): add_colorlog_console_logger(_LOGGER) tmtccmd.init_printout(False) - hook_obj = SatRsConfigHook(json_cfg_path=default_json_path()) + hook_obj = SatrsConfigHook(json_cfg_path=default_json_path()) parser_wrapper = PreArgsParsingWrapper() parser_wrapper.create_default_parent_parser() parser_wrapper.create_default_parser() diff --git a/satrs-example/pytmtc/pytmtc/config.py b/satrs-example/pytmtc/pytmtc/config.py new file mode 100644 index 0000000..6647769 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/config.py @@ -0,0 +1,47 @@ +from typing import Optional +from prompt_toolkit.history import FileHistory, History +from spacepackets.ccsds import PacketId, PacketType +from tmtccmd import HookBase +from tmtccmd.com import ComInterface +from tmtccmd.config import CmdTreeNode +from tmtccmd.util.obj_id import ObjectIdDictT + +from pytmtc.common import Apid +from pytmtc.pus_tc import create_cmd_definition_tree + + +class SatrsConfigHook(HookBase): + def __init__(self, json_cfg_path: str): + super().__init__(json_cfg_path=json_cfg_path) + + def get_communication_interface(self, com_if_key: str) -> Optional[ComInterface]: + from tmtccmd.config.com import ( + create_com_interface_default, + create_com_interface_cfg_default, + ) + + assert self.cfg_path is not None + packet_id_list = [] + for apid in Apid: + packet_id_list.append(PacketId(PacketType.TM, True, apid)) + cfg = create_com_interface_cfg_default( + com_if_key=com_if_key, + json_cfg_path=self.cfg_path, + space_packet_ids=packet_id_list, + ) + assert cfg is not None + return create_com_interface_default(cfg) + + def get_command_definitions(self) -> CmdTreeNode: + """This function should return the root node of the command definition tree.""" + return create_cmd_definition_tree() + + def get_cmd_history(self) -> Optional[History]: + """Optionlly return a history class for the past command paths which will be used + when prompting a command path from the user in CLI mode.""" + return FileHistory(".tmtc-history.txt") + + def get_object_ids(self) -> ObjectIdDictT: + from tmtccmd.config.objects import get_core_object_ids + + return get_core_object_ids() diff --git a/satrs-example/pytmtc/pytmtc/pus_tc.py b/satrs-example/pytmtc/pytmtc/pus_tc.py index d81dd28..486f4f1 100644 --- a/satrs-example/pytmtc/pytmtc/pus_tc.py +++ b/satrs-example/pytmtc/pytmtc/pus_tc.py @@ -4,9 +4,19 @@ import logging from spacepackets.ccsds import CdsShortTimestamp from spacepackets.ecss import PusTelecommand +from spacepackets.seqcount import FileSeqCountProvider +from tmtccmd import ProcedureWrapper, TcHandlerBase from tmtccmd.config import CmdTreeNode +from tmtccmd.pus import VerificationWrapper from tmtccmd.pus.tc.s200_fsfw_mode import Mode -from tmtccmd.tmtc import DefaultPusQueueHelper +from tmtccmd.tmtc import ( + DefaultPusQueueHelper, + FeedWrapper, + QueueWrapper, + SendCbParams, + TcProcedureType, + TcQueueEntryType, +) from tmtccmd.pus.s11_tc_sched import create_time_tagged_cmd from tmtccmd.pus.s200_fsfw_mode import Subservice as ModeSubservice @@ -15,6 +25,49 @@ from pytmtc.common import AcsId, Apid _LOGGER = logging.getLogger(__name__) +class TcHandler(TcHandlerBase): + def __init__( + self, + seq_count_provider: FileSeqCountProvider, + verif_wrapper: VerificationWrapper, + ): + super(TcHandler, self).__init__() + self.seq_count_provider = seq_count_provider + self.verif_wrapper = verif_wrapper + self.queue_helper = DefaultPusQueueHelper( + queue_wrapper=QueueWrapper.empty(), + tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE, + seq_cnt_provider=seq_count_provider, + pus_verificator=self.verif_wrapper.pus_verificator, + default_pus_apid=None, + ) + + def send_cb(self, send_params: SendCbParams): + entry_helper = send_params.entry + if entry_helper.is_tc: + if entry_helper.entry_type == TcQueueEntryType.PUS_TC: + pus_tc_wrapper = entry_helper.to_pus_tc_entry() + raw_tc = pus_tc_wrapper.pus_tc.pack() + _LOGGER.info(f"Sending {pus_tc_wrapper.pus_tc}") + send_params.com_if.send(raw_tc) + elif entry_helper.entry_type == TcQueueEntryType.LOG: + log_entry = entry_helper.to_log_entry() + _LOGGER.info(log_entry.log_str) + + def queue_finished_cb(self, info: ProcedureWrapper): + if info.proc_type == TcProcedureType.TREE_COMMANDING: + def_proc = info.to_tree_commanding_procedure() + _LOGGER.info(f"Queue handling finished for command {def_proc.cmd_path}") + + def feed_cb(self, info: ProcedureWrapper, wrapper: FeedWrapper): + q = self.queue_helper + q.queue_wrapper = wrapper.queue_wrapper + if info.proc_type == TcProcedureType.TREE_COMMANDING: + def_proc = info.to_tree_commanding_procedure() + assert def_proc.cmd_path is not None + pack_pus_telecommands(q, def_proc.cmd_path) + + def create_cmd_definition_tree() -> CmdTreeNode: root_node = CmdTreeNode.root_node() diff --git a/satrs-example/pytmtc/pytmtc/pus_tm.py b/satrs-example/pytmtc/pytmtc/pus_tm.py index e69de29..3d12a79 100644 --- a/satrs-example/pytmtc/pytmtc/pus_tm.py +++ b/satrs-example/pytmtc/pytmtc/pus_tm.py @@ -0,0 +1,98 @@ +import logging +from typing import Any +from spacepackets.ccsds.time import CdsShortTimestamp +from spacepackets.ecss import PusTm +from spacepackets.ecss.pus_17_test import Service17Tm +from spacepackets.ecss.pus_1_verification import Service1Tm, UnpackParams +from tmtccmd.logging.pus import RawTmtcTimedLogWrapper +from tmtccmd.pus import VerificationWrapper +from tmtccmd.tmtc import GenericApidHandlerBase + +from pytmtc.common import Apid, EventU32 + + +_LOGGER = logging.getLogger(__name__) + + +class PusHandler(GenericApidHandlerBase): + def __init__( + self, + file_logger: logging.Logger, + verif_wrapper: VerificationWrapper, + raw_logger: RawTmtcTimedLogWrapper, + ): + super().__init__(None) + self.file_logger = file_logger + self.raw_logger = raw_logger + self.verif_wrapper = verif_wrapper + + def handle_tm(self, apid: int, packet: bytes, _user_args: Any): + try: + pus_tm = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + except ValueError as e: + _LOGGER.warning("Could not generate PUS TM object from raw data") + _LOGGER.warning(f"Raw Packet: [{packet.hex(sep=',')}], REPR: {packet!r}") + raise e + service = pus_tm.service + if service == 1: + tm_packet = Service1Tm.unpack( + data=packet, params=UnpackParams(CdsShortTimestamp.TIMESTAMP_SIZE, 1, 2) + ) + res = self.verif_wrapper.add_tm(tm_packet) + if res is None: + _LOGGER.info( + f"Received Verification TM[{tm_packet.service}, {tm_packet.subservice}] " + f"with Request ID {tm_packet.tc_req_id.as_u32():#08x}" + ) + _LOGGER.warning( + f"No matching telecommand found for {tm_packet.tc_req_id}" + ) + else: + self.verif_wrapper.log_to_console(tm_packet, res) + self.verif_wrapper.log_to_file(tm_packet, res) + elif service == 3: + _LOGGER.info("No handling for HK packets implemented") + _LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]") + pus_tm = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + if pus_tm.subservice == 25: + if len(pus_tm.source_data) < 8: + raise ValueError("No addressable ID in HK packet") + json_str = pus_tm.source_data[8:] + _LOGGER.info(json_str) + elif service == 5: + tm_packet = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + src_data = tm_packet.source_data + event_u32 = EventU32.unpack(src_data) + _LOGGER.info( + f"Received event packet. Source APID: {Apid(tm_packet.apid)!r}, Event: {event_u32}" + ) + if event_u32.group_id == 0 and event_u32.unique_id == 0: + _LOGGER.info("Received test event") + elif service == 17: + tm_packet = Service17Tm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + if tm_packet.subservice == 2: + self.file_logger.info("Received Ping Reply TM[17,2]") + _LOGGER.info("Received Ping Reply TM[17,2]") + else: + self.file_logger.info( + f"Received Test Packet with unknown subservice {tm_packet.subservice}" + ) + _LOGGER.info( + f"Received Test Packet with unknown subservice {tm_packet.subservice}" + ) + else: + _LOGGER.info( + f"The service {service} is not implemented in Telemetry Factory" + ) + tm_packet = PusTm.unpack( + packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE + ) + self.raw_logger.log_tm(pus_tm) diff --git a/satrs-example/pytmtc/tests/__init__.py b/satrs-example/pytmtc/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/satrs-example/pytmtc/tests/test_tc_mods.py b/satrs-example/pytmtc/tests/test_tc_mods.py new file mode 100644 index 0000000..0b56bde --- /dev/null +++ b/satrs-example/pytmtc/tests/test_tc_mods.py @@ -0,0 +1,48 @@ +from unittest import TestCase + +from spacepackets.ccsds import CdsShortTimestamp +from tmtccmd.tmtc import DefaultPusQueueHelper, QueueEntryHelper +from tmtccmd.tmtc.queue import QueueWrapper + +from pytmtc.config import SatrsConfigHook +from pytmtc.pus_tc import pack_pus_telecommands + + +class TestTcModules(TestCase): + def setUp(self): + self.hook = SatrsConfigHook(json_cfg_path="tmtc_conf.json") + self.queue_helper = DefaultPusQueueHelper( + queue_wrapper=QueueWrapper.empty(), + tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE, + seq_cnt_provider=None, + pus_verificator=None, + default_pus_apid=None, + ) + + def test_cmd_tree_creation_works_without_errors(self): + cmd_defs = self.hook.get_command_definitions() + self.assertIsNotNone(cmd_defs) + + def test_ping_cmd_generation(self): + pack_pus_telecommands(self.queue_helper, "/test/ping") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper = QueueEntryHelper(queue_entry) + log_queue = entry_helper.to_log_entry() + self.assertEqual(log_queue.log_str, "Sending PUS ping telecommand") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper.entry = queue_entry + pus_tc_entry = entry_helper.to_pus_tc_entry() + self.assertEqual(pus_tc_entry.pus_tc.service, 17) + self.assertEqual(pus_tc_entry.pus_tc.subservice, 1) + + def test_event_trigger_generation(self): + pack_pus_telecommands(self.queue_helper, "/test/trigger_event") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper = QueueEntryHelper(queue_entry) + log_queue = entry_helper.to_log_entry() + self.assertEqual(log_queue.log_str, "Triggering test event") + queue_entry = self.queue_helper.queue_wrapper.queue.popleft() + entry_helper.entry = queue_entry + pus_tc_entry = entry_helper.to_pus_tc_entry() + self.assertEqual(pus_tc_entry.pus_tc.service, 17) + self.assertEqual(pus_tc_entry.pus_tc.subservice, 128) From 6e5b70af3437b7210e3cd2e0a118f8c01d8695bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 May 2024 13:23:40 +0200 Subject: [PATCH 07/23] basic tests for SIM client --- satrs-example/src/interface/sim_client_udp.rs | 276 ++++++++++++++++-- satrs-minisim/src/acs.rs | 2 +- satrs-minisim/src/controller.rs | 4 +- satrs-minisim/src/eps.rs | 2 +- satrs-minisim/src/lib.rs | 10 +- satrs-minisim/src/udp.rs | 8 +- 6 files changed, 271 insertions(+), 31 deletions(-) diff --git a/satrs-example/src/interface/sim_client_udp.rs b/satrs-example/src/interface/sim_client_udp.rs index d16adfd..16db261 100644 --- a/satrs-example/src/interface/sim_client_udp.rs +++ b/satrs-example/src/interface/sim_client_udp.rs @@ -31,7 +31,7 @@ pub fn create_sim_client(sim_request_rx: mpsc::Receiver) -> Option, - ) -> Result { + ) -> Result { let mut reply_buf: [u8; 4096] = [0; 4096]; - let udp_client = UdpSocket::bind("127.0.0.1:0")?; + let mut udp_client = UdpSocket::bind("127.0.0.1:0")?; udp_client.set_read_timeout(Some(Duration::from_millis(100)))?; + Self::attempt_connection(&mut udp_client, simulator_addr, &mut reply_buf)?; + udp_client.set_nonblocking(true)?; + Ok(Self { + udp_client, + simulator_addr, + sim_request_rx, + reply_map: SimReplyMap(HashMap::new()), + reply_buf, + }) + } + + pub fn attempt_connection( + udp_client: &mut UdpSocket, + simulator_addr: SocketAddr, + reply_buf: &mut [u8], + ) -> Result<(), SimClientCreationError> { let sim_req = SimRequest::new_with_epoch_time(SimCtrlRequest::Ping); let sim_req_json = serde_json::to_string(&sim_req).expect("failed to serialize SimRequest"); udp_client.send_to(sim_req_json.as_bytes(), simulator_addr)?; - match udp_client.recv(&mut reply_buf) { + match udp_client.recv(reply_buf) { Ok(reply_len) => { let sim_reply: SimReply = serde_json::from_slice(&reply_buf[0..reply_len])?; if sim_reply.component() != SimComponent::SimCtrl { - return Err(SimClientCreationResult::ReplyIsNotPong(sim_reply)); + return Err(SimClientCreationError::ReplyIsNotPong(sim_reply)); } - udp_client.set_read_timeout(None)?; let sim_ctrl_reply = SimCtrlReply::from_sim_message(&sim_reply).expect("invalid SIM reply"); match sim_ctrl_reply { - SimCtrlReply::Pong => Ok(Self { - udp_client, - simulator_addr, - sim_request_rx, - reply_map: SimReplyMap(HashMap::new()), - reply_buf, - }), SimCtrlReply::InvalidRequest(_) => { panic!("received invalid request reply from UDP sim server") } + SimCtrlReply::Pong => Ok(()), } } Err(e) => { if e.kind() == std::io::ErrorKind::TimedOut || e.kind() == std::io::ErrorKind::WouldBlock { - Err(SimClientCreationResult::Timeout) + Err(SimClientCreationError::Timeout) } else { - Err(SimClientCreationResult::Io(e)) + Err(SimClientCreationError::Io(e)) } } } @@ -174,7 +183,238 @@ impl SimClientUdp { #[cfg(test)] pub mod tests { - // TODO: Write some basic tests which verify that the ping/pong handling/check for the - // constructor works as expected. - fn test_basic() {} + use std::{ + collections::HashMap, + net::{SocketAddr, UdpSocket}, + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc, Arc, + }, + time::Duration, + }; + + use satrs_minisim::{ + eps::{PcduReply, PcduRequest}, + SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, + SimReply, SimRequest, + }; + + use super::SimClientUdp; + + struct UdpSimTestServer { + udp_server: UdpSocket, + request_tx: mpsc::Sender, + reply_rx: mpsc::Receiver, + last_sender: Option, + stop_signal: Arc, + recv_buf: [u8; 1024], + } + + impl UdpSimTestServer { + pub fn new( + request_tx: mpsc::Sender, + reply_rx: mpsc::Receiver, + stop_signal: Arc, + ) -> Self { + let udp_server = UdpSocket::bind("127.0.0.1:0").expect("creating UDP server failed"); + udp_server + .set_nonblocking(true) + .expect("failed to set UDP server to non-blocking"); + Self { + udp_server, + request_tx, + reply_rx, + last_sender: None, + stop_signal, + recv_buf: [0; 1024], + } + } + + pub fn operation(&mut self) { + loop { + let mut no_sim_replies_handled = true; + let mut no_data_received = true; + if self.stop_signal.load(Ordering::Relaxed) { + break; + } + if let Some(last_sender) = self.last_sender { + loop { + match self.reply_rx.try_recv() { + Ok(sim_reply) => { + let sim_reply_json = serde_json::to_string(&sim_reply) + .expect("failed to serialize SimReply"); + self.udp_server + .send_to(sim_reply_json.as_bytes(), last_sender) + .expect("failed to send reply to client from UDP server"); + no_sim_replies_handled = false; + } + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + panic!("reply sender disconnected") + } + }, + } + } + } + + loop { + match self.udp_server.recv_from(&mut self.recv_buf) { + Ok((read_bytes, from)) => { + let sim_request: SimRequest = + serde_json::from_slice(&self.recv_buf[0..read_bytes]) + .expect("failed to deserialize SimRequest"); + if sim_request.component() == SimComponent::SimCtrl { + // For a ping, we perform the reply handling here directly + let sim_ctrl_request = + SimCtrlRequest::from_sim_message(&sim_request) + .expect("failed to convert SimRequest to SimCtrlRequest"); + match sim_ctrl_request { + SimCtrlRequest::Ping => { + no_data_received = false; + self.last_sender = Some(from); + let sim_reply = SimReply::new(&SimCtrlReply::Pong); + let sim_reply_json = serde_json::to_string(&sim_reply) + .expect("failed to serialize SimReply"); + self.udp_server + .send_to(sim_reply_json.as_bytes(), from) + .expect( + "failed to send reply to client from UDP server", + ); + } + }; + } + // Forward each SIM request for testing purposes. + self.request_tx + .send(sim_request) + .expect("failed to send request"); + } + Err(e) => { + if e.kind() != std::io::ErrorKind::WouldBlock + && e.kind() != std::io::ErrorKind::TimedOut + { + panic!("UDP server error: {}", e); + } + break; + } + } + } + if no_sim_replies_handled && no_data_received { + std::thread::sleep(Duration::from_millis(5)); + } + } + } + + pub fn local_addr(&self) -> SocketAddr { + self.udp_server.local_addr().unwrap() + } + } + + #[test] + fn basic_connection_test() { + let (server_sim_request_tx, server_sim_request_rx) = mpsc::channel(); + let (_server_sim_reply_tx, server_sim_reply_rx) = mpsc::channel(); + let stop_signal = Arc::new(AtomicBool::new(false)); + let mut udp_server = UdpSimTestServer::new( + server_sim_request_tx, + server_sim_reply_rx, + stop_signal.clone(), + ); + let server_addr = udp_server.local_addr(); + let (_client_sim_req_tx, client_sim_req_rx) = mpsc::channel(); + // Need to spawn the simulator UDP server before calling the client constructor. + let jh0 = std::thread::spawn(move || { + udp_server.operation(); + }); + // Creating the client also performs the connection test. + SimClientUdp::new(server_addr, client_sim_req_rx).unwrap(); + let sim_request = server_sim_request_rx + .recv_timeout(Duration::from_millis(50)) + .expect("no SIM request received"); + let ping_request = SimCtrlRequest::from_sim_message(&sim_request) + .expect("failed to create SimCtrlRequest"); + assert_eq!(ping_request, SimCtrlRequest::Ping); + // Stop the server. + stop_signal.store(true, Ordering::Relaxed); + jh0.join().unwrap(); + } + + #[test] + fn basic_request_reply_test() { + let (server_sim_request_tx, server_sim_request_rx) = mpsc::channel(); + let (server_sim_reply_tx, sever_sim_reply_rx) = mpsc::channel(); + let stop_signal = Arc::new(AtomicBool::new(false)); + let mut udp_server = UdpSimTestServer::new( + server_sim_request_tx, + sever_sim_reply_rx, + stop_signal.clone(), + ); + let server_addr = udp_server.local_addr(); + let (client_sim_req_tx, client_sim_req_rx) = mpsc::channel(); + let (client_pcdu_reply_tx, client_pcdu_reply_rx) = mpsc::channel(); + // Need to spawn the simulator UDP server before calling the client constructor. + let jh0 = std::thread::spawn(move || { + udp_server.operation(); + }); + + // Creating the client also performs the connection test. + let mut client = SimClientUdp::new(server_addr, client_sim_req_rx).unwrap(); + client.add_reply_recipient(SimComponent::Pcdu, client_pcdu_reply_tx); + + let sim_request = server_sim_request_rx + .recv_timeout(Duration::from_millis(50)) + .expect("no SIM request received"); + let ping_request = SimCtrlRequest::from_sim_message(&sim_request) + .expect("failed to create SimCtrlRequest"); + assert_eq!(ping_request, SimCtrlRequest::Ping); + + let pcdu_req = PcduRequest::RequestSwitchInfo; + client_sim_req_tx + .send(SimRequest::new_with_epoch_time(pcdu_req)) + .expect("send failed"); + client.operation(); + + // Check that the request arrives properly at the server. + let sim_request = server_sim_request_rx + .recv_timeout(Duration::from_millis(50)) + .expect("no SIM request received"); + let req_recvd_on_server = + PcduRequest::from_sim_message(&sim_request).expect("failed to create SimCtrlRequest"); + matches!(req_recvd_on_server, PcduRequest::RequestSwitchInfo); + + // We inject the reply ourselves. + let pcdu_reply = PcduReply::SwitchInfo(HashMap::new()); + server_sim_reply_tx + .send(SimReply::new(&pcdu_reply)) + .expect("sending PCDU reply failed"); + + // Now we verify that the reply is sent by the UDP server back to the client, and then + // forwarded by the clients internal map. + let mut pcdu_reply_received = false; + for _ in 0..3 { + client.operation(); + + match client_pcdu_reply_rx.try_recv() { + Ok(sim_reply) => { + assert_eq!(sim_reply.component(), SimComponent::Pcdu); + let pcdu_reply_from_client = PcduReply::from_sim_message(&sim_reply) + .expect("failed to create PcduReply"); + assert_eq!(pcdu_reply_from_client, pcdu_reply); + pcdu_reply_received = true; + break; + } + Err(e) => match e { + mpsc::TryRecvError::Empty => std::thread::sleep(Duration::from_millis(10)), + mpsc::TryRecvError::Disconnected => panic!("reply sender disconnected"), + }, + } + } + if !pcdu_reply_received { + panic!("no reply received"); + } + + // Stop the server. + stop_signal.store(true, Ordering::Relaxed); + jh0.join().unwrap(); + } } diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 0e4b0bd..299d09d 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -153,7 +153,7 @@ impl MagnetorquerModel { pub fn send_housekeeping_data(&mut self) { self.reply_sender - .send(SimReply::new(MgtReply::Hk(MgtHkSet { + .send(SimReply::new(&MgtReply::Hk(MgtHkSet { dipole: self.torque_dipole, torquing: self.torquing, }))) diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs index 09d2772..b5404d8 100644 --- a/satrs-minisim/src/controller.rs +++ b/satrs-minisim/src/controller.rs @@ -102,7 +102,7 @@ impl SimController { match sim_ctrl_request { SimCtrlRequest::Ping => { self.reply_sender - .send(SimReply::new(SimCtrlReply::Pong)) + .send(SimReply::new(&SimCtrlReply::Pong)) .expect("sending reply from sim controller failed"); } } @@ -178,7 +178,7 @@ impl SimController { error ); self.reply_sender - .send(SimReply::new(SimCtrlReply::from(error))) + .send(SimReply::new(&SimCtrlReply::from(error))) .expect("sending reply from sim controller failed"); } } diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs index 07c5c4e..af20272 100644 --- a/satrs-minisim/src/eps.rs +++ b/satrs-minisim/src/eps.rs @@ -44,7 +44,7 @@ impl PcduModel { } pub fn send_switch_info(&mut self) { - let reply = SimReply::new(PcduReply::SwitchInfo(self.switcher_map.clone())); + let reply = SimReply::new(&PcduReply::SwitchInfo(self.switcher_map.clone())); self.reply_sender.send(reply).unwrap(); } diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 1f0f1b6..1beb32b 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -96,11 +96,11 @@ pub struct SimReply { } impl SimReply { - pub fn new>(serializable_reply: T) -> Self { + pub fn new>(serializable_reply: &T) -> Self { Self { inner: SimMessage { target: T::TARGET, - payload: serde_json::to_string(&serializable_reply).unwrap(), + payload: serde_json::to_string(serializable_reply).unwrap(), }, } } @@ -185,7 +185,7 @@ pub mod eps { const TARGET: SimComponent = SimComponent::Pcdu; } - #[derive(Debug, Clone, Serialize, Deserialize)] + #[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)] pub enum PcduReply { SwitchInfo(SwitchMap), } @@ -295,7 +295,7 @@ pub mod acs { impl MgmReplyProvider for MgmLis3MdlReply { fn create_mgm_reply(common: MgmReplyCommon) -> SimReply { - SimReply::new(Self::new(common)) + SimReply::new(&Self::new(common)) } } } @@ -382,7 +382,7 @@ pub mod tests { #[test] fn test_basic_reply() { - let sim_reply = SimReply::new(DummyReply::Pong); + let sim_reply = SimReply::new(&DummyReply::Pong); assert_eq!(sim_reply.component(), SimComponent::SimCtrl); assert_eq!(sim_reply.msg_type(), SimMessageType::Reply); let dummy_request = diff --git a/satrs-minisim/src/udp.rs b/satrs-minisim/src/udp.rs index 7d1d56c..e177547 100644 --- a/satrs-minisim/src/udp.rs +++ b/satrs-minisim/src/udp.rs @@ -344,7 +344,7 @@ mod tests { .send_request(&SimRequest::new_with_epoch_time(SimCtrlRequest::Ping)) .expect("sending request failed"); - let sim_reply = SimReply::new(PcduReply::SwitchInfo(get_all_off_switch_map())); + let sim_reply = SimReply::new(&PcduReply::SwitchInfo(get_all_off_switch_map())); udp_testbench.send_reply(&sim_reply); udp_testbench.check_next_sim_reply(&sim_reply); @@ -369,7 +369,7 @@ mod tests { .expect("sending request failed"); // Send a reply to the server, ensure it gets forwarded to the client. - let sim_reply = SimReply::new(PcduReply::SwitchInfo(get_all_off_switch_map())); + let sim_reply = SimReply::new(&PcduReply::SwitchInfo(get_all_off_switch_map())); udp_testbench.send_reply(&sim_reply); std::thread::sleep(Duration::from_millis(SERVER_WAIT_TIME_MS)); @@ -388,7 +388,7 @@ mod tests { let server_thread = std::thread::spawn(move || udp_server.run()); // Send a reply to the server. The client is not connected, so it won't get forwarded. - let sim_reply = SimReply::new(PcduReply::SwitchInfo(get_all_off_switch_map())); + let sim_reply = SimReply::new(&PcduReply::SwitchInfo(get_all_off_switch_map())); udp_testbench.send_reply(&sim_reply); std::thread::sleep(Duration::from_millis(10)); @@ -415,7 +415,7 @@ mod tests { let server_thread = std::thread::spawn(move || udp_server.run()); // The server only caches up to 3 replies. - let sim_reply = SimReply::new(SimCtrlReply::Pong); + let sim_reply = SimReply::new(&SimCtrlReply::Pong); for _ in 0..4 { udp_testbench.send_reply(&sim_reply); } From a4888bce01146567c8d284d036a7cefb8d34a90a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 May 2024 21:38:56 +0200 Subject: [PATCH 08/23] add first MGM device unittests --- satrs-example/src/acs/mgm.rs | 266 +++++++++++++++++++++++++++++------ satrs-example/src/main.rs | 8 +- satrs-minisim/src/lib.rs | 2 +- 3 files changed, 227 insertions(+), 49 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 81bb1f7..f245325 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -5,11 +5,14 @@ use satrs::spacepackets::ecss::hk; use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; use satrs::spacepackets::SpHeader; use satrs_example::{DeviceMode, TimestampHelper}; -use satrs_minisim::acs::lis3mdl::{FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR}; +use satrs_minisim::acs::lis3mdl::{ + MgmLis3MdlReply, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, +}; use satrs_minisim::acs::MgmRequestLis3Mdl; -use satrs_minisim::{SimReply, SimRequest}; +use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; use std::sync::mpsc::{self}; use std::sync::{Arc, Mutex}; +use std::time::Duration; use satrs::mode::{ ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler, @@ -62,13 +65,20 @@ impl SpiInterface for SpiSimInterface { type Error = (); // 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]) -> Result<(), Self::Error> { let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; self.sim_request_tx .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) .expect("failed to send request"); - self.sim_reply_rx.recv().expect("reply timeout"); - // TODO: Write the sensor data to the raw buffer. + let sim_reply = self + .sim_reply_rx + .recv_timeout(Duration::from_millis(100)) + .expect("reply timeout"); + let sim_reply_lis3 = + MgmLis3MdlReply::from_sim_message(&sim_reply).expect("failed to parse LIS3 reply"); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.y.to_be_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_be_bytes()); Ok(()) } } @@ -99,8 +109,8 @@ pub struct MgmData { pub struct MpscModeLeafInterface { pub request_rx: mpsc::Receiver>, - pub reply_tx_to_pus: mpsc::Sender>, - pub reply_tx_to_parent: mpsc::Sender>, + pub reply_to_pus_tx: mpsc::Sender>, + pub reply_to_parent_tx: mpsc::Sender>, } /// Example MGM device handler strongly based on the LIS3MDL MEMS device. @@ -110,8 +120,8 @@ pub struct MgmHandlerLis3Mdl id: UniqueApidTargetId, dev_str: &'static str, mode_interface: MpscModeLeafInterface, - composite_request_receiver: mpsc::Receiver>, - hk_reply_sender: mpsc::Sender>, + composite_request_rx: mpsc::Receiver>, + hk_reply_tx: mpsc::Sender>, tm_sender: TmSender, com_interface: ComInterface, shared_mgm_set: Arc>, @@ -135,43 +145,13 @@ impl MgmHandlerLis3Mdl match &msg.message { CompositeRequest::Hk(hk_request) => { self.handle_hk_request(&msg.requestor_info, hk_request) @@ -199,7 +179,7 @@ impl MgmHandlerLis3Mdl { - self.hk_reply_sender + self.hk_reply_tx .send(GenericMessage::new( *requestor_info, HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), @@ -234,6 +214,38 @@ impl MgmHandlerLis3Mdl ModeRequestHandler mode_and_submode ); self.mode_and_submode = mode_and_submode; + if mode_and_submode.mode() == DeviceMode::Off as u32 { + self.shared_mgm_set.lock().unwrap().valid = false; + } self.handle_mode_reached(Some(requestor))?; Ok(()) } @@ -326,7 +341,7 @@ impl ModeRequestHandler ); } self.mode_interface - .reply_tx_to_pus + .reply_to_pus_tx .send(GenericMessage::new(requestor, reply)) .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; Ok(()) @@ -343,5 +358,168 @@ impl ModeRequestHandler #[cfg(test)] mod tests { - // TODO: Add some basic tests for the modes of the device. + use std::sync::{atomic::AtomicU32, mpsc, Arc, Mutex}; + + use satrs::{ + mode::{ModeReply, ModeRequest}, + request::{GenericMessage, UniqueApidTargetId}, + tmtc::PacketAsVec, + }; + use satrs_example::config::components::Apid; + use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; + + use crate::{pus::hk::HkReply, requests::CompositeRequest}; + + use super::*; + + pub struct TestInterface { + pub call_count: Arc, + pub next_mgm_data: Arc>, + } + + impl TestInterface { + pub fn new( + call_count: Arc, + next_mgm_data: Arc>, + ) -> Self { + Self { + call_count, + next_mgm_data, + } + } + } + + impl SpiInterface for TestInterface { + type Error = (); + + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + let mgm_data = *self.next_mgm_data.lock().unwrap(); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.y.to_be_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.z.to_be_bytes()); + self.call_count + .fetch_add(1, std::sync::atomic::Ordering::Relaxed); + Ok(()) + } + } + + pub struct MgmTestbench { + pub spi_interface_call_count: Arc, + pub next_mgm_data: Arc>, + pub mode_request_tx: mpsc::Sender>, + pub mode_reply_rx_to_pus: mpsc::Receiver>, + pub mode_reply_rx_to_parent: mpsc::Receiver>, + pub composite_request_tx: mpsc::Sender>, + pub hk_reply_rx: mpsc::Receiver>, + pub tm_rx: mpsc::Receiver, + pub handler: MgmHandlerLis3Mdl>, + } + + impl MgmTestbench { + pub fn new() -> Self { + let (request_tx, request_rx) = mpsc::channel(); + let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel(); + let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::channel(); + let mode_interface = MpscModeLeafInterface { + request_rx, + reply_to_pus_tx: reply_tx_to_pus, + reply_to_parent_tx: reply_tx_to_parent, + }; + let (composite_request_tx, composite_request_rx) = mpsc::channel(); + let (hk_reply_tx, hk_reply_rx) = mpsc::channel(); + let (tm_tx, tm_rx) = mpsc::channel::(); + let shared_mgm_set = Arc::default(); + let next_mgm_data = Arc::new(Mutex::default()); + let spi_interface_call_count = Arc::new(AtomicU32::new(0)); + let test_interface = + TestInterface::new(spi_interface_call_count.clone(), next_mgm_data.clone()); + Self { + mode_request_tx: request_tx, + mode_reply_rx_to_pus: reply_rx_to_pus, + mode_reply_rx_to_parent: reply_rx_to_parent, + composite_request_tx, + tm_rx, + hk_reply_rx, + spi_interface_call_count, + next_mgm_data, + handler: MgmHandlerLis3Mdl::new( + UniqueApidTargetId::new(Apid::Acs as u16, 1), + "test-mgm", + mode_interface, + composite_request_rx, + hk_reply_tx, + tm_tx, + test_interface, + shared_mgm_set, + ), + } + } + } + + #[test] + fn test_basic_handler() { + let mut testbench = MgmTestbench::new(); + assert_eq!( + testbench + .spi_interface_call_count + .load(std::sync::atomic::Ordering::Relaxed), + 0 + ); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Off as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); + testbench.handler.periodic_operation(); + // Handler is OFF, no changes expected. + assert_eq!( + testbench + .spi_interface_call_count + .load(std::sync::atomic::Ordering::Relaxed), + 0 + ); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Off as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); + } + + #[test] + fn test_normal_handler() { + let mut testbench = MgmTestbench::new(); + testbench + .mode_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, PUS_MODE_SERVICE.id()), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + )) + .expect("failed to send mode request"); + testbench.handler.periodic_operation(); + assert_eq!( + testbench.handler.mode_and_submode().mode(), + DeviceMode::Normal as u32 + ); + assert_eq!(testbench.handler.mode_and_submode().submode(), 0); + let mode_reply = testbench + .mode_reply_rx_to_pus + .try_recv() + .expect("no mode reply generated"); + match mode_reply.message { + ModeReply::ModeReply(mode) => { + assert_eq!(mode.mode(), DeviceMode::Normal as u32); + assert_eq!(mode.submode(), 0); + } + _ => panic!("unexpected mode reply"), + } + // The device should have been polled once. + assert_eq!( + testbench + .spi_interface_call_count + .load(std::sync::atomic::Ordering::Relaxed), + 1 + ); + // TODO: Check shared MGM set. The field values should be 0, but the entry should be valid. + // TODO: Set non-zero raw values for the next polled MGM set. + } } diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index a0babb8..80ce229 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -209,8 +209,8 @@ fn static_tmtc_pool_main() { let shared_mgm_set = Arc::default(); let mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, - reply_tx_to_pus: pus_mode_reply_tx, - reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx, + reply_to_pus_tx: pus_mode_reply_tx, + reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, }; let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() { @@ -448,8 +448,8 @@ fn dyn_tmtc_pool_main() { let shared_mgm_set = Arc::default(); let mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, - reply_tx_to_pus: pus_mode_reply_tx, - reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx, + reply_to_pus_tx: pus_mode_reply_tx, + reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, }; let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() { diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 1beb32b..361508e 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -247,7 +247,7 @@ pub mod acs { pub const FIELD_LSB_PER_GAUSS_12_SENS: f32 = 1.0 / 2281.0; pub const FIELD_LSB_PER_GAUSS_16_SENS: f32 = 1.0 / 1711.0; - #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + #[derive(Default, Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] pub struct MgmLis3RawValues { pub x: i16, pub y: i16, From 43bd77eef0da8cde61b52a64d1e9db1cb21119ac Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 May 2024 15:33:43 +0200 Subject: [PATCH 09/23] check that MGM data conversion works --- satrs-example/src/acs/mgm.rs | 123 ++++++++++++++++++----------------- 1 file changed, 64 insertions(+), 59 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index f245325..0a3e988 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -10,6 +10,7 @@ use satrs_minisim::acs::lis3mdl::{ }; use satrs_minisim::acs::MgmRequestLis3Mdl; use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; +use std::fmt::Debug; use std::sync::mpsc::{self}; use std::sync::{Arc, Mutex}; use std::time::Duration; @@ -34,7 +35,7 @@ pub const Y_LOWBYTE_IDX: usize = 11; pub const Z_LOWBYTE_IDX: usize = 13; pub trait SpiInterface { - type Error; + type Error: Debug; fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>; } @@ -77,8 +78,8 @@ impl SpiInterface for SpiSimInterface { let sim_reply_lis3 = MgmLis3MdlReply::from_sim_message(&sim_reply).expect("failed to parse LIS3 reply"); rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes()); - rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.y.to_be_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_be_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes()); Ok(()) } } @@ -123,7 +124,7 @@ pub struct MgmHandlerLis3Mdl composite_request_rx: mpsc::Receiver>, hk_reply_tx: mpsc::Sender>, tm_sender: TmSender, - com_interface: ComInterface, + pub com_interface: ComInterface, shared_mgm_set: Arc>, #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] mode_and_submode: ModeAndSubmode, @@ -217,11 +218,12 @@ impl MgmHandlerLis3Mdl ModeRequestHandler #[cfg(test)] mod tests { - use std::sync::{atomic::AtomicU32, mpsc, Arc, Mutex}; + use std::sync::{mpsc, Arc}; use satrs::{ mode::{ModeReply, ModeRequest}, @@ -372,40 +374,28 @@ mod tests { use super::*; + #[derive(Default)] pub struct TestInterface { - pub call_count: Arc, - pub next_mgm_data: Arc>, - } - - impl TestInterface { - pub fn new( - call_count: Arc, - next_mgm_data: Arc>, - ) -> Self { - Self { - call_count, - next_mgm_data, - } - } + pub call_count: u32, + pub next_mgm_data: MgmLis3RawValues, } impl SpiInterface for TestInterface { type Error = (); fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { - let mgm_data = *self.next_mgm_data.lock().unwrap(); - rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.x.to_le_bytes()); - rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.y.to_be_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.z.to_be_bytes()); - self.call_count - .fetch_add(1, std::sync::atomic::Ordering::Relaxed); + 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(()) } } pub struct MgmTestbench { - pub spi_interface_call_count: Arc, - pub next_mgm_data: Arc>, pub mode_request_tx: mpsc::Sender>, pub mode_reply_rx_to_pus: mpsc::Receiver>, pub mode_reply_rx_to_parent: mpsc::Receiver>, @@ -429,10 +419,6 @@ mod tests { let (hk_reply_tx, hk_reply_rx) = mpsc::channel(); let (tm_tx, tm_rx) = mpsc::channel::(); let shared_mgm_set = Arc::default(); - let next_mgm_data = Arc::new(Mutex::default()); - let spi_interface_call_count = Arc::new(AtomicU32::new(0)); - let test_interface = - TestInterface::new(spi_interface_call_count.clone(), next_mgm_data.clone()); Self { mode_request_tx: request_tx, mode_reply_rx_to_pus: reply_rx_to_pus, @@ -440,8 +426,6 @@ mod tests { composite_request_tx, tm_rx, hk_reply_rx, - spi_interface_call_count, - next_mgm_data, handler: MgmHandlerLis3Mdl::new( UniqueApidTargetId::new(Apid::Acs as u16, 1), "test-mgm", @@ -449,7 +433,7 @@ mod tests { composite_request_rx, hk_reply_tx, tm_tx, - test_interface, + TestInterface::default(), shared_mgm_set, ), } @@ -459,12 +443,7 @@ mod tests { #[test] fn test_basic_handler() { let mut testbench = MgmTestbench::new(); - assert_eq!( - testbench - .spi_interface_call_count - .load(std::sync::atomic::Ordering::Relaxed), - 0 - ); + assert_eq!(testbench.handler.com_interface.call_count, 0); assert_eq!( testbench.handler.mode_and_submode().mode(), DeviceMode::Off as u32 @@ -472,12 +451,7 @@ mod tests { assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16); testbench.handler.periodic_operation(); // Handler is OFF, no changes expected. - assert_eq!( - testbench - .spi_interface_call_count - .load(std::sync::atomic::Ordering::Relaxed), - 0 - ); + assert_eq!(testbench.handler.com_interface.call_count, 0); assert_eq!( testbench.handler.mode_and_submode().mode(), DeviceMode::Off as u32 @@ -513,13 +487,44 @@ mod tests { _ => panic!("unexpected mode reply"), } // The device should have been polled once. - assert_eq!( - testbench - .spi_interface_call_count - .load(std::sync::atomic::Ordering::Relaxed), - 1 - ); - // TODO: Check shared MGM set. The field values should be 0, but the entry should be valid. - // TODO: Set non-zero raw values for the next polled MGM set. + assert_eq!(testbench.handler.com_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); + assert!(mgm_set.z < 0.001); + assert!(mgm_set.valid); + } + + #[test] + fn test_normal_handler_mgm_set_conversion() { + let mut testbench = MgmTestbench::new(); + let raw_values = MgmLis3RawValues { + x: 1000, + y: -1000, + z: 1000, + }; + testbench.handler.com_interface.next_mgm_data = raw_values; + testbench + .mode_request_tx + .send(GenericMessage::new( + MessageMetadata::new(0, PUS_MODE_SERVICE.id()), + ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)), + )) + .expect("failed to send mode request"); + testbench.handler.periodic_operation(); + let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap(); + let expected_x = + raw_values.x as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + let expected_y = + raw_values.y as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + let expected_z = + raw_values.z as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS; + let x_diff = (mgm_set.x - expected_x).abs(); + let y_diff = (mgm_set.y - expected_y).abs(); + let z_diff = (mgm_set.z - expected_z).abs(); + assert!(x_diff < 0.001, "x diff too large: {}", x_diff); + assert!(y_diff < 0.001, "y diff too large: {}", y_diff); + assert!(z_diff < 0.001, "z diff too large: {}", z_diff); + assert!(mgm_set.valid); } } From 9e096193dd86cf71474caf0521a05bf690a8a594 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 May 2024 17:21:59 +0200 Subject: [PATCH 10/23] clean up python commander a bit --- satrs-example/pytmtc/pytmtc/hk.py | 15 ++++++++ satrs-example/pytmtc/pytmtc/mgms.py | 25 +++++++++++++ satrs-example/pytmtc/pytmtc/mode.py | 31 ++++++++++++++++ satrs-example/pytmtc/pytmtc/pus_tc.py | 51 ++------------------------- 4 files changed, 74 insertions(+), 48 deletions(-) create mode 100644 satrs-example/pytmtc/pytmtc/hk.py create mode 100644 satrs-example/pytmtc/pytmtc/mgms.py create mode 100644 satrs-example/pytmtc/pytmtc/mode.py diff --git a/satrs-example/pytmtc/pytmtc/hk.py b/satrs-example/pytmtc/pytmtc/hk.py new file mode 100644 index 0000000..a2af2b9 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/hk.py @@ -0,0 +1,15 @@ +import struct +from spacepackets.ecss.pus_3_hk import Subservice +from spacepackets.ecss import PusService, PusTc + + +def create_request_one_shot_hk_cmd(apid: int, unique_id: int, set_id: int) -> PusTc: + app_data = bytearray() + app_data.extend(struct.pack("!I", unique_id)) + app_data.extend(struct.pack("!I", set_id)) + return PusTc( + service=PusService.S3_HOUSEKEEPING, + subservice=Subservice.TC_GENERATE_ONE_PARAMETER_REPORT, + apid=apid, + app_data=app_data, + ) diff --git a/satrs-example/pytmtc/pytmtc/mgms.py b/satrs-example/pytmtc/pytmtc/mgms.py new file mode 100644 index 0000000..ff39072 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/mgms.py @@ -0,0 +1,25 @@ +import enum +from typing import List +from tmtccmd.tmtc import DefaultPusQueueHelper + +from pytmtc.common import AcsId, Apid +from pytmtc.hk import create_request_one_shot_hk_cmd +from pytmtc.mode import handle_set_mode_cmd + + +class SetId(enum.IntEnum): + SENSOR_SET = 0 + + +def create_mgm_cmds(q: DefaultPusQueueHelper, cmd_path: List[str]): + assert len(cmd_path) >= 3 + if cmd_path[2] == "hk": + if cmd_path[3] == "one_shot_hk": + q.add_log_cmd("Sending HK one shot request") + q.add_pus_tc( + create_request_one_shot_hk_cmd(Apid.ACS, AcsId.MGM_0, SetId.SENSOR_SET) + ) + + if cmd_path[2] == "mode": + if cmd_path[3] == "set_mode": + handle_set_mode_cmd(q, "MGM 0", cmd_path[4], Apid.ACS, AcsId.MGM_0) diff --git a/satrs-example/pytmtc/pytmtc/mode.py b/satrs-example/pytmtc/pytmtc/mode.py new file mode 100644 index 0000000..918fdb1 --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/mode.py @@ -0,0 +1,31 @@ +import struct +from spacepackets.ecss import PusTc +from tmtccmd.pus.s200_fsfw_mode import Mode, Subservice +from tmtccmd.tmtc import DefaultPusQueueHelper + + +def create_set_mode_cmd(apid: int, unique_id: int, mode: int, submode: int) -> PusTc: + app_data = bytearray() + app_data.extend(struct.pack("!I", unique_id)) + app_data.extend(struct.pack("!I", mode)) + app_data.extend(struct.pack("!H", submode)) + return PusTc( + service=200, + subservice=Subservice.TC_MODE_COMMAND, + apid=apid, + app_data=app_data, + ) + + +def handle_set_mode_cmd( + q: DefaultPusQueueHelper, target_str: str, mode_str: str, apid: int, unique_id: int +): + if mode_str == "off": + q.add_log_cmd(f"Sending Mode OFF to {target_str}") + q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.OFF, 0)) + elif mode_str == "on": + q.add_log_cmd(f"Sending Mode ON to {target_str}") + q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.ON, 0)) + elif mode_str == "normal": + q.add_log_cmd(f"Sending Mode NORMAL to {target_str}") + q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.NORMAL, 0)) diff --git a/satrs-example/pytmtc/pytmtc/pus_tc.py b/satrs-example/pytmtc/pytmtc/pus_tc.py index 486f4f1..6e1570c 100644 --- a/satrs-example/pytmtc/pytmtc/pus_tc.py +++ b/satrs-example/pytmtc/pytmtc/pus_tc.py @@ -1,5 +1,4 @@ import datetime -import struct import logging from spacepackets.ccsds import CdsShortTimestamp @@ -8,7 +7,6 @@ from spacepackets.seqcount import FileSeqCountProvider from tmtccmd import ProcedureWrapper, TcHandlerBase from tmtccmd.config import CmdTreeNode from tmtccmd.pus import VerificationWrapper -from tmtccmd.pus.tc.s200_fsfw_mode import Mode from tmtccmd.tmtc import ( DefaultPusQueueHelper, FeedWrapper, @@ -18,9 +16,9 @@ from tmtccmd.tmtc import ( TcQueueEntryType, ) from tmtccmd.pus.s11_tc_sched import create_time_tagged_cmd -from tmtccmd.pus.s200_fsfw_mode import Subservice as ModeSubservice -from pytmtc.common import AcsId, Apid +from pytmtc.common import Apid +from pytmtc.mgms import create_mgm_cmds _LOGGER = logging.getLogger(__name__) @@ -115,21 +113,6 @@ def create_cmd_definition_tree() -> CmdTreeNode: return root_node -def create_set_mode_cmd( - apid: int, unique_id: int, mode: int, submode: int -) -> PusTelecommand: - app_data = bytearray() - app_data.extend(struct.pack("!I", unique_id)) - app_data.extend(struct.pack("!I", mode)) - app_data.extend(struct.pack("!H", submode)) - return PusTelecommand( - service=200, - subservice=ModeSubservice.TC_MODE_COMMAND, - apid=apid, - app_data=app_data, - ) - - def pack_pus_telecommands(q: DefaultPusQueueHelper, cmd_path: str): # It should always be at least the root path "/", so we split of the empty portion left of it. cmd_path_list = cmd_path.split("/")[1:] @@ -165,32 +148,4 @@ def pack_pus_telecommands(q: DefaultPusQueueHelper, cmd_path: str): if cmd_path_list[0] == "acs": assert len(cmd_path_list) >= 2 if cmd_path_list[1] == "mgms": - assert len(cmd_path_list) >= 3 - if cmd_path_list[2] == "hk": - if cmd_path_list[3] == "one_shot_hk": - q.add_log_cmd("Sending HK one shot request") - # TODO: Fix - # q.add_pus_tc( - # create_request_one_hk_command( - # make_addressable_id(Apid.ACS, AcsId.MGM_SET) - # ) - # ) - if cmd_path_list[2] == "mode": - if cmd_path_list[3] == "set_mode": - handle_set_mode_cmd( - q, "MGM 0", cmd_path_list[4], Apid.ACS, AcsId.MGM_0 - ) - - -def handle_set_mode_cmd( - q: DefaultPusQueueHelper, target_str: str, mode_str: str, apid: int, unique_id: int -): - if mode_str == "off": - q.add_log_cmd(f"Sending Mode OFF to {target_str}") - q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.OFF, 0)) - elif mode_str == "on": - q.add_log_cmd(f"Sending Mode ON to {target_str}") - q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.ON, 0)) - elif mode_str == "normal": - q.add_log_cmd(f"Sending Mode NORMAL to {target_str}") - q.add_pus_tc(create_set_mode_cmd(apid, unique_id, Mode.NORMAL, 0)) + create_mgm_cmds(q, cmd_path_list) From 37b32a90089d933352c0b6afb638886829f1bbfe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 May 2024 17:55:11 +0200 Subject: [PATCH 11/23] try to make MGM set HK data work --- satrs-example/pytmtc/pytmtc/hk.py | 47 +++++++++++++++++++----- satrs-example/pytmtc/pytmtc/hk_common.py | 16 ++++++++ satrs-example/pytmtc/pytmtc/mgms.py | 22 ++++++++++- satrs-example/pytmtc/pytmtc/pus_tm.py | 9 +---- satrs-example/src/acs/mgm.rs | 23 +++++++++--- satrs/src/hal/std/tcp_server.rs | 4 +- 6 files changed, 95 insertions(+), 26 deletions(-) create mode 100644 satrs-example/pytmtc/pytmtc/hk_common.py diff --git a/satrs-example/pytmtc/pytmtc/hk.py b/satrs-example/pytmtc/pytmtc/hk.py index a2af2b9..6e8aa71 100644 --- a/satrs-example/pytmtc/pytmtc/hk.py +++ b/satrs-example/pytmtc/pytmtc/hk.py @@ -1,15 +1,42 @@ +import logging import struct from spacepackets.ecss.pus_3_hk import Subservice -from spacepackets.ecss import PusService, PusTc +from spacepackets.ecss import PusTm + +from pytmtc.common import AcsId, Apid +from pytmtc.mgms import handle_mgm_hk_report -def create_request_one_shot_hk_cmd(apid: int, unique_id: int, set_id: int) -> PusTc: - app_data = bytearray() - app_data.extend(struct.pack("!I", unique_id)) - app_data.extend(struct.pack("!I", set_id)) - return PusTc( - service=PusService.S3_HOUSEKEEPING, - subservice=Subservice.TC_GENERATE_ONE_PARAMETER_REPORT, - apid=apid, - app_data=app_data, +_LOGGER = logging.getLogger(__name__) + + +def handle_hk_packet(pus_tm: PusTm): + if len(pus_tm.source_data) < 4: + raise ValueError("no unique ID in HK packet") + unique_id = struct.unpack("!I", pus_tm.source_data[:4])[0] + if ( + pus_tm.subservice == Subservice.TM_HK_REPORT + or pus_tm.subservice == Subservice.TM_DIAGNOSTICS_REPORT + ): + if len(pus_tm.source_data) < 8: + raise ValueError("no set ID in HK packet") + set_id = struct.unpack("!I", pus_tm.source_data[4:8])[0] + handle_hk_report(pus_tm, unique_id, set_id) + _LOGGER.warning( + f"handling for HK packet with subservice {pus_tm.subservice} not implemented yet" ) + + +def handle_hk_report(pus_tm: PusTm, unique_id: int, set_id: int): + hk_data = pus_tm.source_data[8:] + if pus_tm.apid == Apid.ACS: + if unique_id == AcsId.MGM_0: + handle_mgm_hk_report(pus_tm, set_id, hk_data) + else: + _LOGGER.warning( + f"handling for HK report with unique ID {unique_id} not implemented yet" + ) + else: + _LOGGER.warning( + f"handling for HK report with apid {pus_tm.apid} not implemented yet" + ) diff --git a/satrs-example/pytmtc/pytmtc/hk_common.py b/satrs-example/pytmtc/pytmtc/hk_common.py new file mode 100644 index 0000000..bb9890a --- /dev/null +++ b/satrs-example/pytmtc/pytmtc/hk_common.py @@ -0,0 +1,16 @@ +import struct + +from spacepackets.ecss import PusService, PusTc +from spacepackets.ecss.pus_3_hk import Subservice + + +def create_request_one_shot_hk_cmd(apid: int, unique_id: int, set_id: int) -> PusTc: + app_data = bytearray() + app_data.extend(struct.pack("!I", unique_id)) + app_data.extend(struct.pack("!I", set_id)) + return PusTc( + service=PusService.S3_HOUSEKEEPING, + subservice=Subservice.TC_GENERATE_ONE_PARAMETER_REPORT, + apid=apid, + app_data=app_data, + ) diff --git a/satrs-example/pytmtc/pytmtc/mgms.py b/satrs-example/pytmtc/pytmtc/mgms.py index ff39072..d420b3e 100644 --- a/satrs-example/pytmtc/pytmtc/mgms.py +++ b/satrs-example/pytmtc/pytmtc/mgms.py @@ -1,12 +1,18 @@ +import logging +import struct import enum from typing import List +from spacepackets.ecss import PusTm from tmtccmd.tmtc import DefaultPusQueueHelper from pytmtc.common import AcsId, Apid -from pytmtc.hk import create_request_one_shot_hk_cmd +from pytmtc.hk_common import create_request_one_shot_hk_cmd from pytmtc.mode import handle_set_mode_cmd +_LOGGER = logging.getLogger(__name__) + + class SetId(enum.IntEnum): SENSOR_SET = 0 @@ -23,3 +29,17 @@ def create_mgm_cmds(q: DefaultPusQueueHelper, cmd_path: List[str]): if cmd_path[2] == "mode": if cmd_path[3] == "set_mode": handle_set_mode_cmd(q, "MGM 0", cmd_path[4], Apid.ACS, AcsId.MGM_0) + + +def handle_mgm_hk_report(pus_tm: PusTm, set_id: int, hk_data: bytes): + if set_id == SetId.SENSOR_SET: + if len(hk_data) != 13: + raise ValueError(f"invalid HK data length, expected 13, got {len(hk_data)}") + data_valid = hk_data[0] + mgm_x = struct.unpack("!f", hk_data[1:5])[0] + mgm_y = struct.unpack("!f", hk_data[5:9])[0] + mgm_z = struct.unpack("!f", hk_data[9:13])[0] + _LOGGER.info( + f"received MGM HK set in uT: Valid {data_valid} X {mgm_x} Y {mgm_y} Z {mgm_z}" + ) + pass diff --git a/satrs-example/pytmtc/pytmtc/pus_tm.py b/satrs-example/pytmtc/pytmtc/pus_tm.py index 3d12a79..ed55212 100644 --- a/satrs-example/pytmtc/pytmtc/pus_tm.py +++ b/satrs-example/pytmtc/pytmtc/pus_tm.py @@ -9,6 +9,7 @@ from tmtccmd.pus import VerificationWrapper from tmtccmd.tmtc import GenericApidHandlerBase from pytmtc.common import Apid, EventU32 +from pytmtc.hk import handle_hk_packet _LOGGER = logging.getLogger(__name__) @@ -53,16 +54,10 @@ class PusHandler(GenericApidHandlerBase): self.verif_wrapper.log_to_console(tm_packet, res) self.verif_wrapper.log_to_file(tm_packet, res) elif service == 3: - _LOGGER.info("No handling for HK packets implemented") - _LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]") pus_tm = PusTm.unpack( packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE ) - if pus_tm.subservice == 25: - if len(pus_tm.source_data) < 8: - raise ValueError("No addressable ID in HK packet") - json_str = pus_tm.source_data[8:] - _LOGGER.info(json_str) + handle_hk_packet(pus_tm) elif service == 5: tm_packet = PusTm.unpack( packet, timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 0a3e988..21424b5 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -34,6 +34,12 @@ pub const X_LOWBYTE_IDX: usize = 9; pub const Y_LOWBYTE_IDX: usize = 11; pub const Z_LOWBYTE_IDX: usize = 13; +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +#[repr(u32)] +pub enum SetId { + SensorData = 0, +} + pub trait SpiInterface { type Error: Debug; fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>; @@ -133,7 +139,7 @@ pub struct MgmHandlerLis3Mdl #[new(default)] rx_buf: [u8; 32], #[new(default)] - tm_buf: [u8; 16], + tm_buf: [u8; 32], #[new(default)] stamp_helper: TimestampHelper, } @@ -180,6 +186,9 @@ impl MgmHandlerLis3Mdl { + // TODO: We should provide a helper class for generating some of the boilerplate. + // This includes the APID, subservice, unique ID and set ID handling. The user + // should be able to simply specify the HK data as a slice. self.hk_reply_tx .send(GenericMessage::new( *requestor_info, @@ -194,15 +203,17 @@ impl MgmHandlerLis3Mdl Date: Sat, 11 May 2024 19:11:41 +0200 Subject: [PATCH 12/23] the PCDU handler is already required --- satrs-example/src/acs/mgm.rs | 43 ++--- satrs-example/src/eps/mod.rs | 1 + satrs-example/src/eps/pcdu.rs | 44 ++++++ satrs-example/src/main.rs | 1 + satrs-minisim/src/acs.rs | 13 +- satrs-minisim/src/lib.rs | 53 ++++--- satrs/src/power.rs | 290 +++++++++++++++++++++++++++------- 7 files changed, 339 insertions(+), 106 deletions(-) create mode 100644 satrs-example/src/eps/mod.rs create mode 100644 satrs-example/src/eps/pcdu.rs diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 21424b5..8d0c03d 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -6,7 +6,7 @@ use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; use satrs::spacepackets::SpHeader; use satrs_example::{DeviceMode, TimestampHelper}; use satrs_minisim::acs::lis3mdl::{ - MgmLis3MdlReply, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, + MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, }; use satrs_minisim::acs::MgmRequestLis3Mdl; use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; @@ -47,18 +47,16 @@ pub trait SpiInterface { #[derive(Default)] pub struct SpiDummyInterface { - pub dummy_val_0: i16, - pub dummy_val_1: i16, - pub dummy_val_2: i16, + pub dummy_values: MgmLis3RawValues, } impl SpiInterface for SpiDummyInterface { 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.dummy_val_0.to_le_bytes()); - rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_1.to_be_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_2.to_be_bytes()); + 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(()) } } @@ -74,18 +72,27 @@ impl SpiInterface for 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> { let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; - self.sim_request_tx + if let Err(e) = self + .sim_request_tx .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) - .expect("failed to send request"); - let sim_reply = self - .sim_reply_rx - .recv_timeout(Duration::from_millis(100)) - .expect("reply timeout"); - let sim_reply_lis3 = - MgmLis3MdlReply::from_sim_message(&sim_reply).expect("failed to parse LIS3 reply"); - rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes()); - rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes()); + { + log::error!("failed to send MGM LIS3 request: {}", e); + } + match self.sim_reply_rx.recv_timeout(Duration::from_millis(50)) { + Ok(sim_reply) => { + let sim_reply_lis3 = MgmLis3MdlReply::from_sim_message(&sim_reply) + .expect("failed to parse LIS3 reply"); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes()); + } + Err(e) => { + log::warn!("MGM LIS3 SIM reply timeout: {}", e); + } + } Ok(()) } } diff --git a/satrs-example/src/eps/mod.rs b/satrs-example/src/eps/mod.rs new file mode 100644 index 0000000..055b3ab --- /dev/null +++ b/satrs-example/src/eps/mod.rs @@ -0,0 +1 @@ +pub mod pcdu; diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs new file mode 100644 index 0000000..e572cf5 --- /dev/null +++ b/satrs-example/src/eps/pcdu.rs @@ -0,0 +1,44 @@ +use std::{ + collections::HashMap, + sync::{mpsc, Arc, Mutex}, +}; + +use derive_new::new; +use satrs::{ + mode::ModeAndSubmode, + power::SwitchState, + pus::EcssTmSender, + request::{GenericMessage, UniqueApidTargetId}, +}; +use satrs_example::TimestampHelper; + +use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; + +pub trait SerialInterface {} + +#[derive(Copy, Clone, PartialEq, Eq, Hash)] +#[repr(u32)] +pub enum SwitchId { + Mgm0 = 0, + Mgt = 1, +} + +pub type SwitchMap = HashMap; + +/// Example PCDU device handler. +#[derive(new)] +#[allow(clippy::too_many_arguments)] +pub struct PcduHandler { + id: UniqueApidTargetId, + dev_str: &'static str, + mode_interface: MpscModeLeafInterface, + composite_request_rx: mpsc::Receiver>, + hk_reply_tx: mpsc::Sender>, + tm_sender: TmSender, + pub com_interface: ComInterface, + shared_switch_map: Arc>, + #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] + mode_and_submode: ModeAndSubmode, + #[new(default)] + stamp_helper: TimestampHelper, +} diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 80ce229..712e895 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -1,4 +1,5 @@ mod acs; +mod eps; mod events; mod hk; mod interface; diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 299d09d..2403487 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -15,7 +15,7 @@ use satrs_minisim::{ use crate::time::current_millis; -// Earth magnetic field varies between -30 uT and 30 uT +// Earth magnetic field varies between roughly -30 uT and 30 uT const AMPLITUDE_MGM_UT: f32 = 30.0; // Lets start with a simple frequency here. const FREQUENCY_MGM: f32 = 1.0; @@ -26,14 +26,9 @@ const PHASE_Z: f32 = 0.2; /// Simple model for a magnetometer where the measure magnetic fields are modeled with sine waves. /// -/// Please note that that a more realistic MGM model wouold include the following components -/// which are not included here to simplify the model: -/// -/// 1. It would probably generate signed [i16] values which need to be converted to SI units -/// because it is a digital sensor -/// 2. It would sample the magnetic field at a high fixed rate. This might not be possible for -/// a general purpose OS, but self self-sampling at a relatively high rate (20-40 ms) might -/// stil lbe possible. +/// An ideal sensor would sample the magnetic field at a high fixed rate. This might not be +/// possible for a general purpose OS, but self self-sampling at a relatively high rate (20-40 ms) +/// might still be possible and is probably sufficient for many OBSW needs. pub struct MagnetometerModel { pub switch_state: SwitchStateBinary, pub periodicity: Duration, diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 361508e..124ef32 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -236,6 +236,7 @@ pub mod acs { y: -30.0, z: 30.0, }; + pub const ALL_ONES_SENSOR_VAL: i16 = 0xffff_u16 as i16; pub mod lis3mdl { use super::*; @@ -264,27 +265,39 @@ pub mod acs { impl MgmLis3MdlReply { pub fn new(common: MgmReplyCommon) -> Self { - let mut raw_reply: [u8; 7] = [0; 7]; - let raw_x: i16 = (common.sensor_values.x - / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) - .round() as i16; - let raw_y: i16 = (common.sensor_values.y - / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) - .round() as i16; - let raw_z: i16 = (common.sensor_values.z - / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) - .round() as i16; - // The first byte is a dummy byte. - raw_reply[1..3].copy_from_slice(&raw_x.to_be_bytes()); - raw_reply[3..5].copy_from_slice(&raw_y.to_be_bytes()); - raw_reply[5..7].copy_from_slice(&raw_z.to_be_bytes()); - Self { - common, - raw: MgmLis3RawValues { - x: raw_x, - y: raw_y, - z: raw_z, + match common.switch_state { + SwitchStateBinary::Off => Self { + common, + raw: MgmLis3RawValues { + x: ALL_ONES_SENSOR_VAL, + y: ALL_ONES_SENSOR_VAL, + z: ALL_ONES_SENSOR_VAL, + }, }, + SwitchStateBinary::On => { + let mut raw_reply: [u8; 7] = [0; 7]; + let raw_x: i16 = (common.sensor_values.x + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_y: i16 = (common.sensor_values.y + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_z: i16 = (common.sensor_values.z + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + // The first byte is a dummy byte. + raw_reply[1..3].copy_from_slice(&raw_x.to_be_bytes()); + raw_reply[3..5].copy_from_slice(&raw_y.to_be_bytes()); + raw_reply[5..7].copy_from_slice(&raw_z.to_be_bytes()); + Self { + common, + raw: MgmLis3RawValues { + x: raw_x, + y: raw_y, + z: raw_z, + }, + } + } } } } diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 1e1fda1..48eae0e 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -1,22 +1,15 @@ +use derive_new::new; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +#[cfg(feature = "std")] +#[allow(unused_imports)] +pub use std_mod::*; -/// Generic trait for a device capable of switching itself on or off. -pub trait PowerSwitch { - type Error; - - fn switch_on(&mut self) -> Result<(), Self::Error>; - fn switch_off(&mut self) -> Result<(), Self::Error>; - - fn is_switch_on(&self) -> bool { - self.switch_state() == SwitchState::On - } - - fn switch_state(&self) -> SwitchState; -} +use crate::request::MessageMetadata; #[derive(Debug, Eq, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum SwitchState { Off = 0, On = 1, @@ -26,6 +19,7 @@ pub enum SwitchState { #[derive(Debug, Eq, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum SwitchStateBinary { Off = 0, On = 1, @@ -66,18 +60,26 @@ pub type SwitchId = u16; pub trait PowerSwitcherCommandSender { type Error; - fn send_switch_on_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error>; - fn send_switch_off_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error>; + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error>; + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error>; } pub trait PowerSwitchInfo { type Error; /// Retrieve the switch state - fn get_switch_state(&mut self, switch_id: SwitchId) -> Result; + fn switch_state(&self, switch_id: SwitchId) -> Result; - fn get_is_switch_on(&mut self, switch_id: SwitchId) -> Result { - Ok(self.get_switch_state(switch_id)? == SwitchState::On) + fn is_switch_on(&self, switch_id: SwitchId) -> Result { + Ok(self.switch_state(switch_id)? == SwitchState::On) } /// The maximum delay it will take to change a switch. @@ -87,52 +89,222 @@ pub trait PowerSwitchInfo { fn switch_delay_ms(&self) -> u32; } -#[cfg(test)] -mod tests { - #![allow(dead_code)] +#[derive(new)] +pub struct SwitchRequest { + switch_id: SwitchId, + target_state: SwitchStateBinary, +} + +impl SwitchRequest { + pub fn switch_id(&self) -> SwitchId { + self.switch_id + } + + pub fn target_state(&self) -> SwitchStateBinary { + self.target_state + } +} + +#[cfg(feature = "std")] +pub mod std_mod { + use std::sync::mpsc; + + use crate::{ + queue::GenericSendError, + request::{GenericMessage, MessageMetadata}, + }; + use super::*; - use std::boxed::Box; - struct Pcdu { - switch_rx: std::sync::mpsc::Receiver<(SwitchId, u16)>, + pub type MpscSwitchCmdSender = mpsc::Sender>; + pub type MpscSwitchCmdSenderBounded = mpsc::SyncSender>; + + impl PowerSwitcherCommandSender for MpscSwitchCmdSender { + type Error = GenericSendError; + + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::On), + )) + .map_err(|_| GenericSendError::RxDisconnected) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::Off), + )) + .map_err(|_| GenericSendError::RxDisconnected) + } } - #[derive(Eq, PartialEq)] - enum DeviceState { - OFF, - SwitchingPower, - ON, - SETUP, - IDLE, - } - struct MyComplexDevice { - power_switcher: Box>, - power_info: Box>, - switch_id: SwitchId, - some_state: u16, - dev_state: DeviceState, - mode: u32, - submode: u16, - } + impl PowerSwitcherCommandSender for MpscSwitchCmdSenderBounded { + type Error = GenericSendError; - impl MyComplexDevice { - pub fn periodic_op(&mut self) { - // .. mode command coming in - let mode = 1; - if mode == 1 { - if self.dev_state == DeviceState::OFF { - self.power_switcher - .send_switch_on_cmd(self.switch_id) - .expect("sending siwthc cmd failed"); - self.dev_state = DeviceState::SwitchingPower; - } - if self.dev_state == DeviceState::SwitchingPower { - if self.power_info.get_is_switch_on(0).unwrap() { - self.dev_state = DeviceState::ON; - self.mode = 1; - } - } - } + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.try_send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::On), + )) + .map_err(|e| match e { + mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), + mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected, + }) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.try_send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::Off), + )) + .map_err(|e| match e { + mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), + mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected, + }) } } } + +#[cfg(test)] +mod tests { + // TODO: Add unittests for PowerSwitcherCommandSender impls for mpsc. + + use std::sync::mpsc::{self, TryRecvError}; + + use crate::{queue::GenericSendError, request::GenericMessage, ComponentId}; + + use super::*; + + const TEST_REQ_ID: u32 = 2; + const TEST_SENDER_ID: ComponentId = 5; + + const TEST_SWITCH_ID: u16 = 0x1ff; + + fn common_checks(request: &GenericMessage) { + assert_eq!(request.requestor_info.sender_id(), TEST_SENDER_ID); + assert_eq!(request.requestor_info.request_id(), TEST_REQ_ID); + assert_eq!(request.message.switch_id(), TEST_SWITCH_ID); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_on_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + switch_cmd_tx + .send_switch_on_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::On); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_off_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + switch_cmd_tx + .send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::Off); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_rx_disconnected() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + drop(switch_cmd_rx); + let result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::RxDisconnected); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_on_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(3); + switch_cmd_tx + .send_switch_on_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::On); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_off_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(3); + switch_cmd_tx + .send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::Off); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_rx_disconnected() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(1); + drop(switch_cmd_rx); + let result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::RxDisconnected); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_queue_full() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(1); + let mut result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_ok()); + result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::QueueFull(None)); + matches!(switch_cmd_rx.try_recv(), Err(TryRecvError::Empty)); + } +} From 8728c7ebeaec5c0ea5d2f614489f7f9e2636c20c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 12 May 2024 14:23:42 +0200 Subject: [PATCH 13/23] continued sample PCDU handler --- satrs-example/src/acs/mgm.rs | 52 ++++----- satrs-example/src/eps/pcdu.rs | 192 ++++++++++++++++++++++++++++++++-- 2 files changed, 212 insertions(+), 32 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 8d0c03d..62baf04 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -233,6 +233,32 @@ impl MgmHandlerLis3Mdl { + let result = self.handle_mode_request(msg); + // TODO: Trigger event? + if result.is_err() { + log::warn!( + "{}: mode request failed with error {:?}", + self.dev_str, + result.err().unwrap() + ); + } + } + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); + } else { + break; + } + } + } + } + } + pub fn poll_sensor(&mut self) { // Communicate with the device. This is actually how to read the data from the LIS3 device // SPI interface. @@ -265,32 +291,6 @@ impl MgmHandlerLis3Mdl { - let result = self.handle_mode_request(msg); - // TODO: Trigger event? - if result.is_err() { - log::warn!( - "{}: mode request failed with error {:?}", - self.dev_str, - result.err().unwrap() - ); - } - } - Err(e) => { - if e != mpsc::TryRecvError::Empty { - log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); - } else { - break; - } - } - } - } - } } impl ModeProvider diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index e572cf5..ad331a0 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -5,18 +5,33 @@ use std::{ use derive_new::new; use satrs::{ - mode::ModeAndSubmode, + hk::{HkRequest, HkRequestVariant}, + mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, power::SwitchState, pus::EcssTmSender, - request::{GenericMessage, UniqueApidTargetId}, + queue::{GenericSendError, GenericTargetedMessagingError}, + request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, }; -use satrs_example::TimestampHelper; +use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; -pub trait SerialInterface {} +pub trait SerialInterface { + type Error; + /// Send some data via the serial interface. + fn send(&self, data: &[u8]) -> Result<(), Self::Error>; + /// Receive all replies received on the serial interface so far. This function takes a closure + /// and call its for each received packet, passing the received packet into it. + fn recv_replies(&self, f: ReplyHandler) -> Result<(), Self::Error>; +} -#[derive(Copy, Clone, PartialEq, Eq, Hash)] +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum OpCode { + RegularOp = 0, + PollAndRecvReplies = 1, +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq, Hash)] #[repr(u32)] pub enum SwitchId { Mgm0 = 0, @@ -25,6 +40,12 @@ pub enum SwitchId { pub type SwitchMap = HashMap; +#[derive(Clone, PartialEq, Eq)] +pub struct SwitchSet { + pub valid: bool, + pub switch_map: SwitchMap, +} + /// Example PCDU device handler. #[derive(new)] #[allow(clippy::too_many_arguments)] @@ -36,9 +57,168 @@ pub struct PcduHandler { hk_reply_tx: mpsc::Sender>, tm_sender: TmSender, pub com_interface: ComInterface, - shared_switch_map: Arc>, + shared_switch_map: Arc>, #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] mode_and_submode: ModeAndSubmode, #[new(default)] stamp_helper: TimestampHelper, } + +impl PcduHandler { + pub fn periodic_operation(&mut self, op_code: OpCode) { + match op_code { + OpCode::RegularOp => { + self.stamp_helper.update_from_now(); + // Handle requests. + self.handle_composite_requests(); + self.handle_mode_requests(); + } + OpCode::PollAndRecvReplies => {} + } + } + + pub fn handle_composite_requests(&mut self) { + loop { + match self.composite_request_rx.try_recv() { + Ok(ref msg) => match &msg.message { + CompositeRequest::Hk(hk_request) => { + self.handle_hk_request(&msg.requestor_info, hk_request) + } + // TODO: This object does not have actions (yet).. Still send back completion failure + // reply. + CompositeRequest::Action(_action_req) => {} + }, + + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!( + "{}: failed to receive composite request: {:?}", + self.dev_str, + e + ); + } else { + break; + } + } + } + } + } + + pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) { + match hk_request.variant { + HkRequestVariant::OneShot => todo!(), + HkRequestVariant::EnablePeriodic => todo!(), + HkRequestVariant::DisablePeriodic => todo!(), + HkRequestVariant::ModifyCollectionInterval(_) => todo!(), + } + } + + pub fn handle_mode_requests(&mut self) { + loop { + // TODO: Only allow one set mode request per cycle? + match self.mode_interface.request_rx.try_recv() { + Ok(msg) => { + let result = self.handle_mode_request(msg); + // TODO: Trigger event? + if result.is_err() { + log::warn!( + "{}: mode request failed with error {:?}", + self.dev_str, + result.err().unwrap() + ); + } + } + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); + } else { + break; + } + } + } + } + } +} + +impl ModeProvider + for PcduHandler +{ + fn mode_and_submode(&self) -> ModeAndSubmode { + self.mode_and_submode + } +} + +impl ModeRequestHandler + for PcduHandler +{ + type Error = ModeError; + fn start_transition( + &mut self, + requestor: MessageMetadata, + mode_and_submode: ModeAndSubmode, + ) -> Result<(), satrs::mode::ModeError> { + log::info!( + "{}: transitioning to mode {:?}", + self.dev_str, + mode_and_submode + ); + self.mode_and_submode = mode_and_submode; + if mode_and_submode.mode() == DeviceMode::Off as u32 { + self.shared_switch_map.lock().unwrap().valid = false; + } + self.handle_mode_reached(Some(requestor))?; + Ok(()) + } + + fn announce_mode(&self, _requestor_info: Option, _recursive: bool) { + log::info!( + "{} announcing mode: {:?}", + self.dev_str, + self.mode_and_submode + ); + } + + fn handle_mode_reached( + &mut self, + requestor: Option, + ) -> Result<(), Self::Error> { + self.announce_mode(requestor, false); + if let Some(requestor) = requestor { + if requestor.sender_id() != PUS_MODE_SERVICE.id() { + log::warn!( + "can not send back mode reply to sender {}", + requestor.sender_id() + ); + } else { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode()))?; + } + } + Ok(()) + } + + fn send_mode_reply( + &self, + requestor: MessageMetadata, + reply: ModeReply, + ) -> Result<(), Self::Error> { + if requestor.sender_id() != PUS_MODE_SERVICE.id() { + log::warn!( + "can not send back mode reply to sender {}", + requestor.sender_id() + ); + } + self.mode_interface + .reply_to_pus_tx + .send(GenericMessage::new(requestor, reply)) + .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; + Ok(()) + } + + fn handle_mode_info( + &mut self, + _requestor_info: MessageMetadata, + _info: ModeAndSubmode, + ) -> Result<(), Self::Error> { + Ok(()) + } +} From 15fcb17363ee9c6e326096014a71bf736fb3a442 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 16 May 2024 16:28:22 +0200 Subject: [PATCH 14/23] continue PCDU handler --- satrs-example/src/eps/pcdu.rs | 51 +++++++++++++++++++++++++++++++++-- satrs-minisim/src/lib.rs | 25 +++++++++++++++++ 2 files changed, 74 insertions(+), 2 deletions(-) diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index ad331a0..2b20520 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -13,6 +13,7 @@ use satrs::{ request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; +use satrs_minisim::{SimReply, SimRequest}; use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; @@ -22,7 +23,49 @@ pub trait SerialInterface { fn send(&self, data: &[u8]) -> Result<(), Self::Error>; /// Receive all replies received on the serial interface so far. This function takes a closure /// and call its for each received packet, passing the received packet into it. - fn recv_replies(&self, f: ReplyHandler) -> Result<(), Self::Error>; + fn try_recv_replies( + &self, + f: ReplyHandler, + ) -> Result<(), Self::Error>; +} + +pub struct SimSerialInterface { + pub sim_request_tx: mpsc::Sender, + pub sim_reply_rx: mpsc::Receiver, +} + +impl SerialInterface for SimSerialInterface { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + let request: SimRequest = serde_json::from_slice(data).unwrap(); + self.sim_request_tx + .send(request) + .expect("failed to send request to simulation"); + Ok(()) + } + + fn try_recv_replies( + &self, + mut f: ReplyHandler, + ) -> Result<(), Self::Error> { + loop { + match self.sim_reply_rx.try_recv() { + Ok(reply) => { + let reply = serde_json::to_string(&reply).unwrap(); + f(reply.as_bytes()); + } + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + log::warn!("sim reply sender has disconnected"); + break; + } + }, + } + } + Ok(()) + } } #[derive(Debug, Copy, Clone, PartialEq, Eq)] @@ -73,7 +116,9 @@ impl PcduHandler {} + OpCode::PollAndRecvReplies => { + self.poll_and_handle_replies(); + } } } @@ -138,6 +183,8 @@ impl PcduHandler ModeProvider diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 124ef32..c4b952c 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -172,6 +172,13 @@ pub mod eps { Mgt = 1, } + #[derive(Debug, Copy, Clone)] + #[repr(u8)] + pub enum PcduRequestId { + SwitchDevice = 0, + RequestSwitchInfo = 1, + } + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] pub enum PcduRequest { SwitchDevice { @@ -181,6 +188,24 @@ pub mod eps { RequestSwitchInfo, } + /* + impl PcduRequest { + /// The sole purpose of this method + pub fn write_to_be_bytes(&self, buf: &mut [u8]) { + match self { + PcduRequest::SwitchDevice { switch, state } => { + buf[0] = PcduRequestId::SwitchDevice as u8; + buf[1..3].copy_from_slice(&(*switch as u16).to_be_bytes()); + buf[4] = *state as u8; + } + PcduRequest::RequestSwitchInfo => { + buf[0] = PcduRequestId::RequestSwitchInfo as u8; + } + } + } + } + */ + impl SerializableSimMsgPayload for PcduRequest { const TARGET: SimComponent = SimComponent::Pcdu; } From cb0a65c4d44053f014061895b8684df9bbf31320 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 May 2024 14:08:42 +0200 Subject: [PATCH 15/23] continue PCDU --- satrs-example/src/config.rs | 8 +++ satrs-example/src/eps/pcdu.rs | 120 +++++++++++++++++++++++++++++----- satrs-example/src/main.rs | 72 ++++++++++++++++++-- satrs-minisim/Cargo.toml | 1 + satrs-minisim/src/eps.rs | 27 ++++---- satrs-minisim/src/lib.rs | 71 +++++++++++++------- 6 files changed, 240 insertions(+), 59 deletions(-) diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs index 9e3ec6f..f3b2037 100644 --- a/satrs-example/src/config.rs +++ b/satrs-example/src/config.rs @@ -132,6 +132,7 @@ pub mod components { Acs = 3, Cfdp = 4, Tmtc = 5, + Eps = 6, } // Component IDs for components with the PUS APID. @@ -150,6 +151,11 @@ pub mod components { Mgm0 = 0, } + #[derive(Copy, Clone, PartialEq, Eq)] + pub enum EpsId { + Pcdu = 0, + } + #[derive(Copy, Clone, PartialEq, Eq)] pub enum TmtcId { UdpServer = 0, @@ -172,6 +178,8 @@ pub mod components { UniqueApidTargetId::new(Apid::Sched as u16, 0); pub const MGM_HANDLER_0: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Mgm0 as u32); + pub const PCDU_HANDLER: UniqueApidTargetId = + UniqueApidTargetId::new(Apid::Eps as u16, EpsId::Pcdu as u32); pub const UDP_SERVER: UniqueApidTargetId = UniqueApidTargetId::new(Apid::Tmtc as u16, TmtcId::UdpServer as u32); pub const TCP_SERVER: UniqueApidTargetId = diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 2b20520..e39377c 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -1,5 +1,6 @@ use std::{ - collections::HashMap, + cell::RefCell, + collections::VecDeque, sync::{mpsc, Arc, Mutex}, }; @@ -7,13 +8,16 @@ use derive_new::new; use satrs::{ hk::{HkRequest, HkRequestVariant}, mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, - power::SwitchState, + power::{SwitchState, SwitchStateBinary}, pus::EcssTmSender, queue::{GenericSendError, GenericTargetedMessagingError}, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; -use satrs_minisim::{SimReply, SimRequest}; +use satrs_minisim::{ + eps::{PcduReply, PcduRequest, SwitchMap, SwitchMapWrapper}, + SerializableSimMsgPayload, SimReply, SimRequest, +}; use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; @@ -29,12 +33,13 @@ pub trait SerialInterface { ) -> Result<(), Self::Error>; } -pub struct SimSerialInterface { +#[derive(new)] +pub struct SerialInterfaceToSim { pub sim_request_tx: mpsc::Sender, pub sim_reply_rx: mpsc::Receiver, } -impl SerialInterface for SimSerialInterface { +impl SerialInterface for SerialInterfaceToSim { type Error = (); fn send(&self, data: &[u8]) -> Result<(), Self::Error> { @@ -68,27 +73,112 @@ impl SerialInterface for SimSerialInterface { } } +#[derive(Default)] +pub struct SerialInterfaceDummy { + // Need interior mutability here for both fields. + pub switch_map: RefCell, + pub reply_deque: RefCell>, +} + +impl SerialInterface for SerialInterfaceDummy { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + let sim_req: SimRequest = serde_json::from_slice(data).unwrap(); + let pcdu_req = + PcduRequest::from_sim_message(&sim_req).expect("PCDU request creation failed"); + let switch_map_mut = &mut self.switch_map.borrow_mut().0; + match pcdu_req { + PcduRequest::SwitchDevice { switch, state } => { + match switch_map_mut.entry(switch) { + std::collections::hash_map::Entry::Occupied(mut val) => { + match state { + SwitchStateBinary::Off => { + *val.get_mut() = SwitchState::Off; + } + SwitchStateBinary::On => { + *val.get_mut() = SwitchState::On; + } + }; + } + std::collections::hash_map::Entry::Vacant(vacant) => { + match state { + SwitchStateBinary::Off => vacant.insert(SwitchState::Off), + SwitchStateBinary::On => vacant.insert(SwitchState::On), + }; + } + }; + } + PcduRequest::RequestSwitchInfo => { + let mut reply_deque_mut = self.reply_deque.borrow_mut(); + reply_deque_mut.push_back(SimReply::new(&PcduReply::SwitchInfo( + self.switch_map.borrow().0.clone(), + ))); + } + }; + Ok(()) + } + + fn try_recv_replies( + &self, + mut f: ReplyHandler, + ) -> Result<(), Self::Error> { + if self.reply_deque.borrow().is_empty() { + return Ok(()); + } + loop { + let mut reply_deque_mut = self.reply_deque.borrow_mut(); + let next_reply = reply_deque_mut.pop_front().unwrap(); + let reply = serde_json::to_string(&next_reply).unwrap(); + f(reply.as_bytes()); + if reply_deque_mut.is_empty() { + break; + } + } + Ok(()) + } +} + +pub enum SerialSimInterfaceWrapper { + Dummy(SerialInterfaceDummy), + Sim(SerialInterfaceToSim), +} + +impl SerialInterface for SerialSimInterfaceWrapper { + type Error = (); + + fn send(&self, data: &[u8]) -> Result<(), Self::Error> { + match self { + SerialSimInterfaceWrapper::Dummy(dummy) => dummy.send(data), + SerialSimInterfaceWrapper::Sim(sim) => sim.send(data), + } + } + + fn try_recv_replies( + &self, + f: ReplyHandler, + ) -> Result<(), Self::Error> { + match self { + SerialSimInterfaceWrapper::Dummy(dummy) => dummy.try_recv_replies(f), + SerialSimInterfaceWrapper::Sim(sim) => sim.try_recv_replies(f), + } + } +} + #[derive(Debug, Copy, Clone, PartialEq, Eq)] pub enum OpCode { RegularOp = 0, PollAndRecvReplies = 1, } -#[derive(Debug, Copy, Clone, PartialEq, Eq, Hash)] -#[repr(u32)] -pub enum SwitchId { - Mgm0 = 0, - Mgt = 1, -} - -pub type SwitchMap = HashMap; - -#[derive(Clone, PartialEq, Eq)] +#[derive(Clone, PartialEq, Eq, Default)] pub struct SwitchSet { pub valid: bool, pub switch_map: SwitchMap, } +pub type SharedSwitchSet = Arc>; + /// Example PCDU device handler. #[derive(new)] #[allow(clippy::too_many_arguments)] diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 712e895..bc8a124 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -8,6 +8,9 @@ mod pus; mod requests; mod tmtc; +use crate::eps::pcdu::{ + PcduHandler, SerialInterfaceDummy, SerialInterfaceToSim, SerialSimInterfaceWrapper, +}; use crate::events::EventHandler; use crate::interface::udp::DynamicUdpTmHandler; use crate::pus::stack::PusStack; @@ -45,7 +48,7 @@ use crate::requests::{CompositeRequest, GenericRequestRouter}; use satrs::mode::ModeRequest; use satrs::pus::event_man::EventRequestWithToken; use satrs::spacepackets::{time::cds::CdsTime, time::TimeWriter}; -use satrs_example::config::components::{MGM_HANDLER_0, TCP_SERVER, UDP_SERVER}; +use satrs_example::config::components::{MGM_HANDLER_0, PCDU_HANDLER, TCP_SERVER, UDP_SERVER}; use std::net::{IpAddr, SocketAddr}; use std::sync::mpsc; use std::sync::{Arc, RwLock}; @@ -68,11 +71,17 @@ fn static_tmtc_pool_main() { let (sim_request_tx, sim_request_rx) = mpsc::channel(); let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); + let (pcdu_sim_reply_tx, pcdu_sim_reply_rx) = mpsc::channel(); let mut opt_sim_client = create_sim_client(sim_request_rx); let (mgm_handler_composite_tx, mgm_handler_composite_rx) = mpsc::channel::>(); + let (pcdu_handler_composite_tx, pcdu_handler_composite_rx) = + mpsc::channel::>(); + let (mgm_handler_mode_tx, mgm_handler_mode_rx) = mpsc::channel::>(); + let (pcdu_handler_mode_tx, pcdu_handler_mode_rx) = + mpsc::channel::>(); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let mut request_map = GenericRequestRouter::default(); @@ -82,6 +91,12 @@ fn static_tmtc_pool_main() { request_map .mode_router_map .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx); + request_map + .composite_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx); + request_map + .mode_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_mode_tx); // This helper structure is used by all telecommand providers which need to send telecommands // to the TC source. @@ -207,10 +222,11 @@ fn static_tmtc_pool_main() { let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = mpsc::channel(); + let shared_switch_set = Arc::default(); let shared_mgm_set = Arc::default(); - let mode_leaf_interface = MpscModeLeafInterface { + let mgm_mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, - reply_to_pus_tx: pus_mode_reply_tx, + reply_to_pus_tx: pus_mode_reply_tx.clone(), reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, }; @@ -226,14 +242,41 @@ fn static_tmtc_pool_main() { let mut mgm_handler = MgmHandlerLis3Mdl::new( MGM_HANDLER_0, "MGM_0", - mode_leaf_interface, + mgm_mode_leaf_interface, mgm_handler_composite_rx, - pus_hk_reply_tx, - tm_sink_tx, + pus_hk_reply_tx.clone(), + tm_sink_tx.clone(), mgm_spi_interface, shared_mgm_set, ); + let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = + mpsc::channel(); + let pcdu_mode_leaf_interface = MpscModeLeafInterface { + request_rx: pcdu_handler_mode_rx, + reply_to_pus_tx: pus_mode_reply_tx, + reply_to_parent_tx: pcdu_handler_mode_reply_to_parent_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); + SerialSimInterfaceWrapper::Sim(SerialInterfaceToSim::new( + sim_request_tx.clone(), + pcdu_sim_reply_rx, + )) + } else { + SerialSimInterfaceWrapper::Dummy(SerialInterfaceDummy::default()) + }; + let mut pcdu_handler = PcduHandler::new( + PCDU_HANDLER, + "PCDU", + pcdu_mode_leaf_interface, + pcdu_handler_composite_rx, + pus_hk_reply_tx, + tm_sink_tx, + pcdu_serial_interface, + shared_switch_set, + ); + info!("Starting TMTC and UDP task"); let jh_udp_tmtc = thread::Builder::new() .name("SATRS tmtc-udp".to_string()) @@ -290,6 +333,21 @@ fn static_tmtc_pool_main() { }) .unwrap(); + info!("Starting EPS thread"); + let jh_eps = thread::Builder::new() + .name("sat-rs 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. + pcdu_handler.periodic_operation(eps::pcdu::OpCode::RegularOp); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(300)); + }) + .unwrap(); + info!("Starting PUS handler thread"); let jh_pus_handler = thread::Builder::new() .name("sat-rs pus".to_string()) @@ -315,6 +373,7 @@ fn static_tmtc_pool_main() { .expect("Joining SIM client thread failed"); } jh_aocs.join().expect("Joining AOCS thread failed"); + jh_eps.join().expect("Joining EPS thread failed"); jh_pus_handler .join() .expect("Joining PUS handler thread failed"); @@ -328,6 +387,7 @@ fn dyn_tmtc_pool_main() { let (sim_request_tx, sim_request_rx) = mpsc::channel(); let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); + // let (pcdu_sim_reply_tx, pcdu_sim_reply_rx) = mpsc::channel(); let mut opt_sim_client = create_sim_client(sim_request_rx); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index e1449a9..0e8b18c 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -11,6 +11,7 @@ serde_json = "1" log = "0.4" thiserror = "1" fern = "0.5" +strum = { version = "0.26", features = ["derive"] } humantime = "2" [dependencies.asynchronix] diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs index af20272..aedc31a 100644 --- a/satrs-minisim/src/eps.rs +++ b/satrs-minisim/src/eps.rs @@ -1,4 +1,4 @@ -use std::{collections::HashMap, sync::mpsc, time::Duration}; +use std::{sync::mpsc, time::Duration}; use asynchronix::{ model::{Model, Output}, @@ -6,14 +6,14 @@ use asynchronix::{ }; use satrs::power::SwitchStateBinary; use satrs_minisim::{ - eps::{PcduReply, PcduSwitch, SwitchMap}, + eps::{PcduReply, PcduSwitch, SwitchMapBinaryWrapper}, SimReply, }; pub const SWITCH_INFO_DELAY_MS: u64 = 10; pub struct PcduModel { - pub switcher_map: SwitchMap, + pub switcher_map: SwitchMapBinaryWrapper, pub mgm_switch: Output, pub mgt_switch: Output, pub reply_sender: mpsc::Sender, @@ -21,12 +21,8 @@ pub struct PcduModel { impl PcduModel { pub fn new(reply_sender: mpsc::Sender) -> Self { - let mut switcher_map = HashMap::new(); - switcher_map.insert(PcduSwitch::Mgm, SwitchStateBinary::Off); - switcher_map.insert(PcduSwitch::Mgt, SwitchStateBinary::Off); - Self { - switcher_map, + switcher_map: Default::default(), mgm_switch: Output::new(), mgt_switch: Output::new(), reply_sender, @@ -44,7 +40,7 @@ impl PcduModel { } pub fn send_switch_info(&mut self) { - let reply = SimReply::new(&PcduReply::SwitchInfo(self.switcher_map.clone())); + let reply = SimReply::new(&PcduReply::SwitchInfo(self.switcher_map.0.clone())); self.reply_sender.send(reply).unwrap(); } @@ -54,6 +50,7 @@ impl PcduModel { ) { let val = self .switcher_map + .0 .get_mut(&switch_and_target_state.0) .unwrap_or_else(|| panic!("switch {:?} not found", switch_and_target_state.0)); *val = switch_and_target_state.1; @@ -76,7 +73,8 @@ pub(crate) mod tests { use std::time::Duration; use satrs_minisim::{ - eps::PcduRequest, SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, + eps::{PcduRequest, SwitchMapBinary}, + SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, }; use crate::test_helpers::SimTestbench; @@ -105,14 +103,11 @@ pub(crate) mod tests { switch_device(sim_testbench, switch, SwitchStateBinary::On); } - pub(crate) fn get_all_off_switch_map() -> SwitchMap { - let mut switcher_map = SwitchMap::new(); - switcher_map.insert(super::PcduSwitch::Mgm, super::SwitchStateBinary::Off); - switcher_map.insert(super::PcduSwitch::Mgt, super::SwitchStateBinary::Off); - switcher_map + pub(crate) fn get_all_off_switch_map() -> SwitchMapBinary { + SwitchMapBinary::default() } - fn check_switch_state(sim_testbench: &mut SimTestbench, expected_switch_map: &SwitchMap) { + fn check_switch_state(sim_testbench: &mut SimTestbench, expected_switch_map: &SwitchMapBinary) { let request = SimRequest::new_with_epoch_time(PcduRequest::RequestSwitchInfo); sim_testbench .send_request(request) diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index c4b952c..515e448 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -160,18 +160,62 @@ impl From for SimCtrlReply { pub mod eps { use super::*; + use satrs::power::{SwitchState, SwitchStateBinary}; use std::collections::HashMap; + use strum::{EnumIter, IntoEnumIterator}; - use satrs::power::SwitchStateBinary; + pub type SwitchMap = HashMap; + pub type SwitchMapBinary = HashMap; - pub type SwitchMap = HashMap; + pub struct SwitchMapWrapper(pub SwitchMap); + pub struct SwitchMapBinaryWrapper(pub SwitchMapBinary); - #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash)] + #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash, EnumIter)] pub enum PcduSwitch { Mgm = 0, Mgt = 1, } + impl Default for SwitchMapBinaryWrapper { + fn default() -> Self { + let mut switch_map = SwitchMapBinary::default(); + for entry in PcduSwitch::iter() { + switch_map.insert(entry, SwitchStateBinary::Off); + } + Self(switch_map) + } + } + + impl Default for SwitchMapWrapper { + fn default() -> Self { + let mut switch_map = SwitchMap::default(); + for entry in PcduSwitch::iter() { + switch_map.insert(entry, SwitchState::Unknown); + } + Self(switch_map) + } + } + + impl SwitchMapWrapper { + pub fn new_with_init_switches_off() -> Self { + let mut switch_map = SwitchMap::default(); + for entry in PcduSwitch::iter() { + switch_map.insert(entry, SwitchState::Off); + } + Self(switch_map) + } + } + + impl From for SwitchMapWrapper { + fn from(value: SwitchMapBinaryWrapper) -> Self { + value + .0 + .iter() + .map(|(key, value)| (*key, SwitchState::from(value.into()))) + .collect() + } + } + #[derive(Debug, Copy, Clone)] #[repr(u8)] pub enum PcduRequestId { @@ -188,31 +232,14 @@ pub mod eps { RequestSwitchInfo, } - /* - impl PcduRequest { - /// The sole purpose of this method - pub fn write_to_be_bytes(&self, buf: &mut [u8]) { - match self { - PcduRequest::SwitchDevice { switch, state } => { - buf[0] = PcduRequestId::SwitchDevice as u8; - buf[1..3].copy_from_slice(&(*switch as u16).to_be_bytes()); - buf[4] = *state as u8; - } - PcduRequest::RequestSwitchInfo => { - buf[0] = PcduRequestId::RequestSwitchInfo as u8; - } - } - } - } - */ - impl SerializableSimMsgPayload for PcduRequest { const TARGET: SimComponent = SimComponent::Pcdu; } #[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)] pub enum PcduReply { - SwitchInfo(SwitchMap), + // Ack, + SwitchInfo(SwitchMapBinary), } impl SerializableSimMsgPayload for PcduReply { From 8e89c8dd660cff48336a5d9286a1cbad62f600b2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 May 2024 17:58:54 +0200 Subject: [PATCH 16/23] compiles again --- satrs-example/src/eps/pcdu.rs | 21 ++++++--------------- satrs-minisim/src/lib.rs | 15 +++++++-------- 2 files changed, 13 insertions(+), 23 deletions(-) diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index e39377c..a18b114 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -8,14 +8,13 @@ use derive_new::new; use satrs::{ hk::{HkRequest, HkRequestVariant}, mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, - power::{SwitchState, SwitchStateBinary}, pus::EcssTmSender, queue::{GenericSendError, GenericTargetedMessagingError}, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; use satrs_minisim::{ - eps::{PcduReply, PcduRequest, SwitchMap, SwitchMapWrapper}, + eps::{PcduReply, PcduRequest, SwitchMap, SwitchMapBinaryWrapper}, SerializableSimMsgPayload, SimReply, SimRequest, }; @@ -76,7 +75,7 @@ impl SerialInterface for SerialInterfaceToSim { #[derive(Default)] pub struct SerialInterfaceDummy { // Need interior mutability here for both fields. - pub switch_map: RefCell, + pub switch_map: RefCell, pub reply_deque: RefCell>, } @@ -92,20 +91,10 @@ impl SerialInterface for SerialInterfaceDummy { PcduRequest::SwitchDevice { switch, state } => { match switch_map_mut.entry(switch) { std::collections::hash_map::Entry::Occupied(mut val) => { - match state { - SwitchStateBinary::Off => { - *val.get_mut() = SwitchState::Off; - } - SwitchStateBinary::On => { - *val.get_mut() = SwitchState::On; - } - }; + *val.get_mut() = state; } std::collections::hash_map::Entry::Vacant(vacant) => { - match state { - SwitchStateBinary::Off => vacant.insert(SwitchState::Off), - SwitchStateBinary::On => vacant.insert(SwitchState::On), - }; + vacant.insert(state); } }; } @@ -205,6 +194,8 @@ impl PcduHandler { self.poll_and_handle_replies(); diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 515e448..35ddd97 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -204,15 +204,14 @@ pub mod eps { } Self(switch_map) } - } - impl From for SwitchMapWrapper { - fn from(value: SwitchMapBinaryWrapper) -> Self { - value - .0 - .iter() - .map(|(key, value)| (*key, SwitchState::from(value.into()))) - .collect() + pub fn from_binary_switch_map_ref(switch_map: &SwitchMapBinary) -> Self { + Self( + switch_map + .iter() + .map(|(key, value)| (*key, SwitchState::from(*value))) + .collect(), + ) } } From 295fed9a72bf5d94cd7574868009c54cc6a3d2f1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 May 2024 18:39:25 +0200 Subject: [PATCH 17/23] continue PCDU handler --- satrs-example/src/eps/pcdu.rs | 42 ++++++++++++++++++++++++++++++++--- satrs-example/src/main.rs | 3 +++ satrs-minisim/Cargo.toml | 1 + satrs-minisim/src/lib.rs | 16 ++++++++++++- 4 files changed, 58 insertions(+), 4 deletions(-) diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index a18b114..17657c6 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -8,13 +8,14 @@ use derive_new::new; use satrs::{ hk::{HkRequest, HkRequestVariant}, mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, + power::{SwitchRequest, SwitchStateBinary}, pus::EcssTmSender, queue::{GenericSendError, GenericTargetedMessagingError}, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; use satrs_minisim::{ - eps::{PcduReply, PcduRequest, SwitchMap, SwitchMapBinaryWrapper}, + eps::{PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinary, SwitchMapBinaryWrapper}, SerializableSimMsgPayload, SimReply, SimRequest, }; @@ -177,6 +178,7 @@ pub struct PcduHandler { mode_interface: MpscModeLeafInterface, composite_request_rx: mpsc::Receiver>, hk_reply_tx: mpsc::Sender>, + switch_request_rx: mpsc::Receiver>, tm_sender: TmSender, pub com_interface: ComInterface, shared_switch_map: Arc>, @@ -194,8 +196,12 @@ impl PcduHandler { self.poll_and_handle_replies(); @@ -239,6 +245,14 @@ impl PcduHandler PcduHandler match PcduSwitch::try_from(switch_req.message.switch_id()) { + Ok(pcdu_switch) => { + let pcdu_req = PcduRequest::SwitchDevice { + switch: pcdu_switch, + state: switch_req.message.target_state(), + }; + let pcdu_req_ser = serde_json::to_string(&pcdu_req).unwrap(); + self.com_interface.send(pcdu_req_ser.as_bytes()) + } + Err(e) => todo!("failed to convert switch ID {:?} to typed PCDU switch", e), + }, + Err(e) => match e { + mpsc::TryRecvError::Empty => todo!(), + mpsc::TryRecvError::Disconnected => todo!(), + }, + }; + } + } + pub fn poll_and_handle_replies(&mut self) {} } diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index bc8a124..2db7872 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -223,6 +223,8 @@ fn static_tmtc_pool_main() { mpsc::channel(); let shared_switch_set = Arc::default(); + let (switch_request_tx, switch_request_rx) = mpsc::sync_channel(20); + let shared_mgm_set = Arc::default(); let mgm_mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, @@ -272,6 +274,7 @@ fn static_tmtc_pool_main() { pcdu_mode_leaf_interface, pcdu_handler_composite_rx, pus_hk_reply_tx, + switch_request_rx, tm_sink_tx, pcdu_serial_interface, shared_switch_set, diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index 0e8b18c..771ffaa 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -12,6 +12,7 @@ log = "0.4" thiserror = "1" fern = "0.5" strum = { version = "0.26", features = ["derive"] } +num_enum = "0.7" humantime = "2" [dependencies.asynchronix] diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 35ddd97..eb1b420 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -1,4 +1,5 @@ use asynchronix::time::MonotonicTime; +use num_enum::{IntoPrimitive, TryFromPrimitive}; use serde::{de::DeserializeOwned, Deserialize, Serialize}; #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash)] @@ -170,7 +171,20 @@ pub mod eps { pub struct SwitchMapWrapper(pub SwitchMap); pub struct SwitchMapBinaryWrapper(pub SwitchMapBinary); - #[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize, Hash, EnumIter)] + #[derive( + Debug, + Copy, + Clone, + PartialEq, + Eq, + Serialize, + Deserialize, + Hash, + EnumIter, + IntoPrimitive, + TryFromPrimitive, + )] + #[repr(u16)] pub enum PcduSwitch { Mgm = 0, Mgt = 1, From 27e88ed7f735d164d04726ea8ae189ffe56fddfb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 May 2024 18:45:42 +0200 Subject: [PATCH 18/23] fix tests --- satrs-minisim/src/eps.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs index aedc31a..c07e290 100644 --- a/satrs-minisim/src/eps.rs +++ b/satrs-minisim/src/eps.rs @@ -104,7 +104,7 @@ pub(crate) mod tests { } pub(crate) fn get_all_off_switch_map() -> SwitchMapBinary { - SwitchMapBinary::default() + SwitchMapBinaryWrapper::default().0 } fn check_switch_state(sim_testbench: &mut SimTestbench, expected_switch_map: &SwitchMapBinary) { From fe60cb9ccf555d0ba86c565cc2ab9fb70a3089a0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 19 May 2024 17:33:37 +0200 Subject: [PATCH 19/23] continue integrating power subsystem --- satrs-example/src/acs/mgm.rs | 79 ++++++++----- satrs-example/src/eps/mod.rs | 203 ++++++++++++++++++++++++++++++++++ satrs-example/src/eps/pcdu.rs | 4 +- satrs-example/src/main.rs | 12 +- satrs/src/power.rs | 36 +++--- 5 files changed, 285 insertions(+), 49 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 62baf04..9780acd 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -1,5 +1,6 @@ use derive_new::new; use satrs::hk::{HkRequest, HkRequestVariant}; +use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender}; use satrs::queue::{GenericSendError, GenericTargetedMessagingError}; use satrs::spacepackets::ecss::hk; use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; @@ -9,6 +10,7 @@ use satrs_minisim::acs::lis3mdl::{ MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, }; use satrs_minisim::acs::MgmRequestLis3Mdl; +use satrs_minisim::eps::PcduSwitch; use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; use std::fmt::Debug; use std::sync::mpsc::{self}; @@ -127,31 +129,44 @@ pub struct MpscModeLeafInterface { pub reply_to_parent_tx: mpsc::Sender>, } +#[derive(Default)] +pub struct BufWrapper { + tx_buf: [u8; 32], + rx_buf: [u8; 32], + tm_buf: [u8; 32], +} + /// Example MGM device handler strongly based on the LIS3MDL MEMS device. #[derive(new)] #[allow(clippy::too_many_arguments)] -pub struct MgmHandlerLis3Mdl { +pub struct MgmHandlerLis3Mdl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, +> { id: UniqueApidTargetId, dev_str: &'static str, mode_interface: MpscModeLeafInterface, composite_request_rx: mpsc::Receiver>, hk_reply_tx: mpsc::Sender>, + switch_helper: SwitchHelper, tm_sender: TmSender, pub com_interface: ComInterface, shared_mgm_set: Arc>, #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] mode_and_submode: ModeAndSubmode, #[new(default)] - tx_buf: [u8; 32], - #[new(default)] - rx_buf: [u8; 32], - #[new(default)] - tm_buf: [u8; 32], + bufs: BufWrapper, #[new(default)] stamp_helper: TimestampHelper, } -impl MgmHandlerLis3Mdl { +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > MgmHandlerLis3Mdl +{ pub fn periodic_operation(&mut self) { self.stamp_helper.update_from_now(); // Handle requests. @@ -210,17 +225,17 @@ impl MgmHandlerLis3Mdl MgmHandlerLis3Mdl MgmHandlerLis3Mdl ModeProvider - for MgmHandlerLis3Mdl +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > ModeProvider for MgmHandlerLis3Mdl { fn mode_and_submode(&self) -> ModeAndSubmode { self.mode_and_submode } } -impl ModeRequestHandler - for MgmHandlerLis3Mdl +impl< + ComInterface: SpiInterface, + TmSender: EcssTmSender, + SwitchHelper: PowerSwitchInfo + PowerSwitcherCommandSender, + > ModeRequestHandler for MgmHandlerLis3Mdl { type Error = ModeError; fn start_transition( @@ -388,17 +409,17 @@ mod tests { use satrs_example::config::components::Apid; use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; - use crate::{pus::hk::HkReply, requests::CompositeRequest}; + use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest}; use super::*; #[derive(Default)] - pub struct TestInterface { + pub struct TestSpiInterface { pub call_count: u32, pub next_mgm_data: MgmLis3RawValues, } - impl SpiInterface for TestInterface { + impl SpiInterface for TestSpiInterface { type Error = (); fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { @@ -420,7 +441,8 @@ mod tests { pub composite_request_tx: mpsc::Sender>, pub hk_reply_rx: mpsc::Receiver>, pub tm_rx: mpsc::Receiver, - pub handler: MgmHandlerLis3Mdl>, + pub handler: + MgmHandlerLis3Mdl, TestSwitchHelper>, } impl MgmTestbench { @@ -450,8 +472,9 @@ mod tests { mode_interface, composite_request_rx, hk_reply_tx, + TestSwitchHelper::default(), tm_tx, - TestInterface::default(), + TestSpiInterface::default(), shared_mgm_set, ), } diff --git a/satrs-example/src/eps/mod.rs b/satrs-example/src/eps/mod.rs index 055b3ab..ee325e4 100644 --- a/satrs-example/src/eps/mod.rs +++ b/satrs-example/src/eps/mod.rs @@ -1 +1,204 @@ +use derive_new::new; +use std::{ + borrow::BorrowMut, + cell::RefCell, + collections::VecDeque, + sync::mpsc, + time::{Duration, Instant}, +}; + +use satrs::{ + power::{ + PowerSwitchInfo, PowerSwitcherCommandSender, SwitchId, SwitchRequest, SwitchState, + SwitchStateBinary, + }, + queue::GenericSendError, + request::{GenericMessage, MessageMetadata}, +}; +use satrs_minisim::eps::{PcduSwitch, SwitchMapWrapper}; +use thiserror::Error; + +use self::pcdu::SharedSwitchSet; + pub mod pcdu; + +#[derive(new, Clone)] +pub struct PowerSwitchHelper { + switcher_tx: mpsc::SyncSender>, + shared_switch_set: SharedSwitchSet, + #[new(default)] + switch_cmd_sent_instant: Option, +} + +#[derive(Debug, Error, Copy, Clone, PartialEq, Eq)] +pub enum SwitchCommandingError { + #[error("invalid switch id")] + InvalidSwitchId(SwitchId), + #[error("send error: {0}")] + Send(#[from] GenericSendError), +} +#[derive(Debug, Error, Copy, Clone, PartialEq, Eq)] +pub enum SwitchInfoError { + /// This is a configuration error which should not occur. + #[error("switch ID not in map")] + SwitchIdNotInMap(PcduSwitch), + #[error("switch set invalid")] + SwitchSetInvalid, +} + +impl PowerSwitchInfo for PowerSwitchHelper { + type Error = SwitchInfoError; + + fn switch_state( + &self, + switch_id: PcduSwitch, + ) -> Result { + let switch_set = self + .shared_switch_set + .lock() + .expect("failed to lock switch set"); + if !switch_set.valid { + return Err(SwitchInfoError::SwitchSetInvalid); + } + + if let Some(state) = switch_set.switch_map.get(&switch_id) { + return Ok(*state); + } + Err(SwitchInfoError::SwitchIdNotInMap(switch_id)) + } + + fn switch_delay_ms(&self) -> Duration { + // Here, we could set device specific switch delays theoretically. Set it to this value + // for now. + Duration::from_millis(1000) + } +} + +impl PowerSwitcherCommandSender for PowerSwitchHelper { + type Error = SwitchCommandingError; + + fn send_switch_on_cmd( + &self, + requestor_info: satrs::request::MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + self.switcher_tx + .send_switch_on_cmd(requestor_info, switch_id)?; + Ok(()) + } + + fn send_switch_off_cmd( + &self, + requestor_info: satrs::request::MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + self.switcher_tx + .send_switch_off_cmd(requestor_info, switch_id)?; + Ok(()) + } +} + +#[derive(new)] +pub struct SwitchRequestInfo { + pub requestor_info: MessageMetadata, + pub switch_id: PcduSwitch, + pub target_state: satrs::power::SwitchStateBinary, +} + +// Test switch helper which can be used for unittests. +pub struct TestSwitchHelper { + pub switch_requests: RefCell>, + pub switch_info_requests: RefCell>, + pub switch_delay_request_count: u32, + pub next_switch_delay: Duration, + pub switch_map: RefCell, + pub switch_map_valid: bool, +} + +impl Default for TestSwitchHelper { + fn default() -> Self { + Self { + switch_requests: Default::default(), + switch_info_requests: Default::default(), + switch_delay_request_count: Default::default(), + next_switch_delay: Duration::from_millis(1000), + switch_map: Default::default(), + switch_map_valid: Default::default(), + } + } +} + +impl PowerSwitchInfo for TestSwitchHelper { + type Error = SwitchInfoError; + + fn switch_state( + &self, + switch_id: PcduSwitch, + ) -> Result { + let mut switch_info_requests_mut = self.switch_info_requests.borrow_mut(); + switch_info_requests_mut.push_back(switch_id); + if !self.switch_map_valid { + return Err(SwitchInfoError::SwitchSetInvalid); + } + let switch_map_mut = self.switch_map.borrow_mut(); + if let Some(state) = switch_map_mut.0.get(&switch_id) { + return Ok(*state); + } + Err(SwitchInfoError::SwitchIdNotInMap(switch_id)) + } + + fn switch_delay_ms(&self) -> Duration { + self.next_switch_delay + } +} + +impl PowerSwitcherCommandSender for TestSwitchHelper { + type Error = SwitchCommandingError; + + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + let mut switch_requests_mut = self.switch_requests.borrow_mut(); + switch_requests_mut.push_back(SwitchRequestInfo { + requestor_info, + switch_id, + target_state: SwitchStateBinary::On, + }); + // By default, the test helper immediately acknowledges the switch request by setting + // the appropriate switch state in the internal switch map. + let mut switch_map_mut = self.switch_map.borrow_mut(); + if let Some(switch_state) = switch_map_mut.0.get_mut(&switch_id) { + *switch_state = SwitchState::On; + } + Ok(()) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: PcduSwitch, + ) -> Result<(), Self::Error> { + let mut switch_requests_mut = self.switch_requests.borrow_mut(); + switch_requests_mut.push_back(SwitchRequestInfo { + requestor_info, + switch_id, + target_state: SwitchStateBinary::Off, + }); + // By default, the test helper immediately acknowledges the switch request by setting + // the appropriate switch state in the internal switch map. + let mut switch_map_mut = self.switch_map.borrow_mut(); + if let Some(switch_state) = switch_map_mut.0.get_mut(&switch_id) { + *switch_state = SwitchState::Off; + } + Ok(()) + } +} + +impl TestSwitchHelper { + // Helper function which can be used to force a switch to another state for test purposes. + pub fn set_switch_state(&mut self, switch: PcduSwitch, state: SwitchState) { + self.switch_map.get_mut().0.insert(switch, state); + } +} diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 17657c6..639b2cd 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -8,14 +8,14 @@ use derive_new::new; use satrs::{ hk::{HkRequest, HkRequestVariant}, mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, - power::{SwitchRequest, SwitchStateBinary}, + power::SwitchRequest, pus::EcssTmSender, queue::{GenericSendError, GenericTargetedMessagingError}, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; use satrs_minisim::{ - eps::{PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinary, SwitchMapBinaryWrapper}, + eps::{PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinaryWrapper}, SerializableSimMsgPayload, SimReply, SimRequest, }; diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 2db7872..00e704c 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -11,6 +11,7 @@ mod tmtc; use crate::eps::pcdu::{ PcduHandler, SerialInterfaceDummy, SerialInterfaceToSim, SerialSimInterfaceWrapper, }; +use crate::eps::PowerSwitchHelper; use crate::events::EventHandler; use crate::interface::udp::DynamicUdpTmHandler; use crate::pus::stack::PusStack; @@ -50,7 +51,7 @@ use satrs::pus::event_man::EventRequestWithToken; use satrs::spacepackets::{time::cds::CdsTime, time::TimeWriter}; use satrs_example::config::components::{MGM_HANDLER_0, PCDU_HANDLER, TCP_SERVER, UDP_SERVER}; use std::net::{IpAddr, SocketAddr}; -use std::sync::mpsc; +use std::sync::{mpsc, Mutex}; use std::sync::{Arc, RwLock}; use std::thread; use std::time::Duration; @@ -222,8 +223,9 @@ fn static_tmtc_pool_main() { let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = mpsc::channel(); - let shared_switch_set = Arc::default(); + let shared_switch_set = Arc::new(Mutex::default()); let (switch_request_tx, switch_request_rx) = mpsc::sync_channel(20); + let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone()); let shared_mgm_set = Arc::default(); let mgm_mode_leaf_interface = MpscModeLeafInterface { @@ -247,6 +249,7 @@ fn static_tmtc_pool_main() { mgm_mode_leaf_interface, mgm_handler_composite_rx, pus_hk_reply_tx.clone(), + switch_helper.clone(), tm_sink_tx.clone(), mgm_spi_interface, shared_mgm_set, @@ -507,6 +510,10 @@ fn dyn_tmtc_pool_main() { let mut tm_funnel = TmSinkDynamic::new(sync_tm_tcp_source, tm_funnel_rx, tm_server_tx); + let shared_switch_set = Arc::new(Mutex::default()); + 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_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = mpsc::channel(); let shared_mgm_set = Arc::default(); @@ -531,6 +538,7 @@ fn dyn_tmtc_pool_main() { mode_leaf_interface, mgm_handler_composite_rx, pus_hk_reply_tx, + switch_helper.clone(), tm_funnel_tx, mgm_spi_interface, shared_mgm_set, diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 48eae0e..72e50b6 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -1,3 +1,5 @@ +use core::time::Duration; + use derive_new::new; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; @@ -57,28 +59,28 @@ impl From for SwitchState { pub type SwitchId = u16; /// Generic trait for a device capable of turning on and off switches. -pub trait PowerSwitcherCommandSender { +pub trait PowerSwitcherCommandSender> { type Error; fn send_switch_on_cmd( &self, requestor_info: MessageMetadata, - switch_id: SwitchId, + switch_id: SwitchType, ) -> Result<(), Self::Error>; fn send_switch_off_cmd( &self, requestor_info: MessageMetadata, - switch_id: SwitchId, + switch_id: SwitchType, ) -> Result<(), Self::Error>; } -pub trait PowerSwitchInfo { +pub trait PowerSwitchInfo { type Error; /// Retrieve the switch state - fn switch_state(&self, switch_id: SwitchId) -> Result; + fn switch_state(&self, switch_id: SwitchType) -> Result; - fn is_switch_on(&self, switch_id: SwitchId) -> Result { + fn is_switch_on(&self, switch_id: SwitchType) -> Result { Ok(self.switch_state(switch_id)? == SwitchState::On) } @@ -86,7 +88,7 @@ pub trait PowerSwitchInfo { /// /// This may take into account the time to send a command, wait for it to be executed, and /// see the switch changed. - fn switch_delay_ms(&self) -> u32; + fn switch_delay_ms(&self) -> Duration; } #[derive(new)] @@ -119,17 +121,17 @@ pub mod std_mod { pub type MpscSwitchCmdSender = mpsc::Sender>; pub type MpscSwitchCmdSenderBounded = mpsc::SyncSender>; - impl PowerSwitcherCommandSender for MpscSwitchCmdSender { + impl> PowerSwitcherCommandSender for MpscSwitchCmdSender { type Error = GenericSendError; fn send_switch_on_cmd( &self, requestor_info: MessageMetadata, - switch_id: SwitchId, + switch_id: SwitchType, ) -> Result<(), Self::Error> { self.send(GenericMessage::new( requestor_info, - SwitchRequest::new(switch_id, SwitchStateBinary::On), + SwitchRequest::new(switch_id.into(), SwitchStateBinary::On), )) .map_err(|_| GenericSendError::RxDisconnected) } @@ -137,27 +139,27 @@ pub mod std_mod { fn send_switch_off_cmd( &self, requestor_info: MessageMetadata, - switch_id: SwitchId, + switch_id: SwitchType, ) -> Result<(), Self::Error> { self.send(GenericMessage::new( requestor_info, - SwitchRequest::new(switch_id, SwitchStateBinary::Off), + SwitchRequest::new(switch_id.into(), SwitchStateBinary::Off), )) .map_err(|_| GenericSendError::RxDisconnected) } } - impl PowerSwitcherCommandSender for MpscSwitchCmdSenderBounded { + impl> PowerSwitcherCommandSender for MpscSwitchCmdSenderBounded { type Error = GenericSendError; fn send_switch_on_cmd( &self, requestor_info: MessageMetadata, - switch_id: SwitchId, + switch_id: SwitchType, ) -> Result<(), Self::Error> { self.try_send(GenericMessage::new( requestor_info, - SwitchRequest::new(switch_id, SwitchStateBinary::On), + SwitchRequest::new(switch_id.into(), SwitchStateBinary::On), )) .map_err(|e| match e { mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), @@ -168,11 +170,11 @@ pub mod std_mod { fn send_switch_off_cmd( &self, requestor_info: MessageMetadata, - switch_id: SwitchId, + switch_id: SwitchType, ) -> Result<(), Self::Error> { self.try_send(GenericMessage::new( requestor_info, - SwitchRequest::new(switch_id, SwitchStateBinary::Off), + SwitchRequest::new(switch_id.into(), SwitchStateBinary::Off), )) .map_err(|e| match e { mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), From b4febefa33a0174cfc5723733c61452aed0f0cfb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 May 2024 16:48:51 +0200 Subject: [PATCH 20/23] introduce switch handling for MGM --- satrs-example/src/acs/mgm.rs | 98 +++++++++++++++++++++++++++++++++--- satrs-example/src/eps/mod.rs | 2 +- satrs/src/power.rs | 4 +- 3 files changed, 95 insertions(+), 9 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 9780acd..4e4fd07 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -42,6 +42,14 @@ pub enum SetId { SensorData = 0, } +#[derive(Default, Debug, PartialEq, Eq)] +pub enum TransitionState { + #[default] + Idle, + PowerSwitching, + Done, +} + pub trait SpiInterface { type Error: Debug; fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>; @@ -136,6 +144,24 @@ pub struct BufWrapper { tm_buf: [u8; 32], } +pub struct ModeHelpers { + current: ModeAndSubmode, + target: Option, + requestor_info: Option, + transition_state: TransitionState, +} + +impl Default for ModeHelpers { + fn default() -> Self { + Self { + current: ModeAndSubmode::new(DeviceMode::Off as u32, 0), + target: Default::default(), + requestor_info: Default::default(), + transition_state: Default::default(), + } + } +} + /// Example MGM device handler strongly based on the LIS3MDL MEMS device. #[derive(new)] #[allow(clippy::too_many_arguments)] @@ -153,8 +179,8 @@ pub struct MgmHandlerLis3Mdl< tm_sender: TmSender, pub com_interface: ComInterface, shared_mgm_set: Arc>, - #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] - mode_and_submode: ModeAndSubmode, + #[new(default)] + mode_helpers: ModeHelpers, #[new(default)] bufs: BufWrapper, #[new(default)] @@ -172,6 +198,9 @@ impl< // Handle requests. self.handle_composite_requests(); self.handle_mode_requests(); + if let Some(target_mode_submode) = self.mode_helpers.target { + self.handle_mode_transition(target_mode_submode); + } if self.mode() == DeviceMode::Normal as u32 { log::trace!("polling LIS3MDL sensor {}", self.dev_str); self.poll_sensor(); @@ -306,6 +335,37 @@ impl< mgm_guard.valid = true; drop(mgm_guard); } + + pub fn handle_mode_transition(&mut self, target_mode_submode: ModeAndSubmode) { + if target_mode_submode.mode() == DeviceMode::On as u32 + || target_mode_submode.mode() == DeviceMode::Normal as u32 + { + if self.mode_helpers.transition_state == TransitionState::Idle { + let result = self + .switch_helper + .send_switch_on_cmd(MessageMetadata::new(0, self.id.id()), PcduSwitch::Mgm); + if result.is_err() { + // Could not send switch command.. still continue with transition. + log::error!("failed to send switch on command"); + } + self.mode_helpers.transition_state = TransitionState::PowerSwitching; + } + if self.mode_helpers.transition_state == TransitionState::PowerSwitching + && self + .switch_helper + .is_switch_on(PcduSwitch::Mgm) + .expect("switch info error") + { + self.mode_helpers.transition_state = TransitionState::Done; + } + if self.mode_helpers.transition_state == TransitionState::Done { + self.mode_helpers.current = self.mode_helpers.target.unwrap(); + self.handle_mode_reached(self.mode_helpers.requestor_info) + .expect("failed to handle mode reached"); + self.mode_helpers.transition_state = TransitionState::Idle; + } + } + } } impl< @@ -315,7 +375,7 @@ impl< > ModeProvider for MgmHandlerLis3Mdl { fn mode_and_submode(&self) -> ModeAndSubmode { - self.mode_and_submode + self.mode_helpers.current } } @@ -326,6 +386,7 @@ impl< > ModeRequestHandler for MgmHandlerLis3Mdl { type Error = ModeError; + fn start_transition( &mut self, requestor: MessageMetadata, @@ -336,11 +397,18 @@ impl< self.dev_str, mode_and_submode ); - self.mode_and_submode = mode_and_submode; + self.mode_helpers.current = mode_and_submode; if mode_and_submode.mode() == DeviceMode::Off as u32 { self.shared_mgm_set.lock().unwrap().valid = false; + self.handle_mode_reached(Some(requestor))?; + } else if mode_and_submode.mode() == DeviceMode::Normal as u32 + || mode_and_submode.mode() == DeviceMode::On as u32 + { + // TODO: Write helper method for the struct? Might help for other handlers as well.. + self.mode_helpers.transition_state = TransitionState::Idle; + self.mode_helpers.requestor_info = Some(requestor); + self.mode_helpers.target = Some(mode_and_submode); } - self.handle_mode_reached(Some(requestor))?; Ok(()) } @@ -348,7 +416,7 @@ impl< log::info!( "{} announcing mode: {:?}", self.dev_str, - self.mode_and_submode + self.mode_and_submode() ); } @@ -356,6 +424,7 @@ impl< &mut self, requestor: Option, ) -> Result<(), Self::Error> { + self.mode_helpers.target = None; self.announce_mode(requestor, false); if let Some(requestor) = requestor { if requestor.sender_id() != PUS_MODE_SERVICE.id() { @@ -403,6 +472,7 @@ mod tests { use satrs::{ mode::{ModeReply, ModeRequest}, + power::SwitchStateBinary, request::{GenericMessage, UniqueApidTargetId}, tmtc::PacketAsVec, }; @@ -516,6 +586,22 @@ mod tests { DeviceMode::Normal as u32 ); assert_eq!(testbench.handler.mode_and_submode().submode(), 0); + + // Verify power switch handling. + let mut switch_requests = testbench.handler.switch_helper.switch_requests.borrow_mut(); + assert_eq!(switch_requests.len(), 1); + let switch_req = switch_requests.pop_front().expect("no switch request"); + assert_eq!(switch_req.target_state, SwitchStateBinary::On); + assert_eq!(switch_req.switch_id, PcduSwitch::Mgm); + let mut switch_info_requests = testbench + .handler + .switch_helper + .switch_info_requests + .borrow_mut(); + assert_eq!(switch_info_requests.len(), 1); + let switch_info_req = switch_info_requests.pop_front().expect("no switch request"); + assert_eq!(switch_info_req, PcduSwitch::Mgm); + let mode_reply = testbench .mode_reply_rx_to_pus .try_recv() diff --git a/satrs-example/src/eps/mod.rs b/satrs-example/src/eps/mod.rs index ee325e4..9ff3f7d 100644 --- a/satrs-example/src/eps/mod.rs +++ b/satrs-example/src/eps/mod.rs @@ -123,7 +123,7 @@ impl Default for TestSwitchHelper { switch_delay_request_count: Default::default(), next_switch_delay: Duration::from_millis(1000), switch_map: Default::default(), - switch_map_valid: Default::default(), + switch_map_valid: true, } } } diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 72e50b6..cb2648a 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -60,7 +60,7 @@ pub type SwitchId = u16; /// Generic trait for a device capable of turning on and off switches. pub trait PowerSwitcherCommandSender> { - type Error; + type Error: core::fmt::Debug; fn send_switch_on_cmd( &self, @@ -75,7 +75,7 @@ pub trait PowerSwitcherCommandSender> { } pub trait PowerSwitchInfo { - type Error; + type Error: core::fmt::Debug; /// Retrieve the switch state fn switch_state(&self, switch_id: SwitchType) -> Result; From 2507469e682b112608c63e07109a7bdc6404054a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 May 2024 18:34:37 +0200 Subject: [PATCH 21/23] continue PCDU integration --- satrs-example/src/acs/mgm.rs | 2 +- satrs-example/src/eps/mod.rs | 17 ++---- satrs-example/src/eps/pcdu.rs | 14 +++-- satrs-example/src/main.rs | 106 ++++++++++++++++++++++++++-------- satrs-example/src/requests.rs | 5 +- 5 files changed, 99 insertions(+), 45 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 4e4fd07..aa85dad 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -134,7 +134,7 @@ pub struct MgmData { pub struct MpscModeLeafInterface { pub request_rx: mpsc::Receiver>, pub reply_to_pus_tx: mpsc::Sender>, - pub reply_to_parent_tx: mpsc::Sender>, + pub reply_to_parent_tx: mpsc::SyncSender>, } #[derive(Default)] diff --git a/satrs-example/src/eps/mod.rs b/satrs-example/src/eps/mod.rs index 9ff3f7d..351cf76 100644 --- a/satrs-example/src/eps/mod.rs +++ b/satrs-example/src/eps/mod.rs @@ -1,16 +1,9 @@ use derive_new::new; -use std::{ - borrow::BorrowMut, - cell::RefCell, - collections::VecDeque, - sync::mpsc, - time::{Duration, Instant}, -}; +use std::{cell::RefCell, collections::VecDeque, sync::mpsc, time::Duration}; use satrs::{ power::{ - PowerSwitchInfo, PowerSwitcherCommandSender, SwitchId, SwitchRequest, SwitchState, - SwitchStateBinary, + PowerSwitchInfo, PowerSwitcherCommandSender, SwitchRequest, SwitchState, SwitchStateBinary, }, queue::GenericSendError, request::{GenericMessage, MessageMetadata}, @@ -26,17 +19,14 @@ pub mod pcdu; pub struct PowerSwitchHelper { switcher_tx: mpsc::SyncSender>, shared_switch_set: SharedSwitchSet, - #[new(default)] - switch_cmd_sent_instant: Option, } #[derive(Debug, Error, Copy, Clone, PartialEq, Eq)] pub enum SwitchCommandingError { - #[error("invalid switch id")] - InvalidSwitchId(SwitchId), #[error("send error: {0}")] Send(#[from] GenericSendError), } + #[derive(Debug, Error, Copy, Clone, PartialEq, Eq)] pub enum SwitchInfoError { /// This is a configuration error which should not occur. @@ -196,6 +186,7 @@ impl PowerSwitcherCommandSender for TestSwitchHelper { } } +#[allow(dead_code)] impl TestSwitchHelper { // Helper function which can be used to force a switch to another state for test purposes. pub fn set_switch_state(&mut self, switch: PcduSwitch, state: SwitchState) { diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 639b2cd..6684648 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -22,7 +22,8 @@ use satrs_minisim::{ use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; pub trait SerialInterface { - type Error; + type Error: core::fmt::Debug; + /// Send some data via the serial interface. fn send(&self, data: &[u8]) -> Result<(), Self::Error>; /// Receive all replies received on the serial interface so far. This function takes a closure @@ -289,13 +290,18 @@ impl PcduHandler todo!("failed to convert switch ID {:?} to typed PCDU switch", e), }, Err(e) => match e { - mpsc::TryRecvError::Empty => todo!(), - mpsc::TryRecvError::Disconnected => todo!(), + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + log::warn!("switch request receiver has disconnected"); + break; + } }, }; } diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 00e704c..1782bb6 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -76,13 +76,14 @@ fn static_tmtc_pool_main() { let mut opt_sim_client = create_sim_client(sim_request_rx); let (mgm_handler_composite_tx, mgm_handler_composite_rx) = - mpsc::channel::>(); + mpsc::sync_channel::>(10); let (pcdu_handler_composite_tx, pcdu_handler_composite_rx) = - mpsc::channel::>(); + mpsc::sync_channel::>(30); - let (mgm_handler_mode_tx, mgm_handler_mode_rx) = mpsc::channel::>(); + let (mgm_handler_mode_tx, mgm_handler_mode_rx) = + mpsc::sync_channel::>(5); let (pcdu_handler_mode_tx, pcdu_handler_mode_rx) = - mpsc::channel::>(); + mpsc::sync_channel::>(5); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let mut request_map = GenericRequestRouter::default(); @@ -221,7 +222,7 @@ fn static_tmtc_pool_main() { ); let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = - mpsc::channel(); + mpsc::sync_channel(5); let shared_switch_set = Arc::new(Mutex::default()); let (switch_request_tx, switch_request_rx) = mpsc::sync_channel(20); @@ -256,7 +257,7 @@ fn static_tmtc_pool_main() { ); let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = - mpsc::channel(); + mpsc::sync_channel(10); let pcdu_mode_leaf_interface = MpscModeLeafInterface { request_rx: pcdu_handler_mode_rx, reply_to_pus_tx: pus_mode_reply_tx, @@ -388,27 +389,38 @@ fn static_tmtc_pool_main() { #[allow(dead_code)] fn dyn_tmtc_pool_main() { let (tc_source_tx, tc_source_rx) = mpsc::channel(); - let (tm_funnel_tx, tm_funnel_rx) = mpsc::channel(); + let (tm_sink_tx, tm_sink_rx) = mpsc::channel(); let (tm_server_tx, tm_server_rx) = mpsc::channel(); let (sim_request_tx, sim_request_rx) = mpsc::channel(); let (mgm_sim_reply_tx, mgm_sim_reply_rx) = mpsc::channel(); - // let (pcdu_sim_reply_tx, pcdu_sim_reply_rx) = mpsc::channel(); + let (pcdu_sim_reply_tx, pcdu_sim_reply_rx) = mpsc::channel(); let mut opt_sim_client = create_sim_client(sim_request_rx); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let (mgm_handler_composite_tx, mgm_handler_composite_rx) = - mpsc::channel::>(); - let (mgm_handler_mode_tx, mgm_handler_mode_rx) = mpsc::channel::>(); + mpsc::sync_channel::>(5); + let (pcdu_handler_composite_tx, pcdu_handler_composite_rx) = + mpsc::sync_channel::>(10); + let (mgm_handler_mode_tx, mgm_handler_mode_rx) = + mpsc::sync_channel::>(5); + let (pcdu_handler_mode_tx, pcdu_handler_mode_rx) = + mpsc::sync_channel::>(10); // Some request are targetable. This map is used to retrieve sender handles based on a target ID. let mut request_map = GenericRequestRouter::default(); request_map .composite_router_map - .insert(MGM_HANDLER_0.raw(), mgm_handler_composite_tx); + .insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx); request_map .mode_router_map - .insert(MGM_HANDLER_0.raw(), mgm_handler_mode_tx); + .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx); + request_map + .composite_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx); + request_map + .mode_router_map + .insert(PCDU_HANDLER.id(), pcdu_handler_mode_tx); // Create event handling components // These sender handles are used to send event requests, for example to enable or disable @@ -417,7 +429,7 @@ fn dyn_tmtc_pool_main() { let (event_request_tx, event_request_rx) = mpsc::channel::(); // The event task is the core handler to perform the event routing and TM handling as specified // in the sat-rs documentation. - let mut event_handler = EventHandler::new(tm_funnel_tx.clone(), event_rx, event_request_rx); + let mut event_handler = EventHandler::new(tm_sink_tx.clone(), event_rx, event_request_rx); let (pus_test_tx, pus_test_rx) = mpsc::channel(); let (pus_event_tx, pus_event_rx) = mpsc::channel(); @@ -440,30 +452,30 @@ fn dyn_tmtc_pool_main() { }; let pus_test_service = - create_test_service_dynamic(tm_funnel_tx.clone(), event_tx.clone(), pus_test_rx); + create_test_service_dynamic(tm_sink_tx.clone(), event_tx.clone(), pus_test_rx); let pus_scheduler_service = create_scheduler_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), tc_source_tx.clone(), pus_sched_rx, create_sched_tc_pool(), ); let pus_event_service = - create_event_service_dynamic(tm_funnel_tx.clone(), pus_event_rx, event_request_tx); + create_event_service_dynamic(tm_sink_tx.clone(), pus_event_rx, event_request_tx); let pus_action_service = create_action_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), pus_action_rx, request_map.clone(), pus_action_reply_rx, ); let pus_hk_service = create_hk_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), pus_hk_rx, request_map.clone(), pus_hk_reply_rx, ); let pus_mode_service = create_mode_service_dynamic( - tm_funnel_tx.clone(), + tm_sink_tx.clone(), pus_mode_rx, request_map, pus_mode_reply_rx, @@ -479,7 +491,7 @@ fn dyn_tmtc_pool_main() { let mut tmtc_task = TcSourceTaskDynamic::new( tc_source_rx, - PusTcDistributor::new(tm_funnel_tx.clone(), pus_router), + PusTcDistributor::new(tm_sink_tx.clone(), pus_router), ); let sock_addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT); @@ -508,18 +520,18 @@ fn dyn_tmtc_pool_main() { ) .expect("tcp server creation failed"); - let mut tm_funnel = TmSinkDynamic::new(sync_tm_tcp_source, tm_funnel_rx, tm_server_tx); + let mut tm_funnel = TmSinkDynamic::new(sync_tm_tcp_source, tm_sink_rx, tm_server_tx); let shared_switch_set = Arc::new(Mutex::default()); 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_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) = - mpsc::channel(); + mpsc::sync_channel(5); let shared_mgm_set = Arc::default(); let mode_leaf_interface = MpscModeLeafInterface { request_rx: mgm_handler_mode_rx, - reply_to_pus_tx: pus_mode_reply_tx, + reply_to_pus_tx: pus_mode_reply_tx.clone(), reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx, }; @@ -537,13 +549,41 @@ fn dyn_tmtc_pool_main() { "MGM_0", mode_leaf_interface, mgm_handler_composite_rx, - pus_hk_reply_tx, + pus_hk_reply_tx.clone(), switch_helper.clone(), - tm_funnel_tx, + tm_sink_tx.clone(), mgm_spi_interface, shared_mgm_set, ); + let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) = + mpsc::sync_channel(10); + let pcdu_mode_leaf_interface = MpscModeLeafInterface { + request_rx: pcdu_handler_mode_rx, + reply_to_pus_tx: pus_mode_reply_tx, + reply_to_parent_tx: pcdu_handler_mode_reply_to_parent_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); + SerialSimInterfaceWrapper::Sim(SerialInterfaceToSim::new( + sim_request_tx.clone(), + pcdu_sim_reply_rx, + )) + } else { + SerialSimInterfaceWrapper::Dummy(SerialInterfaceDummy::default()) + }; + let mut pcdu_handler = PcduHandler::new( + PCDU_HANDLER, + "PCDU", + pcdu_mode_leaf_interface, + pcdu_handler_composite_rx, + pus_hk_reply_tx, + switch_request_rx, + tm_sink_tx, + pcdu_serial_interface, + shared_switch_set, + ); + info!("Starting TMTC and UDP task"); let jh_udp_tmtc = thread::Builder::new() .name("sat-rs tmtc-udp".to_string()) @@ -600,6 +640,21 @@ fn dyn_tmtc_pool_main() { }) .unwrap(); + info!("Starting EPS thread"); + let jh_eps = thread::Builder::new() + .name("sat-rs 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. + pcdu_handler.periodic_operation(eps::pcdu::OpCode::RegularOp); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(50)); + pcdu_handler.periodic_operation(eps::pcdu::OpCode::PollAndRecvReplies); + thread::sleep(Duration::from_millis(300)); + }) + .unwrap(); + info!("Starting PUS handler thread"); let jh_pus_handler = thread::Builder::new() .name("sat-rs pus".to_string()) @@ -625,6 +680,7 @@ fn dyn_tmtc_pool_main() { .expect("Joining SIM client thread failed"); } jh_aocs.join().expect("Joining AOCS thread failed"); + jh_eps.join().expect("Joining EPS thread failed"); jh_pus_handler .join() .expect("Joining PUS handler thread failed"); diff --git a/satrs-example/src/requests.rs b/satrs-example/src/requests.rs index 445e05e..316a486 100644 --- a/satrs-example/src/requests.rs +++ b/satrs-example/src/requests.rs @@ -28,8 +28,9 @@ pub enum CompositeRequest { pub struct GenericRequestRouter { pub id: ComponentId, // All messages which do not have a dedicated queue. - pub composite_router_map: HashMap>>, - pub mode_router_map: HashMap>>, + pub composite_router_map: + HashMap>>, + pub mode_router_map: HashMap>>, } impl Default for GenericRequestRouter { From 2a2a3a3eabd2af2ed733717a0751380794432681 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 May 2024 18:48:46 +0200 Subject: [PATCH 22/23] PCDU switch set TM handling --- satrs-example/src/acs/mgm.rs | 2 +- satrs-example/src/eps/pcdu.rs | 70 +++++++++++++++++++++++++++++++-- satrs-example/src/pus/action.rs | 2 +- 3 files changed, 68 insertions(+), 6 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index aa85dad..51b4fca 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -519,7 +519,7 @@ mod tests { pub fn new() -> Self { let (request_tx, request_rx) = mpsc::channel(); let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel(); - let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::channel(); + let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5); let mode_interface = MpscModeLeafInterface { request_rx, reply_to_pus_tx: reply_tx_to_pus, diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 6684648..8dce829 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -5,21 +5,34 @@ use std::{ }; use derive_new::new; +use num_enum::{IntoPrimitive, TryFromPrimitive}; use satrs::{ hk::{HkRequest, HkRequestVariant}, mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler}, power::SwitchRequest, - pus::EcssTmSender, + pus::{EcssTmSender, PusTmVariant}, queue::{GenericSendError, GenericTargetedMessagingError}, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, + spacepackets::{ + ecss::{ + hk, + tm::{PusTmCreator, PusTmSecondaryHeader}, + }, + SpHeader, + }, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; use satrs_minisim::{ eps::{PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinaryWrapper}, SerializableSimMsgPayload, SimReply, SimRequest, }; +use serde::{Deserialize, Serialize}; -use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; +use crate::{ + acs::mgm::MpscModeLeafInterface, + pus::hk::{HkReply, HkReplyVariant}, + requests::CompositeRequest, +}; pub trait SerialInterface { type Error: core::fmt::Debug; @@ -40,6 +53,12 @@ pub struct SerialInterfaceToSim { pub sim_reply_rx: mpsc::Receiver, } +#[derive(Debug, Copy, Clone, PartialEq, Eq, TryFromPrimitive, IntoPrimitive)] +#[repr(u32)] +pub enum SetId { + SwitcherSet = 0, +} + impl SerialInterface for SerialInterfaceToSim { type Error = (); @@ -162,7 +181,7 @@ pub enum OpCode { PollAndRecvReplies = 1, } -#[derive(Clone, PartialEq, Eq, Default)] +#[derive(Clone, PartialEq, Eq, Default, Serialize, Deserialize)] pub struct SwitchSet { pub valid: bool, pub switch_map: SwitchMap, @@ -187,6 +206,8 @@ pub struct PcduHandler { mode_and_submode: ModeAndSubmode, #[new(default)] stamp_helper: TimestampHelper, + #[new(value = "[0; 256]")] + tm_buf: [u8; 256], } impl PcduHandler { @@ -239,7 +260,48 @@ impl PcduHandler todo!(), + HkRequestVariant::OneShot => { + if hk_request.unique_id == SetId::SwitcherSet as u32 { + self.hk_reply_tx + .send(GenericMessage::new( + *requestor_info, + HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), + )) + .expect("failed to send HK reply"); + let sec_header = PusTmSecondaryHeader::new( + 3, + hk::Subservice::TmHkPacket as u8, + 0, + 0, + self.stamp_helper.stamp(), + ); + // Send TM down as JSON. + let switch_map_snapshot = self + .shared_switch_map + .lock() + .expect("failed to lock switch map") + .clone(); + let switch_map_json = serde_json::to_string(&switch_map_snapshot) + .expect("failed to serialize switch map"); + if switch_map_json.len() > self.tm_buf.len() { + log::error!("switch map JSON too large for telemetry buffer"); + return; + } + self.tm_buf[0..4].copy_from_slice(&self.id.unique_id.to_be_bytes()); + self.tm_buf[4..8].copy_from_slice(&(SetId::SwitcherSet as u32).to_be_bytes()); + self.tm_buf[8..8 + switch_map_json.len()] + .copy_from_slice(switch_map_json.as_bytes()); + let hk_tm = PusTmCreator::new( + SpHeader::new_from_apid(self.id.apid), + sec_header, + &self.tm_buf[0..8 + switch_map_json.len()], + true, + ); + self.tm_sender + .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) + .expect("failed to send HK TM"); + } + } HkRequestVariant::EnablePeriodic => todo!(), HkRequestVariant::DisablePeriodic => todo!(), HkRequestVariant::ModifyCollectionInterval(_) => todo!(), diff --git a/satrs-example/src/pus/action.rs b/satrs-example/src/pus/action.rs index 03d362c..238f1c5 100644 --- a/satrs-example/src/pus/action.rs +++ b/satrs-example/src/pus/action.rs @@ -341,7 +341,7 @@ mod tests { let (tm_funnel_tx, tm_funnel_rx) = mpsc::channel(); let (pus_action_tx, pus_action_rx) = mpsc::channel(); let (action_reply_tx, action_reply_rx) = mpsc::channel(); - let (action_req_tx, action_req_rx) = mpsc::channel(); + let (action_req_tx, action_req_rx) = mpsc::sync_channel(10); let verif_reporter = TestVerificationReporter::new(owner_id); let mut generic_req_router = GenericRequestRouter::default(); generic_req_router From 29783b2b0783daa81205bee1763ed44fda339f97 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 25 May 2024 12:29:44 +0200 Subject: [PATCH 23/23] introduce new HK helper --- satrs-example/src/acs/mgm.rs | 64 ++++++++++----------- satrs-example/src/eps/pcdu.rs | 101 +++++++++++++++++++--------------- satrs-example/src/hk.rs | 36 +++++++++++- satrs-example/src/pus/hk.rs | 11 ++++ 4 files changed, 131 insertions(+), 81 deletions(-) diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 51b4fca..1b0f3b5 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -2,9 +2,6 @@ use derive_new::new; use satrs::hk::{HkRequest, HkRequestVariant}; use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender}; use satrs::queue::{GenericSendError, GenericTargetedMessagingError}; -use satrs::spacepackets::ecss::hk; -use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; -use satrs::spacepackets::SpHeader; use satrs_example::{DeviceMode, TimestampHelper}; use satrs_minisim::acs::lis3mdl::{ MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, @@ -24,6 +21,7 @@ use satrs::pus::{EcssTmSender, PusTmVariant}; use satrs::request::{GenericMessage, MessageMetadata, UniqueApidTargetId}; use satrs_example::config::components::PUS_MODE_SERVICE; +use crate::hk::PusHkHelper; use crate::pus::hk::{HkReply, HkReplyVariant}; use crate::requests::CompositeRequest; @@ -179,6 +177,8 @@ pub struct MgmHandlerLis3Mdl< tm_sender: TmSender, pub com_interface: ComInterface, shared_mgm_set: Arc>, + #[new(value = "PusHkHelper::new(id)")] + hk_helper: PusHkHelper, #[new(default)] mode_helpers: ModeHelpers, #[new(default)] @@ -237,39 +237,33 @@ impl< pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) { match hk_request.variant { HkRequestVariant::OneShot => { - // TODO: We should provide a helper class for generating some of the boilerplate. - // This includes the APID, subservice, unique ID and set ID handling. The user - // should be able to simply specify the HK data as a slice. - self.hk_reply_tx - .send(GenericMessage::new( - *requestor_info, - HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), - )) - .expect("failed to send HK reply"); - let sec_header = PusTmSecondaryHeader::new( - 3, - hk::Subservice::TmHkPacket as u8, - 0, - 0, - self.stamp_helper.stamp(), - ); let mgm_snapshot = *self.shared_mgm_set.lock().unwrap(); - self.bufs.tm_buf[0..4].copy_from_slice(&self.id.unique_id.to_be_bytes()); - self.bufs.tm_buf[4..8].copy_from_slice(&(SetId::SensorData as u32).to_be_bytes()); - // Use binary serialization here. We want the data to be tightly packed. - self.bufs.tm_buf[8] = mgm_snapshot.valid as u8; - self.bufs.tm_buf[9..13].copy_from_slice(&mgm_snapshot.x.to_be_bytes()); - self.bufs.tm_buf[13..17].copy_from_slice(&mgm_snapshot.y.to_be_bytes()); - self.bufs.tm_buf[17..21].copy_from_slice(&mgm_snapshot.z.to_be_bytes()); - let hk_tm = PusTmCreator::new( - SpHeader::new_from_apid(self.id.apid), - sec_header, - &self.bufs.tm_buf[0..21], - true, - ); - self.tm_sender - .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) - .expect("failed to send HK TM"); + if let Ok(hk_tm) = self.hk_helper.generate_hk_report_packet( + self.stamp_helper.stamp(), + SetId::SensorData as u32, + &mut |hk_buf| { + hk_buf[0] = mgm_snapshot.valid as u8; + hk_buf[1..5].copy_from_slice(&mgm_snapshot.x.to_be_bytes()); + hk_buf[5..9].copy_from_slice(&mgm_snapshot.y.to_be_bytes()); + hk_buf[9..13].copy_from_slice(&mgm_snapshot.z.to_be_bytes()); + Ok(13) + }, + &mut self.bufs.tm_buf, + ) { + // TODO: If sending the TM fails, we should also send a failure reply. + self.tm_sender + .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) + .expect("failed to send HK TM"); + self.hk_reply_tx + .send(GenericMessage::new( + *requestor_info, + HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), + )) + .expect("failed to send HK reply"); + } else { + // TODO: Send back failure reply. Need result code for this. + log::error!("TM buffer too small to generate HK data"); + } } HkRequestVariant::EnablePeriodic => todo!(), HkRequestVariant::DisablePeriodic => todo!(), diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs index 8dce829..d51ca55 100644 --- a/satrs-example/src/eps/pcdu.rs +++ b/satrs-example/src/eps/pcdu.rs @@ -13,23 +13,20 @@ use satrs::{ pus::{EcssTmSender, PusTmVariant}, queue::{GenericSendError, GenericTargetedMessagingError}, request::{GenericMessage, MessageMetadata, UniqueApidTargetId}, - spacepackets::{ - ecss::{ - hk, - tm::{PusTmCreator, PusTmSecondaryHeader}, - }, - SpHeader, - }, + spacepackets::ByteConversionError, }; use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper}; use satrs_minisim::{ - eps::{PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinaryWrapper}, + eps::{ + PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinaryWrapper, SwitchMapWrapper, + }, SerializableSimMsgPayload, SimReply, SimRequest, }; use serde::{Deserialize, Serialize}; use crate::{ acs::mgm::MpscModeLeafInterface, + hk::PusHkHelper, pus::hk::{HkReply, HkReplyVariant}, requests::CompositeRequest, }; @@ -202,6 +199,8 @@ pub struct PcduHandler { tm_sender: TmSender, pub com_interface: ComInterface, shared_switch_map: Arc>, + #[new(value = "PusHkHelper::new(id)")] + hk_helper: PusHkHelper, #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] mode_and_submode: ModeAndSubmode, #[new(default)] @@ -262,44 +261,39 @@ impl PcduHandler { if hk_request.unique_id == SetId::SwitcherSet as u32 { - self.hk_reply_tx - .send(GenericMessage::new( - *requestor_info, - HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), - )) - .expect("failed to send HK reply"); - let sec_header = PusTmSecondaryHeader::new( - 3, - hk::Subservice::TmHkPacket as u8, - 0, - 0, + if let Ok(hk_tm) = self.hk_helper.generate_hk_report_packet( self.stamp_helper.stamp(), - ); - // Send TM down as JSON. - let switch_map_snapshot = self - .shared_switch_map - .lock() - .expect("failed to lock switch map") - .clone(); - let switch_map_json = serde_json::to_string(&switch_map_snapshot) - .expect("failed to serialize switch map"); - if switch_map_json.len() > self.tm_buf.len() { - log::error!("switch map JSON too large for telemetry buffer"); - return; + SetId::SwitcherSet as u32, + &mut |hk_buf| { + // Send TM down as JSON. + let switch_map_snapshot = self + .shared_switch_map + .lock() + .expect("failed to lock switch map") + .clone(); + let switch_map_json = serde_json::to_string(&switch_map_snapshot) + .expect("failed to serialize switch map"); + if switch_map_json.len() > hk_buf.len() { + log::error!("switch map JSON too large for HK buffer"); + return Err(ByteConversionError::ToSliceTooSmall { + found: hk_buf.len(), + expected: switch_map_json.len(), + }); + } + Ok(switch_map_json.len()) + }, + &mut self.tm_buf, + ) { + self.tm_sender + .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) + .expect("failed to send HK TM"); + self.hk_reply_tx + .send(GenericMessage::new( + *requestor_info, + HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), + )) + .expect("failed to send HK reply"); } - self.tm_buf[0..4].copy_from_slice(&self.id.unique_id.to_be_bytes()); - self.tm_buf[4..8].copy_from_slice(&(SetId::SwitcherSet as u32).to_be_bytes()); - self.tm_buf[8..8 + switch_map_json.len()] - .copy_from_slice(switch_map_json.as_bytes()); - let hk_tm = PusTmCreator::new( - SpHeader::new_from_apid(self.id.apid), - sec_header, - &self.tm_buf[0..8 + switch_map_json.len()], - true, - ); - self.tm_sender - .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) - .expect("failed to send HK TM"); } } HkRequestVariant::EnablePeriodic => todo!(), @@ -369,7 +363,24 @@ impl PcduHandler { + let switch_map_wrapper = + SwitchMapWrapper::from_binary_switch_map_ref(&switch_info); + self.shared_switch_map + .lock() + .expect("failed to lock switch map") + .switch_map = switch_map_wrapper.0; + } + } + }) { + log::warn!("receiving PCDU replies failed: {:?}", e); + } + } } impl ModeProvider diff --git a/satrs-example/src/hk.rs b/satrs-example/src/hk.rs index 0852d04..bfad5e8 100644 --- a/satrs-example/src/hk.rs +++ b/satrs-example/src/hk.rs @@ -1,7 +1,9 @@ use derive_new::new; use satrs::hk::UniqueId; use satrs::request::UniqueApidTargetId; -use satrs::spacepackets::ByteConversionError; +use satrs::spacepackets::ecss::hk; +use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; +use satrs::spacepackets::{ByteConversionError, SpHeader}; #[derive(Debug, new, Copy, Clone)] pub struct HkUniqueId { @@ -33,3 +35,35 @@ impl HkUniqueId { Ok(8) } } + +#[derive(new)] +pub struct PusHkHelper { + component_id: UniqueApidTargetId, +} + +impl PusHkHelper { + pub fn generate_hk_report_packet< + 'a, + 'b, + HkWriter: FnMut(&mut [u8]) -> Result, + >( + &self, + timestamp: &'a [u8], + set_id: u32, + hk_data_writer: &mut HkWriter, + buf: &'b mut [u8], + ) -> Result, ByteConversionError> { + let sec_header = + PusTmSecondaryHeader::new(3, hk::Subservice::TmHkPacket as u8, 0, 0, timestamp); + buf[0..4].copy_from_slice(&self.component_id.unique_id.to_be_bytes()); + buf[4..8].copy_from_slice(&set_id.to_be_bytes()); + let (_, second_half) = buf.split_at_mut(8); + let hk_data_len = hk_data_writer(second_half)?; + Ok(PusTmCreator::new( + SpHeader::new_from_apid(self.component_id.apid), + sec_header, + &buf[0..8 + hk_data_len], + true, + )) + } +} diff --git a/satrs-example/src/pus/hk.rs b/satrs-example/src/pus/hk.rs index 0092241..2796151 100644 --- a/satrs-example/src/pus/hk.rs +++ b/satrs-example/src/pus/hk.rs @@ -12,6 +12,7 @@ use satrs::pus::{ PusPacketHandlingError, PusReplyHandler, PusServiceHelper, PusTcToRequestConverter, }; use satrs::request::{GenericMessage, UniqueApidTargetId}; +use satrs::res_code::ResultU16; use satrs::spacepackets::ecss::tc::PusTcReader; use satrs::spacepackets::ecss::{hk, PusPacket, PusServiceId}; use satrs::tmtc::{PacketAsVec, PacketSenderWithSharedPool}; @@ -34,6 +35,7 @@ pub struct HkReply { #[derive(Clone, PartialEq, Debug)] pub enum HkReplyVariant { Ack, + Failed(ResultU16), } #[derive(Default)] @@ -69,6 +71,15 @@ impl PusReplyHandler for HkReplyHandler { .completion_success(tm_sender, started_token, time_stamp) .expect("sending completion success verification failed"); } + HkReplyVariant::Failed(failure_code) => { + verification_handler + .completion_failure( + tm_sender, + started_token, + FailParams::new(time_stamp, &failure_code, &[]), + ) + .expect("sending completion success verification failed"); + } }; Ok(true) }