From c20163b10a8fd0c2f7cb48731839160ec57fb7e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 May 2024 20:38:45 +0200 Subject: [PATCH] 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();