From 2ca6684f7aeff7e1ba119b7c6803dcb724adc9f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Feb 2024 18:17:25 +0100 Subject: [PATCH 01/10] lets go --- Cargo.toml | 1 + satrs-minisim/Cargo.toml | 12 +++ satrs-minisim/src/main.rs | 168 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 181 insertions(+) create mode 100644 satrs-minisim/Cargo.toml create mode 100644 satrs-minisim/src/main.rs diff --git a/Cargo.toml b/Cargo.toml index 6cd72ab..10ec0a1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,6 +4,7 @@ members = [ "satrs", "satrs-mib", "satrs-example", + "satrs-minisim", "satrs-shared", ] diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml new file mode 100644 index 0000000..5c11741 --- /dev/null +++ b/satrs-minisim/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "satrs-minisim" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +asynchronix = "0.2.0" +serde = { version = "1", features = ["derive"] } +serde_json = "1" +log = "0.4" diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs new file mode 100644 index 0000000..ad17974 --- /dev/null +++ b/satrs-minisim/src/main.rs @@ -0,0 +1,168 @@ +use asynchronix::model::{Model, Output}; +use asynchronix::simulation::{EventSlot, Mailbox, SimInit}; +use asynchronix::time::{MonotonicTime, Scheduler}; +use log::warn; +use serde::{Deserialize, Serialize}; +use std::f64::consts::PI; +use std::net::UdpSocket; +use std::time::Duration; +use std::{io, thread}; + +#[derive(Debug, Clone, PartialEq, Serialize)] +pub struct MgmTuple { + x: f64, + y: f64, + z: f64, +} + +// Earth magnetic field varies between -30 uT and 30 uT +const AMPLITUDE_MGM: f64 = 0.03; +// Lets start with a simple frequency here. +const FREQUENCY_MGM: f64 = 1.0; +const PHASE_X: f64 = 0.0; +// Different phases to have different values on the other axes. +const PHASE_Y: f64 = 0.1; +const PHASE_Z: f64 = 0.2; + +#[derive(Default)] +pub struct SimMgm { + pub output: Output, +} + +// A UDP server which exposes all values generated by the simulator. +pub struct UdpServer { + socket: UdpSocket, + mgm_out: EventSlot, +} + +#[derive(Serialize, Deserialize)] +pub struct ValueRequest { + device: String, +} + +#[derive(Serialize, Deserialize)] +pub struct ValueReply { + device: String, + reply: String, +} + +const MGM_DEV_STR: &str = "mgm"; + +impl UdpServer { + pub fn new(mgm_out: EventSlot) -> io::Result { + let socket = UdpSocket::bind("0.0.0.0:7303")?; + Ok(Self { socket, mgm_out }) + } + + pub fn run(&mut self) { + loop { + let mut buffer = [0u8; 1024]; // Buffer to store incoming data. + + // Block until data is received. `recv_from` returns the number of bytes read and the sender's address. + let (bytes_read, src) = self + .socket + .recv_from(&mut buffer) + .expect("could not read from socket"); + + // Convert the buffer into a string slice and print the message. + let req_string = std::str::from_utf8(&buffer[..bytes_read]) + .expect("Could not write buffer as string"); + println!("Received from {}: {}", src, req_string); + let value_result: serde_json::Result = serde_json::from_str(req_string); + match value_result { + Ok(value) => { + if value.device == MGM_DEV_STR { + let tuple = self.mgm_out.take().expect("expected output"); + let reply = ValueReply { + device: MGM_DEV_STR.to_string(), + reply: serde_json::to_string(&tuple).unwrap(), + }; + let reply_string = + serde_json::to_string(&reply).expect("generating reply string failed"); + self.socket + .send_to(reply_string.as_bytes(), src) + .expect("could not send data"); + } + } + Err(e) => { + warn!("received UDP request with invalid format: {e}"); + } + } + } + } +} + +pub fn current_millis(time: MonotonicTime) -> u64 { + (time.as_secs() as u64 * 1000) + (time.subsec_nanos() as u64 / 1_000_000) +} + +impl SimMgm { + fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmTuple { + let base_sin_val = 2.0 * PI * FREQUENCY_MGM * (time_ms as f64 / 1000.0); + MgmTuple { + 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(), + } + } + + // Simple unit input to request MGM tuple for current time. + pub async fn input(&mut self, _: (), scheduler: &Scheduler) { + let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); + self.output.send(value).await; + } +} + +impl Model for SimMgm { + fn init( + self, + scheduler: &Scheduler, + ) -> std::pin::Pin< + Box< + dyn std::future::Future> + + Send + + '_, + >, + > { + //scheduler.schedule_periodic_event(Duration::from_secs(1), Self::send, value).unwrap(); + Box::pin(async move { + let _ = scheduler; // suppress the unused argument warning + self.into() + }) + } +} + +fn main() { + // Instantiate models and their mailboxes. + let mut mgm_sim = SimMgm::default(); + + let mgm_mailbox = Mailbox::new(); + let mgm_input_addr = mgm_mailbox.address(); + + // Keep handles to the main input and output. + let output_slot = mgm_sim.output.connect_slot().0; + let mut output_slot_2 = mgm_sim.output.connect_slot().0; + + // Instantiate the simulator + let t0 = MonotonicTime::EPOCH; // arbitrary start time + let mut simu = SimInit::new().add_model(mgm_sim, mgm_mailbox).init(t0); + + // This thread schedules the simulator. + thread::spawn(move || { + simu.send_event(SimMgm::input, (), &mgm_input_addr); + let mut tuple = output_slot_2.take().expect("expected output"); + println!("output at {:?}: {tuple:?}", simu.time()); + for _ in 0..100 { + simu.step_by(Duration::from_millis(100)); + simu.send_event(SimMgm::input, (), &mgm_input_addr); + tuple = output_slot_2.take().expect("expected output"); + println!("output at {:?}: {tuple:?}", simu.time()); + } + }); + + // This thread manages the simulator UDP server. + thread::spawn(move || { + let mut server = UdpServer::new(output_slot).unwrap(); + server.run(); + }); +} -- 2.43.0 From db814189a0b879ccdf9544b594fb54017a2aef52 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 5 Mar 2024 00:43:01 +0100 Subject: [PATCH 02/10] add MGT and PCDU model --- satrs-minisim/Cargo.toml | 5 +- satrs-minisim/src/main.rs | 186 +++++++++++++++++++++++++++----------- 2 files changed, 137 insertions(+), 54 deletions(-) diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index 5c11741..c272fee 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -6,7 +6,10 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -asynchronix = "0.2.0" +asynchronix = "0.2" serde = { version = "1", features = ["derive"] } serde_json = "1" log = "0.4" + +[dependencies.satrs] +path = "../satrs" diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index ad17974..64bca79 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -2,33 +2,149 @@ use asynchronix::model::{Model, Output}; use asynchronix::simulation::{EventSlot, Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, Scheduler}; use log::warn; +use satrs::power::SwitchState; use serde::{Deserialize, Serialize}; use std::f64::consts::PI; use std::net::UdpSocket; use std::time::Duration; use std::{io, thread}; -#[derive(Debug, Clone, PartialEq, Serialize)] +// 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. +#[derive(Debug, Copy, Clone, PartialEq, Serialize)] pub struct MgmTuple { - x: f64, - y: f64, - z: f64, + x: f32, + y: f32, + z: f32, } // Earth magnetic field varies between -30 uT and 30 uT -const AMPLITUDE_MGM: f64 = 0.03; +const AMPLITUDE_MGM: f32 = 0.03; // Lets start with a simple frequency here. -const FREQUENCY_MGM: f64 = 1.0; -const PHASE_X: f64 = 0.0; +const FREQUENCY_MGM: f32 = 1.0; +const PHASE_X: f32 = 0.0; // Different phases to have different values on the other axes. -const PHASE_Y: f64 = 0.1; -const PHASE_Z: f64 = 0.2; +const PHASE_Y: f32 = 0.1; +const PHASE_Z: f32 = 0.2; -#[derive(Default)] -pub struct SimMgm { - pub output: Output, +pub struct MagnetometerModel { + pub switch_state: SwitchState, + pub external_mag_field: Option, + pub sensor_values: Output, } +impl Default for MagnetometerModel { + fn default() -> Self { + Self { + switch_state: SwitchState::Off, + external_mag_field: None, + sensor_values: Default::default(), + } + } +} + +impl MagnetometerModel { + fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmTuple { + if let SwitchState::On = self.switch_state { + if let Some(ext_field) = self.external_mag_field { + return ext_field; + } + let base_sin_val = 2.0 * PI as f32 * FREQUENCY_MGM * (time_ms as f32 / 1000.0); + return MgmTuple { + 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(), + }; + } + MgmTuple { + x: 0.0, + y: 0.0, + z: 0.0, + } + } + + pub async fn switch_device(&mut self, switch_state: SwitchState) { + self.switch_state = switch_state; + } + + // Simple unit input to request MGM tuple for current time. + pub async fn generate_output(&mut self, _: (), scheduler: &Scheduler) { + let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); + self.sensor_values.send(value).await; + } + + // Devices like magnetorquers generate a strong magnetic field which overrides the default + // model for the measure magnetic field. + pub async fn apply_external_magnetic_field(&mut self, field: MgmTuple) { + self.external_mag_field = Some(field); + } +} + +impl Model for MagnetometerModel {} + +#[derive(Debug, Clone, PartialEq, Serialize)] +pub struct PcduTuple {} + +pub enum PcduSwitches { + Mgm, + Mgt, +} + +pub struct PcduModel { + pub mgm_switch: Output, + pub mgt_switch: Output, +} + +impl PcduModel { + pub async fn switch_device(&mut self, switch: PcduSwitches, switch_state: SwitchState) { + match switch { + PcduSwitches::Mgm => { + self.mgm_switch.send(switch_state).await; + } + PcduSwitches::Mgt => { + self.mgt_switch.send(switch_state).await; + } + } + } +} + +impl Model for PcduModel {} + +// TODO: How to model this? And how to translate the dipole to the generated magnetic field? +pub struct Dipole {} + +pub struct MagnetorquerModel { + switch_state: SwitchState, + torquing: bool, + torque_duration: Duration, + torque_dipole: Option, + gen_magnetic_field: Output, +} + +impl MagnetorquerModel { + pub async fn apply_torque(&mut self, dipole: Dipole, torque_duration: Duration) { + self.torque_dipole = Some(dipole); + self.torque_duration = torque_duration; + self.torquing = true; + } + + pub async fn switch_device(&mut self, switch_state: SwitchState) { + self.switch_state = switch_state; + } + + pub async fn generate_output(&mut self, _: ()) { + if self.switch_state != SwitchState::On || !self.torquing { + return; + } + // TODO: Calculate generated magnetic field based on dipole.. some really simple model + // should suffice here for now. + // self.gen_magnetic_field.send().await; + } +} + +impl Model for MagnetorquerModel {} + // A UDP server which exposes all values generated by the simulator. pub struct UdpServer { socket: UdpSocket, @@ -96,52 +212,16 @@ pub fn current_millis(time: MonotonicTime) -> u64 { (time.as_secs() as u64 * 1000) + (time.subsec_nanos() as u64 / 1_000_000) } -impl SimMgm { - fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmTuple { - let base_sin_val = 2.0 * PI * FREQUENCY_MGM * (time_ms as f64 / 1000.0); - MgmTuple { - 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(), - } - } - - // Simple unit input to request MGM tuple for current time. - pub async fn input(&mut self, _: (), scheduler: &Scheduler) { - let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); - self.output.send(value).await; - } -} - -impl Model for SimMgm { - fn init( - self, - scheduler: &Scheduler, - ) -> std::pin::Pin< - Box< - dyn std::future::Future> - + Send - + '_, - >, - > { - //scheduler.schedule_periodic_event(Duration::from_secs(1), Self::send, value).unwrap(); - Box::pin(async move { - let _ = scheduler; // suppress the unused argument warning - self.into() - }) - } -} - fn main() { // Instantiate models and their mailboxes. - let mut mgm_sim = SimMgm::default(); + let mut mgm_sim = MagnetometerModel::default(); let mgm_mailbox = Mailbox::new(); let mgm_input_addr = mgm_mailbox.address(); // Keep handles to the main input and output. - let output_slot = mgm_sim.output.connect_slot().0; - let mut output_slot_2 = mgm_sim.output.connect_slot().0; + let output_slot = mgm_sim.sensor_values.connect_slot().0; + let mut output_slot_2 = mgm_sim.sensor_values.connect_slot().0; // Instantiate the simulator let t0 = MonotonicTime::EPOCH; // arbitrary start time @@ -149,12 +229,12 @@ fn main() { // This thread schedules the simulator. thread::spawn(move || { - simu.send_event(SimMgm::input, (), &mgm_input_addr); + simu.send_event(MagnetometerModel::generate_output, (), &mgm_input_addr); let mut tuple = output_slot_2.take().expect("expected output"); println!("output at {:?}: {tuple:?}", simu.time()); for _ in 0..100 { simu.step_by(Duration::from_millis(100)); - simu.send_event(SimMgm::input, (), &mgm_input_addr); + simu.send_event(MagnetometerModel::generate_output, (), &mgm_input_addr); tuple = output_slot_2.take().expect("expected output"); println!("output at {:?}: {tuple:?}", simu.time()); } -- 2.43.0 From f813bcf948209e3bba277db6407394a0097ce79d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 5 Mar 2024 10:01:44 +0100 Subject: [PATCH 03/10] simple dipole model --- satrs-minisim/src/main.rs | 27 ++++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index 64bca79..de9036e 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -28,6 +28,12 @@ const PHASE_X: f32 = 0.0; const PHASE_Y: f32 = 0.1; const PHASE_Z: f32 = 0.2; +const MGT_GEN_MAGNETIC_FIELD: MgmTuple = MgmTuple { + x: 0.03, + y: -0.03, + z: 0.03, +}; + pub struct MagnetometerModel { pub switch_state: SwitchState, pub external_mag_field: Option, @@ -111,8 +117,13 @@ impl PcduModel { impl Model for PcduModel {} -// TODO: How to model this? And how to translate the dipole to the generated magnetic field? -pub struct Dipole {} +// Simple model using i16 values. +#[derive(Debug, Copy, Clone, PartialEq, Serialize)] +pub struct Dipole { + pub x: i16, + pub y: i16, + pub z: i16, +} pub struct MagnetorquerModel { switch_state: SwitchState, @@ -133,13 +144,19 @@ impl MagnetorquerModel { self.switch_state = switch_state; } + fn calc_magnetic_field(&self, _: Dipole) -> MgmTuple { + // 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 + } + pub async fn generate_output(&mut self, _: ()) { if self.switch_state != SwitchState::On || !self.torquing { return; } - // TODO: Calculate generated magnetic field based on dipole.. some really simple model - // should suffice here for now. - // self.gen_magnetic_field.send().await; + self.gen_magnetic_field + .send(self.calc_magnetic_field(self.torque_dipole.expect("expected valid dipole"))) + .await; } } -- 2.43.0 From 9569cb76c397c467dc0d943bb86ba9ad71ab0437 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 5 Mar 2024 15:48:57 +0100 Subject: [PATCH 04/10] continue minisim --- satrs-minisim/src/main.rs | 138 +++++++++++++++++++++++++++----------- satrs/src/power.rs | 27 ++++++++ 2 files changed, 125 insertions(+), 40 deletions(-) diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index de9036e..2469ba5 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -2,9 +2,10 @@ use asynchronix::model::{Model, Output}; use asynchronix::simulation::{EventSlot, Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, Scheduler}; use log::warn; -use satrs::power::SwitchState; +use satrs::power::{SwitchState, SwitchStateBinary}; use serde::{Deserialize, Serialize}; use std::f64::consts::PI; +use std::future::Future; use std::net::UdpSocket; use std::time::Duration; use std::{io, thread}; @@ -36,21 +37,66 @@ const MGT_GEN_MAGNETIC_FIELD: MgmTuple = MgmTuple { pub struct MagnetometerModel { pub switch_state: SwitchState, + pub periodicity: Duration, pub external_mag_field: Option, pub sensor_values: Output, } -impl Default for MagnetometerModel { - fn default() -> Self { +impl MagnetometerModel { + fn new(periodicity: Duration) -> Self { Self { switch_state: SwitchState::Off, + periodicity, external_mag_field: None, sensor_values: Default::default(), } } -} -impl MagnetometerModel { + pub async fn start(&mut self, _: (), scheduler: &Scheduler) { + self.generate_output_self_scheduling((), scheduler).await; + } + + pub async fn switch_device(&mut self, switch_state: SwitchState, scheduler: &Scheduler) { + self.switch_state = switch_state; + self.generate_output((), scheduler).await; + } + + // 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: MgmTuple, + scheduler: &Scheduler, + ) { + self.external_mag_field = Some(field); + self.generate_output((), scheduler).await; + } + + // Simple unit input to request MGM tuple for current time. + // + // Need the partially desugared function signature, see [asynchronix::time::Scheduler] docs. + #[allow(clippy::manual_async_fn)] + pub fn generate_output_self_scheduling<'a>( + &'a mut self, + _: (), + scheduler: &'a Scheduler, + ) -> impl Future + Send + 'a { + async move { + if scheduler + .schedule_event(self.periodicity, Self::generate_output_self_scheduling, ()) + .is_err() + { + warn!("output generation can only be set for a future time."); + } + self.generate_output((), scheduler).await; + } + } + + pub async fn generate_output(&mut self, _: (), scheduler: &Scheduler) { + let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); + self.sensor_values.send(value).await; + } + fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmTuple { if let SwitchState::On = self.switch_state { if let Some(ext_field) = self.external_mag_field { @@ -69,22 +115,6 @@ impl MagnetometerModel { z: 0.0, } } - - pub async fn switch_device(&mut self, switch_state: SwitchState) { - self.switch_state = switch_state; - } - - // Simple unit input to request MGM tuple for current time. - pub async fn generate_output(&mut self, _: (), scheduler: &Scheduler) { - let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); - self.sensor_values.send(value).await; - } - - // Devices like magnetorquers generate a strong magnetic field which overrides the default - // model for the measure magnetic field. - pub async fn apply_external_magnetic_field(&mut self, field: MgmTuple) { - self.external_mag_field = Some(field); - } } impl Model for MagnetometerModel {} @@ -93,11 +123,12 @@ impl Model for MagnetometerModel {} pub struct PcduTuple {} pub enum PcduSwitches { - Mgm, - Mgt, + Mgm = 0, + Mgt = 1, } pub struct PcduModel { + pub switcher_list: Output>, pub mgm_switch: Output, pub mgt_switch: Output, } @@ -128,20 +159,38 @@ pub struct Dipole { pub struct MagnetorquerModel { switch_state: SwitchState, torquing: bool, - torque_duration: Duration, + //torque_duration: Duration, torque_dipole: Option, gen_magnetic_field: Output, } impl MagnetorquerModel { - pub async fn apply_torque(&mut self, dipole: Dipole, torque_duration: Duration) { + pub async fn apply_torque( + &mut self, + dipole: Dipole, + torque_duration: Duration, + scheduler: &Scheduler, + ) { self.torque_dipole = Some(dipole); - self.torque_duration = torque_duration; self.torquing = true; + if scheduler + .schedule_event(torque_duration, Self::clear_torque, ()) + .is_err() + { + warn!("torque clearing can only be set for a future time."); + } + self.generate_magnetic_field(()).await; + } + + pub async fn clear_torque(&mut self, _: ()) { + self.torque_dipole = None; + self.torquing = false; + self.generate_magnetic_field(()).await; } pub async fn switch_device(&mut self, switch_state: SwitchState) { self.switch_state = switch_state; + self.generate_magnetic_field(()).await; } fn calc_magnetic_field(&self, _: Dipole) -> MgmTuple { @@ -150,7 +199,9 @@ impl MagnetorquerModel { MGT_GEN_MAGNETIC_FIELD } - pub async fn generate_output(&mut self, _: ()) { + /// A torquing magnetorquer generates a magnetic field. This function can be used to apply + /// the magnetic field. + async fn generate_magnetic_field(&mut self, _: ()) { if self.switch_state != SwitchState::On || !self.torquing { return; } @@ -189,9 +240,10 @@ impl UdpServer { pub fn run(&mut self) { loop { - let mut buffer = [0u8; 1024]; // Buffer to store incoming data. - - // Block until data is received. `recv_from` returns the number of bytes read and the sender's address. + // Buffer to store incoming data. + let mut buffer = [0u8; 4096]; + // Block until data is received. `recv_from` returns the number of bytes read and the + // sender's address. let (bytes_read, src) = self .socket .recv_from(&mut buffer) @@ -231,35 +283,41 @@ pub fn current_millis(time: MonotonicTime) -> u64 { fn main() { // Instantiate models and their mailboxes. - let mut mgm_sim = MagnetometerModel::default(); + let mut mgm_sim = MagnetometerModel::new(Duration::from_millis(50)); let mgm_mailbox = Mailbox::new(); let mgm_input_addr = mgm_mailbox.address(); // Keep handles to the main input and output. let output_slot = mgm_sim.sensor_values.connect_slot().0; - let mut output_slot_2 = mgm_sim.sensor_values.connect_slot().0; + // let output_slot_2 = mgm_sim.sensor_values.connect_slot().0; // Instantiate the simulator let t0 = MonotonicTime::EPOCH; // arbitrary start time let mut simu = SimInit::new().add_model(mgm_sim, mgm_mailbox).init(t0); // This thread schedules the simulator. - thread::spawn(move || { - simu.send_event(MagnetometerModel::generate_output, (), &mgm_input_addr); - let mut tuple = output_slot_2.take().expect("expected output"); - println!("output at {:?}: {tuple:?}", simu.time()); + let sim_thread = thread::spawn(move || { + // The magnetometer will schedule itself at fixed intervals. + simu.send_event(MagnetometerModel::start, (), &mgm_input_addr); + /* for _ in 0..100 { - simu.step_by(Duration::from_millis(100)); - simu.send_event(MagnetometerModel::generate_output, (), &mgm_input_addr); - tuple = output_slot_2.take().expect("expected output"); + simu.step(); + let tuple = output_slot_2.take().expect("expected output"); println!("output at {:?}: {tuple:?}", simu.time()); } + */ + loop { + simu.step(); + } }); // This thread manages the simulator UDP server. - thread::spawn(move || { + let udp_thread = thread::spawn(move || { let mut server = UdpServer::new(output_slot).unwrap(); server.run(); }); + + sim_thread.join().expect("joining simulation thread failed"); + udp_thread.join().expect("joining UDP thread failed"); } diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 1675c01..7786651 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -24,6 +24,33 @@ pub enum SwitchState { Faulty = 3, } +#[derive(Debug, Eq, PartialEq, Copy, Clone)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub enum SwitchStateBinary { + Off = 0, + On = 1, +} + +impl TryFrom for SwitchStateBinary { + type Error = (); + fn try_from(value: SwitchState) -> Result { + match value { + SwitchState::Off => Ok(SwitchStateBinary::Off), + SwitchState::On => Ok(SwitchStateBinary::On), + _ => Err(()), + } + } +} + +impl From for SwitchState { + fn from(value: SwitchStateBinary) -> Self { + match value { + SwitchStateBinary::Off => SwitchState::Off, + SwitchStateBinary::On => SwitchState::On, + } + } +} + pub type SwitchId = u16; /// Generic trait for a device capable of turning on and off switches. -- 2.43.0 From b622e8373141adfed09e5c858672f4fec99c71ac Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 5 Mar 2024 18:10:46 +0100 Subject: [PATCH 05/10] need to think about the exchange interface again.. --- satrs-minisim/src/main.rs | 112 +++++++++++++++++++++++++++----------- 1 file changed, 81 insertions(+), 31 deletions(-) diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index 2469ba5..88d6b69 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -1,12 +1,13 @@ use asynchronix::model::{Model, Output}; use asynchronix::simulation::{EventSlot, Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, Scheduler}; -use log::warn; +use log::{info, warn}; use satrs::power::{SwitchState, SwitchStateBinary}; use serde::{Deserialize, Serialize}; use std::f64::consts::PI; use std::future::Future; -use std::net::UdpSocket; +use std::net::{SocketAddr, UdpSocket}; +use std::sync::{mpsc, Arc, Mutex}; use std::time::Duration; use std::{io, thread}; @@ -127,6 +128,11 @@ pub enum PcduSwitches { Mgt = 1, } +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +pub enum PcduRequest { + RequestSwitchInfo, +} + pub struct PcduModel { pub switcher_list: Output>, pub mgm_switch: Output, @@ -213,29 +219,50 @@ impl MagnetorquerModel { impl Model for MagnetorquerModel {} -// A UDP server which exposes all values generated by the simulator. -pub struct UdpServer { +// A UDP server which handles all TC received by a client application. +pub struct UdpTcServer { socket: UdpSocket, - mgm_out: EventSlot, + last_sender: Arc>>, } -#[derive(Serialize, Deserialize)] -pub struct ValueRequest { - device: String, +// A helper object which sends back all replies to the UDP client. +// +// This helper is scheduled separately to minimize the delay between the requests and replies. +pub struct UdpTmSender { + reply_receiver: mpsc::Receiver, + last_sender: Arc>>, +} + +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +pub enum SimDevice { + Mgm, + Mgt, + Pcdu, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct SimRequest { + device: SimDevice, + request: String, } #[derive(Serialize, Deserialize)] pub struct ValueReply { - device: String, + device: SimDevice, reply: String, } -const MGM_DEV_STR: &str = "mgm"; +struct ReplyHandler { + reply_sender: mpsc::Sender, +} -impl UdpServer { - pub fn new(mgm_out: EventSlot) -> io::Result { +impl UdpTcServer { + pub fn new(reply_receiver: mpsc::Receiver) -> io::Result { let socket = UdpSocket::bind("0.0.0.0:7303")?; - Ok(Self { socket, mgm_out }) + Ok(Self { + socket, + // reply_receiver, + }) } pub fn run(&mut self) { @@ -253,28 +280,51 @@ impl UdpServer { let req_string = std::str::from_utf8(&buffer[..bytes_read]) .expect("Could not write buffer as string"); println!("Received from {}: {}", src, req_string); - let value_result: serde_json::Result = serde_json::from_str(req_string); - match value_result { - Ok(value) => { - if value.device == MGM_DEV_STR { - let tuple = self.mgm_out.take().expect("expected output"); - let reply = ValueReply { - device: MGM_DEV_STR.to_string(), - reply: serde_json::to_string(&tuple).unwrap(), - }; - let reply_string = - serde_json::to_string(&reply).expect("generating reply string failed"); - self.socket - .send_to(reply_string.as_bytes(), src) - .expect("could not send data"); - } + let sim_req: serde_json::Result = serde_json::from_str(req_string); + if sim_req.is_err() { + warn!( + "received UDP request with invalid format: {}", + sim_req.unwrap_err() + ); + continue; + } + let sim_req = sim_req.unwrap(); + match sim_req.device { + SimDevice::Mgm => { + self.handle_mgm_request(&src, &sim_req); } - Err(e) => { - warn!("received UDP request with invalid format: {e}"); + SimDevice::Mgt => {} + SimDevice::Pcdu => { + self.handle_pcdu_request(&src, &sim_req); } } } } + + fn handle_mgm_request(&mut self, sender: &SocketAddr, sim_req: &SimRequest) { + /* + let tuple = self.mgm_out.take().expect("expected output"); + let reply = ValueReply { + device: sim_req.device, + reply: serde_json::to_string(&tuple).unwrap(), + }; + let reply_string = serde_json::to_string(&reply).expect("generating reply string failed"); + self.socket + .send_to(reply_string.as_bytes(), sender) + .expect("could not send data"); + */ + } + + fn handle_pcdu_request(&mut self, sender: &SocketAddr, sim_req: &SimRequest) { + let pcdu_request: serde_json::Result = serde_json::from_str(&sim_req.request); + if pcdu_request.is_err() { + warn!( + "received invalid PCDU request: {}", + pcdu_request.unwrap_err() + ); + return; + } + } } pub fn current_millis(time: MonotonicTime) -> u64 { @@ -314,7 +364,7 @@ fn main() { // This thread manages the simulator UDP server. let udp_thread = thread::spawn(move || { - let mut server = UdpServer::new(output_slot).unwrap(); + let mut server = UdpTcServer::new(output_slot).unwrap(); server.run(); }); -- 2.43.0 From 0a41de5e70828bf6ae1d6c6b7092e0befc63f54d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 6 Mar 2024 14:16:40 +0100 Subject: [PATCH 06/10] continuing the simulation --- satrs-minisim/Cargo.toml | 7 +++- satrs-minisim/src/main.rs | 68 +++++++++++++++++++++++---------------- 2 files changed, 47 insertions(+), 28 deletions(-) diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index c272fee..53910e1 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -6,10 +6,15 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -asynchronix = "0.2" serde = { version = "1", features = ["derive"] } serde_json = "1" log = "0.4" +[dependencies.asynchronix] +version = "0.2" +path = "../../asynchronix/asynchronix" +# git = "https://github.com/us-irs/asynchronix.git" +# branch = "clock-not-sendable" + [dependencies.satrs] path = "../satrs" diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index 88d6b69..ad39ab1 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -1,6 +1,6 @@ use asynchronix::model::{Model, Output}; -use asynchronix::simulation::{EventSlot, Mailbox, SimInit}; -use asynchronix::time::{MonotonicTime, Scheduler}; +use asynchronix::simulation::{EventSlot, Mailbox, SimInit, Simulation}; +use asynchronix::time::{MonotonicTime, Scheduler, SystemClock}; use log::{info, warn}; use satrs::power::{SwitchState, SwitchStateBinary}; use serde::{Deserialize, Serialize}; @@ -8,7 +8,7 @@ use std::f64::consts::PI; use std::future::Future; use std::net::{SocketAddr, UdpSocket}; use std::sync::{mpsc, Arc, Mutex}; -use std::time::Duration; +use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use std::{io, thread}; // Normally, small magnetometers generate their output as a signed 16 bit raw format or something @@ -219,17 +219,11 @@ impl MagnetorquerModel { impl Model for MagnetorquerModel {} -// A UDP server which handles all TC received by a client application. -pub struct UdpTcServer { - socket: UdpSocket, - last_sender: Arc>>, -} - // A helper object which sends back all replies to the UDP client. // // This helper is scheduled separately to minimize the delay between the requests and replies. pub struct UdpTmSender { - reply_receiver: mpsc::Receiver, + reply_receiver: mpsc::Receiver, last_sender: Arc>>, } @@ -247,21 +241,30 @@ pub struct SimRequest { } #[derive(Serialize, Deserialize)] -pub struct ValueReply { +pub struct SimReply { device: SimDevice, reply: String, } -struct ReplyHandler { - reply_sender: mpsc::Sender, +pub type SharedSocketAddr = Arc>>; + +// A UDP server which handles all TC received by a client application. +pub struct UdpTcServer { + socket: UdpSocket, + request_sender: mpsc::Sender, + last_sender: SharedSocketAddr, } impl UdpTcServer { - pub fn new(reply_receiver: mpsc::Receiver) -> io::Result { + pub fn new( + request_sender: mpsc::Sender, + last_sender: SharedSocketAddr, + ) -> io::Result { let socket = UdpSocket::bind("0.0.0.0:7303")?; Ok(Self { socket, - // reply_receiver, + request_sender, + last_sender, }) } @@ -288,6 +291,9 @@ impl UdpTcServer { ); continue; } + self.request_sender.send(sim_req.unwrap()).unwrap(); + self.last_sender.lock().unwrap().replace(src); + /* let sim_req = sim_req.unwrap(); match sim_req.device { SimDevice::Mgm => { @@ -298,6 +304,7 @@ impl UdpTcServer { self.handle_pcdu_request(&src, &sim_req); } } + */ } } @@ -327,11 +334,23 @@ impl UdpTcServer { } } +// The simulation controller processes requests and drives the simulation. +// TODO: How do we process requests and drive the simulation at the same time? +pub struct SimController { + pub request_receiver: mpsc::Receiver, + pub simulation: Simulation, +} + +impl SimController {} + pub fn current_millis(time: MonotonicTime) -> u64 { (time.as_secs() as u64 * 1000) + (time.subsec_nanos() as u64 / 1_000_000) } fn main() { + let shared_socket_addr = SharedSocketAddr::default(); + let (req_sender, req_receiver) = mpsc::channel(); + // Instantiate models and their mailboxes. let mut mgm_sim = MagnetometerModel::new(Duration::from_millis(50)); @@ -339,24 +358,19 @@ fn main() { let mgm_input_addr = mgm_mailbox.address(); // Keep handles to the main input and output. - let output_slot = mgm_sim.sensor_values.connect_slot().0; + // let output_slot = mgm_sim.sensor_values.connect_slot().0; // let output_slot_2 = mgm_sim.sensor_values.connect_slot().0; - + let t0 = MonotonicTime::EPOCH; + let clock = SystemClock::from_system_time(t0, SystemTime::now()); // Instantiate the simulator - let t0 = MonotonicTime::EPOCH; // arbitrary start time - let mut simu = SimInit::new().add_model(mgm_sim, mgm_mailbox).init(t0); + let mut simu = SimInit::new() + .add_model(mgm_sim, mgm_mailbox) + .init_with_clock(t0, clock); // This thread schedules the simulator. let sim_thread = thread::spawn(move || { // The magnetometer will schedule itself at fixed intervals. simu.send_event(MagnetometerModel::start, (), &mgm_input_addr); - /* - for _ in 0..100 { - simu.step(); - let tuple = output_slot_2.take().expect("expected output"); - println!("output at {:?}: {tuple:?}", simu.time()); - } - */ loop { simu.step(); } @@ -364,7 +378,7 @@ fn main() { // This thread manages the simulator UDP server. let udp_thread = thread::spawn(move || { - let mut server = UdpTcServer::new(output_slot).unwrap(); + let mut server = UdpTcServer::new(req_sender, shared_socket_addr).unwrap(); server.run(); }); -- 2.43.0 From 3ad06f63c7d7bab06a03f9699588fe272447151d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Mar 2024 12:24:54 +0100 Subject: [PATCH 07/10] modularized the mini simulator --- satrs-minisim/src/acs.rs | 148 ++++++++++++ satrs-minisim/src/controller.rs | 70 ++++++ satrs-minisim/src/eps.rs | 37 +++ satrs-minisim/src/lib.rs | 53 +++++ satrs-minisim/src/main.rs | 410 ++++---------------------------- satrs-minisim/src/time.rs | 5 + satrs-minisim/src/udp.rs | 152 ++++++++++++ 7 files changed, 508 insertions(+), 367 deletions(-) create mode 100644 satrs-minisim/src/acs.rs create mode 100644 satrs-minisim/src/controller.rs create mode 100644 satrs-minisim/src/eps.rs create mode 100644 satrs-minisim/src/lib.rs create mode 100644 satrs-minisim/src/time.rs create mode 100644 satrs-minisim/src/udp.rs diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs new file mode 100644 index 0000000..604597d --- /dev/null +++ b/satrs-minisim/src/acs.rs @@ -0,0 +1,148 @@ +use std::{f32::consts::PI, sync::mpsc, time::Duration}; + +use asynchronix::{ + model::{Model, Output}, + time::Scheduler, +}; +use satrs::power::SwitchState; +use satrs_minisim::{ + acs::{MgmSensorValues, MgtDipole, MGT_GEN_MAGNETIC_FIELD}, + SimDevice, SimReply, +}; + +use crate::time::current_millis; + +// Earth magnetic field varies between -30 uT and 30 uT +const AMPLITUDE_MGM: f32 = 0.03; +// Lets start with a simple frequency here. +const FREQUENCY_MGM: f32 = 1.0; +const PHASE_X: f32 = 0.0; +// Different phases to have different values on the other axes. +const PHASE_Y: f32 = 0.1; +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. +pub struct MagnetometerModel { + pub switch_state: SwitchState, + pub periodicity: Duration, + pub external_mag_field: Option, + pub reply_sender: mpsc::Sender, +} + +impl MagnetometerModel { + pub fn new(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { + Self { + switch_state: SwitchState::Off, + periodicity, + external_mag_field: None, + reply_sender, + } + } + + pub async fn switch_device(&mut self, switch_state: SwitchState) { + self.switch_state = switch_state; + } + + pub async fn send_sensor_values(&mut self, _: (), scheduler: &Scheduler) { + let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); + let reply = SimReply { + device: SimDevice::Mgm, + reply: serde_json::to_string(&value).unwrap(), + }; + self.reply_sender + .send(reply) + .expect("sending MGM sensor values failed"); + } + + // 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) { + self.external_mag_field = Some(field); + } + + fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmSensorValues { + if let SwitchState::On = self.switch_state { + if let Some(ext_field) = self.external_mag_field { + return ext_field; + } + let base_sin_val = 2.0 * PI as f32 * 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(), + }; + } + MgmSensorValues { + x: 0.0, + y: 0.0, + z: 0.0, + } + } +} + +impl Model for MagnetometerModel {} + +pub struct MagnetorquerModel { + switch_state: SwitchState, + torquing: bool, + torque_dipole: Option, + gen_magnetic_field: Output, +} + +impl MagnetorquerModel { + pub async fn apply_torque( + &mut self, + dipole: MgtDipole, + torque_duration: Duration, + scheduler: &Scheduler, + ) { + self.torque_dipole = Some(dipole); + self.torquing = true; + if scheduler + .schedule_event(torque_duration, Self::clear_torque, ()) + .is_err() + { + log::warn!("torque clearing can only be set for a future time."); + } + self.generate_magnetic_field(()).await; + } + + pub async fn clear_torque(&mut self, _: ()) { + self.torque_dipole = None; + self.torquing = false; + self.generate_magnetic_field(()).await; + } + + pub async fn switch_device(&mut self, switch_state: SwitchState) { + self.switch_state = switch_state; + self.generate_magnetic_field(()).await; + } + + fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValues { + // 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 + } + + /// A torquing magnetorquer generates a magnetic field. This function can be used to apply + /// the magnetic field. + async fn generate_magnetic_field(&mut self, _: ()) { + if self.switch_state != SwitchState::On || !self.torquing { + return; + } + self.gen_magnetic_field + .send(self.calc_magnetic_field(self.torque_dipole.expect("expected valid dipole"))) + .await; + } +} + +impl Model for MagnetorquerModel {} diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs new file mode 100644 index 0000000..0d604f9 --- /dev/null +++ b/satrs-minisim/src/controller.rs @@ -0,0 +1,70 @@ +use std::{sync::mpsc, time::Duration}; + +use asynchronix::{ + simulation::{Address, Simulation}, + time::{Clock, MonotonicTime, SystemClock}, +}; +use satrs_minisim::{acs::MgmRequest, SimRequest}; + +use crate::{ + acs::MagnetometerModel, + eps::{PcduModel, PcduRequest}, +}; + +// The simulation controller processes requests and drives the simulation. +pub struct SimController { + pub sys_clock: SystemClock, + pub request_receiver: mpsc::Receiver, + pub simulation: Simulation, + pub mgm_addr: Address, + pub pcdu_addr: Address, +} + +impl SimController { + pub fn run(&mut self, t0: MonotonicTime) { + let mut t = t0 + Duration::from_millis(10); + loop { + self.simulation + .step_until(t) + .expect("simulation step failed"); + t += Duration::from_millis(10); + // TODO: Received and handle requests. + + // TODO: Incorporate network latency. + self.sys_clock.synchronize(t); + } + } + + fn handle_mgm_request(&mut self, request: &str) { + let mgm_request: serde_json::Result = serde_json::from_str(request); + if mgm_request.is_err() { + log::warn!("received invalid MGM request: {}", mgm_request.unwrap_err()); + return; + } + let mgm_request = mgm_request.unwrap(); + match mgm_request { + MgmRequest::RequestSensorData => { + self.simulation.send_event( + MagnetometerModel::send_sensor_values, + (), + &self.mgm_addr, + ); + } + } + } + + fn handle_pcdu_request(&mut self, request: &str) { + let pcdu_request: serde_json::Result = serde_json::from_str(&request); + if pcdu_request.is_err() { + log::warn!( + "received invalid PCDU request: {}", + pcdu_request.unwrap_err() + ); + return; + } + let pcdu_request = pcdu_request.unwrap(); + match pcdu_request { + PcduRequest::RequestSwitchInfo => todo!(), + } + } +} diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs new file mode 100644 index 0000000..aa16afa --- /dev/null +++ b/satrs-minisim/src/eps.rs @@ -0,0 +1,37 @@ +use asynchronix::model::{Model, Output}; +use satrs::power::{SwitchState, SwitchStateBinary}; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Clone, PartialEq, Serialize)] +pub struct PcduTuple {} + +pub enum PcduSwitches { + Mgm = 0, + Mgt = 1, +} + +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +pub enum PcduRequest { + RequestSwitchInfo, +} + +pub struct PcduModel { + pub switcher_list: Output>, + pub mgm_switch: Output, + pub mgt_switch: Output, +} + +impl PcduModel { + pub async fn switch_device(&mut self, switch: PcduSwitches, switch_state: SwitchState) { + match switch { + PcduSwitches::Mgm => { + self.mgm_switch.send(switch_state).await; + } + PcduSwitches::Mgt => { + self.mgt_switch.send(switch_state).await; + } + } + } +} + +impl Model for PcduModel {} diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs new file mode 100644 index 0000000..b9ed24c --- /dev/null +++ b/satrs-minisim/src/lib.rs @@ -0,0 +1,53 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +pub enum SimDevice { + Mgm, + Mgt, + Pcdu, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct SimRequest { + pub device: SimDevice, + pub request: String, +} + +#[derive(Serialize, Deserialize)] +pub struct SimReply { + pub device: SimDevice, + pub reply: String, +} + +pub mod acs { + use super::*; + + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub enum MgmRequest { + RequestSensorData, + } + + // 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. + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgmSensorValues { + pub x: f32, + pub y: f32, + pub z: f32, + } + + pub const MGT_GEN_MAGNETIC_FIELD: MgmSensorValues = MgmSensorValues { + x: 0.03, + y: -0.03, + z: 0.03, + }; + + // Simple model using i16 values. + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgtDipole { + pub x: i16, + pub y: i16, + pub z: i16, + } +} diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index ad39ab1..bfb0c0b 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -1,387 +1,63 @@ -use asynchronix::model::{Model, Output}; -use asynchronix::simulation::{EventSlot, Mailbox, SimInit, Simulation}; -use asynchronix::time::{MonotonicTime, Scheduler, SystemClock}; -use log::{info, warn}; -use satrs::power::{SwitchState, SwitchStateBinary}; -use serde::{Deserialize, Serialize}; -use std::f64::consts::PI; -use std::future::Future; -use std::net::{SocketAddr, UdpSocket}; -use std::sync::{mpsc, Arc, Mutex}; -use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; -use std::{io, thread}; +use acs::MagnetometerModel; +use asynchronix::model::Model; +use asynchronix::simulation::{Mailbox, SimInit}; +use asynchronix::time::{MonotonicTime, SystemClock}; +use controller::SimController; +use std::sync::mpsc; +use std::thread; +use std::time::{Duration, SystemTime}; +use udp::{SharedSocketAddr, UdpTcServer, UdpTmClient}; -// 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. -#[derive(Debug, Copy, Clone, PartialEq, Serialize)] -pub struct MgmTuple { - x: f32, - y: f32, - z: f32, -} - -// Earth magnetic field varies between -30 uT and 30 uT -const AMPLITUDE_MGM: f32 = 0.03; -// Lets start with a simple frequency here. -const FREQUENCY_MGM: f32 = 1.0; -const PHASE_X: f32 = 0.0; -// Different phases to have different values on the other axes. -const PHASE_Y: f32 = 0.1; -const PHASE_Z: f32 = 0.2; - -const MGT_GEN_MAGNETIC_FIELD: MgmTuple = MgmTuple { - x: 0.03, - y: -0.03, - z: 0.03, -}; - -pub struct MagnetometerModel { - pub switch_state: SwitchState, - pub periodicity: Duration, - pub external_mag_field: Option, - pub sensor_values: Output, -} - -impl MagnetometerModel { - fn new(periodicity: Duration) -> Self { - Self { - switch_state: SwitchState::Off, - periodicity, - external_mag_field: None, - sensor_values: Default::default(), - } - } - - pub async fn start(&mut self, _: (), scheduler: &Scheduler) { - self.generate_output_self_scheduling((), scheduler).await; - } - - pub async fn switch_device(&mut self, switch_state: SwitchState, scheduler: &Scheduler) { - self.switch_state = switch_state; - self.generate_output((), scheduler).await; - } - - // 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: MgmTuple, - scheduler: &Scheduler, - ) { - self.external_mag_field = Some(field); - self.generate_output((), scheduler).await; - } - - // Simple unit input to request MGM tuple for current time. - // - // Need the partially desugared function signature, see [asynchronix::time::Scheduler] docs. - #[allow(clippy::manual_async_fn)] - pub fn generate_output_self_scheduling<'a>( - &'a mut self, - _: (), - scheduler: &'a Scheduler, - ) -> impl Future + Send + 'a { - async move { - if scheduler - .schedule_event(self.periodicity, Self::generate_output_self_scheduling, ()) - .is_err() - { - warn!("output generation can only be set for a future time."); - } - self.generate_output((), scheduler).await; - } - } - - pub async fn generate_output(&mut self, _: (), scheduler: &Scheduler) { - let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); - self.sensor_values.send(value).await; - } - - fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmTuple { - if let SwitchState::On = self.switch_state { - if let Some(ext_field) = self.external_mag_field { - return ext_field; - } - let base_sin_val = 2.0 * PI as f32 * FREQUENCY_MGM * (time_ms as f32 / 1000.0); - return MgmTuple { - 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(), - }; - } - MgmTuple { - x: 0.0, - y: 0.0, - z: 0.0, - } - } -} - -impl Model for MagnetometerModel {} - -#[derive(Debug, Clone, PartialEq, Serialize)] -pub struct PcduTuple {} - -pub enum PcduSwitches { - Mgm = 0, - Mgt = 1, -} - -#[derive(Debug, Copy, Clone, Serialize, Deserialize)] -pub enum PcduRequest { - RequestSwitchInfo, -} - -pub struct PcduModel { - pub switcher_list: Output>, - pub mgm_switch: Output, - pub mgt_switch: Output, -} - -impl PcduModel { - pub async fn switch_device(&mut self, switch: PcduSwitches, switch_state: SwitchState) { - match switch { - PcduSwitches::Mgm => { - self.mgm_switch.send(switch_state).await; - } - PcduSwitches::Mgt => { - self.mgt_switch.send(switch_state).await; - } - } - } -} - -impl Model for PcduModel {} - -// Simple model using i16 values. -#[derive(Debug, Copy, Clone, PartialEq, Serialize)] -pub struct Dipole { - pub x: i16, - pub y: i16, - pub z: i16, -} - -pub struct MagnetorquerModel { - switch_state: SwitchState, - torquing: bool, - //torque_duration: Duration, - torque_dipole: Option, - gen_magnetic_field: Output, -} - -impl MagnetorquerModel { - pub async fn apply_torque( - &mut self, - dipole: Dipole, - torque_duration: Duration, - scheduler: &Scheduler, - ) { - self.torque_dipole = Some(dipole); - self.torquing = true; - if scheduler - .schedule_event(torque_duration, Self::clear_torque, ()) - .is_err() - { - warn!("torque clearing can only be set for a future time."); - } - self.generate_magnetic_field(()).await; - } - - pub async fn clear_torque(&mut self, _: ()) { - self.torque_dipole = None; - self.torquing = false; - self.generate_magnetic_field(()).await; - } - - pub async fn switch_device(&mut self, switch_state: SwitchState) { - self.switch_state = switch_state; - self.generate_magnetic_field(()).await; - } - - fn calc_magnetic_field(&self, _: Dipole) -> MgmTuple { - // 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 - } - - /// A torquing magnetorquer generates a magnetic field. This function can be used to apply - /// the magnetic field. - async fn generate_magnetic_field(&mut self, _: ()) { - if self.switch_state != SwitchState::On || !self.torquing { - return; - } - self.gen_magnetic_field - .send(self.calc_magnetic_field(self.torque_dipole.expect("expected valid dipole"))) - .await; - } -} - -impl Model for MagnetorquerModel {} - -// A helper object which sends back all replies to the UDP client. -// -// This helper is scheduled separately to minimize the delay between the requests and replies. -pub struct UdpTmSender { - reply_receiver: mpsc::Receiver, - last_sender: Arc>>, -} - -#[derive(Debug, Copy, Clone, Serialize, Deserialize)] -pub enum SimDevice { - Mgm, - Mgt, - Pcdu, -} - -#[derive(Debug, Clone, Serialize, Deserialize)] -pub struct SimRequest { - device: SimDevice, - request: String, -} - -#[derive(Serialize, Deserialize)] -pub struct SimReply { - device: SimDevice, - reply: String, -} - -pub type SharedSocketAddr = Arc>>; - -// A UDP server which handles all TC received by a client application. -pub struct UdpTcServer { - socket: UdpSocket, - request_sender: mpsc::Sender, - last_sender: SharedSocketAddr, -} - -impl UdpTcServer { - pub fn new( - request_sender: mpsc::Sender, - last_sender: SharedSocketAddr, - ) -> io::Result { - let socket = UdpSocket::bind("0.0.0.0:7303")?; - Ok(Self { - socket, - request_sender, - last_sender, - }) - } - - pub fn run(&mut self) { - loop { - // Buffer to store incoming data. - let mut buffer = [0u8; 4096]; - // Block until data is received. `recv_from` returns the number of bytes read and the - // sender's address. - let (bytes_read, src) = self - .socket - .recv_from(&mut buffer) - .expect("could not read from socket"); - - // Convert the buffer into a string slice and print the message. - let req_string = std::str::from_utf8(&buffer[..bytes_read]) - .expect("Could not write buffer as string"); - println!("Received from {}: {}", src, req_string); - let sim_req: serde_json::Result = serde_json::from_str(req_string); - if sim_req.is_err() { - warn!( - "received UDP request with invalid format: {}", - sim_req.unwrap_err() - ); - continue; - } - self.request_sender.send(sim_req.unwrap()).unwrap(); - self.last_sender.lock().unwrap().replace(src); - /* - let sim_req = sim_req.unwrap(); - match sim_req.device { - SimDevice::Mgm => { - self.handle_mgm_request(&src, &sim_req); - } - SimDevice::Mgt => {} - SimDevice::Pcdu => { - self.handle_pcdu_request(&src, &sim_req); - } - } - */ - } - } - - fn handle_mgm_request(&mut self, sender: &SocketAddr, sim_req: &SimRequest) { - /* - let tuple = self.mgm_out.take().expect("expected output"); - let reply = ValueReply { - device: sim_req.device, - reply: serde_json::to_string(&tuple).unwrap(), - }; - let reply_string = serde_json::to_string(&reply).expect("generating reply string failed"); - self.socket - .send_to(reply_string.as_bytes(), sender) - .expect("could not send data"); - */ - } - - fn handle_pcdu_request(&mut self, sender: &SocketAddr, sim_req: &SimRequest) { - let pcdu_request: serde_json::Result = serde_json::from_str(&sim_req.request); - if pcdu_request.is_err() { - warn!( - "received invalid PCDU request: {}", - pcdu_request.unwrap_err() - ); - return; - } - } -} - -// The simulation controller processes requests and drives the simulation. -// TODO: How do we process requests and drive the simulation at the same time? -pub struct SimController { - pub request_receiver: mpsc::Receiver, - pub simulation: Simulation, -} - -impl SimController {} - -pub fn current_millis(time: MonotonicTime) -> u64 { - (time.as_secs() as u64 * 1000) + (time.subsec_nanos() as u64 / 1_000_000) -} +mod acs; +mod controller; +mod eps; +mod time; +mod udp; fn main() { let shared_socket_addr = SharedSocketAddr::default(); - let (req_sender, req_receiver) = mpsc::channel(); + let (request_sender, request_receiver) = mpsc::channel(); + let (reply_sender, reply_receiver) = mpsc::channel(); // Instantiate models and their mailboxes. - let mut mgm_sim = MagnetometerModel::new(Duration::from_millis(50)); + let mgm_sim = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); let mgm_mailbox = Mailbox::new(); - let mgm_input_addr = mgm_mailbox.address(); + let mgm_addr = mgm_mailbox.address(); + let pcdu_mailbox = Mailbox::new(); + let pcdu_addr = pcdu_mailbox.address(); - // Keep handles to the main input and output. - // let output_slot = mgm_sim.sensor_values.connect_slot().0; - // let output_slot_2 = mgm_sim.sensor_values.connect_slot().0; - let t0 = MonotonicTime::EPOCH; - let clock = SystemClock::from_system_time(t0, SystemTime::now()); // Instantiate the simulator - let mut simu = SimInit::new() - .add_model(mgm_sim, mgm_mailbox) - .init_with_clock(t0, clock); + let t0 = MonotonicTime::EPOCH; + let sys_clock = SystemClock::from_system_time(t0, SystemTime::now()); + let simulation = SimInit::new().add_model(mgm_sim, mgm_mailbox).init(t0); + + let mut sim_controller = SimController { + sys_clock, + request_receiver, + simulation, + mgm_addr, + pcdu_addr, + }; // This thread schedules the simulator. let sim_thread = thread::spawn(move || { - // The magnetometer will schedule itself at fixed intervals. - simu.send_event(MagnetometerModel::start, (), &mgm_input_addr); - loop { - simu.step(); - } + sim_controller.run(t0); }); - // This thread manages the simulator UDP server. - let udp_thread = thread::spawn(move || { - let mut server = UdpTcServer::new(req_sender, shared_socket_addr).unwrap(); + let mut server = UdpTcServer::new(request_sender, shared_socket_addr.clone()).unwrap(); + // This thread manages the simulator UDP TC server. + let udp_tc_thread = thread::spawn(move || { server.run(); }); + let mut client = UdpTmClient::new(reply_receiver, 200, shared_socket_addr); + // This thread manages the simulator UDP TM client. + let udp_tm_thread = thread::spawn(move || { + client.run(); + }); + sim_thread.join().expect("joining simulation thread failed"); - udp_thread.join().expect("joining UDP thread failed"); + udp_tc_thread.join().expect("joining UDP TC thread failed"); + udp_tm_thread.join().expect("joining UDP TM thread failed"); } diff --git a/satrs-minisim/src/time.rs b/satrs-minisim/src/time.rs new file mode 100644 index 0000000..63ae327 --- /dev/null +++ b/satrs-minisim/src/time.rs @@ -0,0 +1,5 @@ +use asynchronix::time::MonotonicTime; + +pub fn current_millis(time: MonotonicTime) -> u64 { + (time.as_secs() as u64 * 1000) + (time.subsec_nanos() as u64 / 1_000_000) +} diff --git a/satrs-minisim/src/udp.rs b/satrs-minisim/src/udp.rs new file mode 100644 index 0000000..f93b4ea --- /dev/null +++ b/satrs-minisim/src/udp.rs @@ -0,0 +1,152 @@ +use std::{ + collections::VecDeque, + net::{SocketAddr, UdpSocket}, + sync::{mpsc, Arc, Mutex}, + time::Duration, +}; + +use satrs_minisim::{SimReply, SimRequest}; + +pub type SharedSocketAddr = Arc>>; + +// A UDP server which handles all TC received by a client application. +pub struct UdpTcServer { + socket: UdpSocket, + request_sender: mpsc::Sender, + shared_last_sender: SharedSocketAddr, +} + +impl UdpTcServer { + pub fn new( + request_sender: mpsc::Sender, + shared_last_sender: SharedSocketAddr, + ) -> std::io::Result { + let socket = UdpSocket::bind("0.0.0.0:7303")?; + Ok(Self { + socket, + request_sender, + shared_last_sender, + }) + } + + pub fn run(&mut self) { + let mut last_socket_addr = None; + loop { + // Buffer to store incoming data. + let mut buffer = [0u8; 4096]; + // Block until data is received. `recv_from` returns the number of bytes read and the + // sender's address. + let (bytes_read, src) = self + .socket + .recv_from(&mut buffer) + .expect("could not read from socket"); + + // Convert the buffer into a string slice and print the message. + let req_string = std::str::from_utf8(&buffer[..bytes_read]) + .expect("Could not write buffer as string"); + println!("Received from {}: {}", src, req_string); + let sim_req: serde_json::Result = serde_json::from_str(req_string); + if sim_req.is_err() { + log::warn!( + "received UDP request with invalid format: {}", + sim_req.unwrap_err() + ); + continue; + } + self.request_sender.send(sim_req.unwrap()).unwrap(); + // Only set last sender if it has changed. + if last_socket_addr.is_some() && src != last_socket_addr.unwrap() { + self.shared_last_sender.lock().unwrap().replace(src); + } + last_socket_addr = Some(src); + } + } +} + +// A helper object which sends back all replies to the UDP client. +// +// This helper is scheduled separately to minimize the delay between the requests and replies. +pub struct UdpTmClient { + reply_receiver: mpsc::Receiver, + reply_queue: VecDeque, + max_num_replies: usize, + socket: UdpSocket, + last_sender: SharedSocketAddr, +} + +impl UdpTmClient { + pub fn new( + reply_receiver: mpsc::Receiver, + max_num_replies: usize, + last_sender: SharedSocketAddr, + ) -> Self { + let socket = + UdpSocket::bind("127.0.0.1:0").expect("creating UDP client for TM sender failed"); + Self { + reply_receiver, + reply_queue: VecDeque::new(), + max_num_replies, + socket, + last_sender, + } + } + + pub fn run(&mut self) { + loop { + let processed_replies = self.process_replies(); + let last_sender_lock = self + .last_sender + .lock() + .expect("locking last UDP sender failed"); + let last_sender = *last_sender_lock; + drop(last_sender_lock); + let mut sent_replies = false; + if let Some(last_sender) = last_sender { + sent_replies = self.send_replies(last_sender); + } + if !processed_replies && !sent_replies { + std::thread::sleep(Duration::from_millis(20)); + } + } + } + + fn process_replies(&mut self) -> bool { + let mut processed_replies = false; + loop { + match self.reply_receiver.try_recv() { + Ok(reply) => { + if self.reply_queue.len() >= self.max_num_replies { + self.reply_queue.pop_front(); + } + self.reply_queue.push_back(reply); + processed_replies = true; + } + Err(e) => match e { + mpsc::TryRecvError::Empty => return processed_replies, + mpsc::TryRecvError::Disconnected => { + log::error!("all UDP reply senders disconnected") + } + }, + } + } + } + + fn send_replies(&mut self, last_sender: SocketAddr) -> bool { + let mut sent_replies = false; + self.socket + .connect(last_sender) + .expect("connecting to last sender failed"); + while !self.reply_queue.is_empty() { + let next_reply_to_send = self.reply_queue.pop_front().unwrap(); + self.socket + .send( + serde_json::to_string(&next_reply_to_send) + .unwrap() + .as_bytes(), + ) + .expect("sending reply failed"); + sent_replies = true; + } + sent_replies + } +} -- 2.43.0 From c9f71125a3fa468aa7fae695d2dce2c15b726d03 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Mar 2024 16:15:43 +0100 Subject: [PATCH 08/10] add first unittests --- satrs-minisim/Cargo.toml | 3 + satrs-minisim/src/acs.rs | 2 + satrs-minisim/src/controller.rs | 32 ++++++++-- satrs-minisim/src/lib.rs | 2 +- satrs-minisim/src/main.rs | 104 +++++++++++++++++++++++++++++++- 5 files changed, 135 insertions(+), 8 deletions(-) diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index 53910e1..72fbff9 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -18,3 +18,6 @@ path = "../../asynchronix/asynchronix" [dependencies.satrs] path = "../satrs" + +[dev-dependencies] +delegate = "0.12" diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 604597d..45dc7b6 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -53,6 +53,8 @@ impl MagnetometerModel { } pub async fn send_sensor_values(&mut self, _: (), scheduler: &Scheduler) { + let current_time = scheduler.time(); + println!("current monotonic time: {:?}", current_time); let value = self.calculate_current_mgm_tuple(current_millis(scheduler.time())); let reply = SimReply { device: SimDevice::Mgm, diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs index 0d604f9..3c1e91a 100644 --- a/satrs-minisim/src/controller.rs +++ b/satrs-minisim/src/controller.rs @@ -21,20 +21,38 @@ pub struct SimController { } impl SimController { - pub fn run(&mut self, t0: MonotonicTime) { - let mut t = t0 + Duration::from_millis(10); + pub fn run(&mut self, start_time: MonotonicTime) { + let mut t = start_time + Duration::from_millis(1); + self.sys_clock.synchronize(t); loop { self.simulation .step_until(t) .expect("simulation step failed"); - t += Duration::from_millis(10); - // TODO: Received and handle requests. + // Check for UDP requests every millisecond. + t += Duration::from_millis(1); + self.handle_sim_requests(); - // TODO: Incorporate network latency. self.sys_clock.synchronize(t); } } + pub fn handle_sim_requests(&mut self) { + loop { + match self.request_receiver.try_recv() { + Ok(request) => match request.device { + satrs_minisim::SimDevice::Mgm => self.handle_mgm_request(&request.request), + satrs_minisim::SimDevice::Mgt => self.handle_mgt_request(&request.request), + satrs_minisim::SimDevice::Pcdu => self.handle_pcdu_request(&request.request), + }, + Err(e) => match e { + mpsc::TryRecvError::Empty => break, + mpsc::TryRecvError::Disconnected => { + panic!("all request sender disconnected") + } + }, + } + } + } fn handle_mgm_request(&mut self, request: &str) { let mgm_request: serde_json::Result = serde_json::from_str(request); if mgm_request.is_err() { @@ -54,7 +72,7 @@ impl SimController { } fn handle_pcdu_request(&mut self, request: &str) { - let pcdu_request: serde_json::Result = serde_json::from_str(&request); + let pcdu_request: serde_json::Result = serde_json::from_str(request); if pcdu_request.is_err() { log::warn!( "received invalid PCDU request: {}", @@ -67,4 +85,6 @@ impl SimController { PcduRequest::RequestSwitchInfo => todo!(), } } + + fn handle_mgt_request(&mut self, request: &str) {} } diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index b9ed24c..8731a31 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -1,6 +1,6 @@ use serde::{Deserialize, Serialize}; -#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] pub enum SimDevice { Mgm, Mgt, diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index bfb0c0b..4c5f1f6 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -1,5 +1,4 @@ use acs::MagnetometerModel; -use asynchronix::model::Model; use asynchronix::simulation::{Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, SystemClock}; use controller::SimController; @@ -61,3 +60,106 @@ fn main() { udp_tc_thread.join().expect("joining UDP TC thread failed"); udp_tm_thread.join().expect("joining UDP TM thread failed"); } + +#[cfg(test)] +mod tests { + use delegate::delegate; + use satrs_minisim::{ + acs::{MgmRequest, MgmSensorValues}, + SimDevice, SimReply, SimRequest, + }; + + use super::*; + + struct SimTestbench { + pub sim_controller: SimController, + pub reply_receiver: mpsc::Receiver, + pub request_sender: mpsc::Sender, + } + + impl SimTestbench { + fn new() -> Self { + let (request_sender, request_receiver) = mpsc::channel(); + let (reply_sender, reply_receiver) = mpsc::channel(); + // Instantiate models and their mailboxes. + let mgm_sim = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); + + let mgm_mailbox = Mailbox::new(); + let mgm_addr = mgm_mailbox.address(); + let pcdu_mailbox = Mailbox::new(); + let pcdu_addr = pcdu_mailbox.address(); + + // Instantiate the simulator + let t0 = MonotonicTime::EPOCH; + let sys_clock = SystemClock::from_system_time(t0, SystemTime::now()); + let simulation = SimInit::with_num_threads(1) + .add_model(mgm_sim, mgm_mailbox) + .init(t0); + + Self { + sim_controller: SimController { + sys_clock, + request_receiver, + simulation, + mgm_addr, + pcdu_addr, + }, + reply_receiver, + request_sender, + } + } + + delegate! { + to self.sim_controller { + pub fn handle_sim_requests(&mut self); + } + to self.sim_controller.simulation { + pub fn step(&mut self); + } + } + + pub fn send_request(&self, request: SimRequest) -> Result<(), mpsc::SendError> { + self.request_sender.send(request) + } + + pub fn try_receive_next_reply(&self) -> Option { + match self.reply_receiver.try_recv() { + Ok(reply) => Some(reply), + Err(e) => { + if e == mpsc::TryRecvError::Empty { + None + } else { + panic!("reply_receiver disconnected"); + } + } + } + } + } + + #[test] + fn test_basic_mgm_request() { + let mut sim_testbench = SimTestbench::new(); + let mgm_request = MgmRequest::RequestSensorData; + let request = SimRequest { + device: SimDevice::Mgm, + request: serde_json::to_string(&mgm_request).unwrap(), + }; + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests(); + sim_testbench.step(); + let sim_reply = sim_testbench.try_receive_next_reply(); + assert!(sim_reply.is_some()); + let sim_reply = sim_reply.unwrap(); + assert_eq!(sim_reply.device, SimDevice::Mgm); + let reply: MgmSensorValues = serde_json::from_str(&sim_reply.reply) + .expect("failed to deserialize MGM sensor values"); + assert_eq!(reply.x, 0.0); + assert_eq!(reply.y, 0.0); + assert_eq!(reply.z, 0.0); + } + + #[test] + fn test_basic_mgm_request_switched_on() {} +} -- 2.43.0 From 3dd6ad61553a98b4fac738a3f2b6dc535e6ec50b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Mar 2024 17:11:24 +0100 Subject: [PATCH 09/10] unittests are working well --- satrs-minisim/src/acs.rs | 10 ++-- satrs-minisim/src/controller.rs | 32 +++++++++--- satrs-minisim/src/eps.rs | 53 ++++++++++++++++--- satrs-minisim/src/lib.rs | 34 +++++++++++- satrs-minisim/src/main.rs | 92 ++++++++++++++++++--------------- satrs/src/power.rs | 9 ++++ 6 files changed, 168 insertions(+), 62 deletions(-) diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 45dc7b6..e09a77a 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -4,7 +4,7 @@ use asynchronix::{ model::{Model, Output}, time::Scheduler, }; -use satrs::power::SwitchState; +use satrs::power::{SwitchState, SwitchStateBinary}; use satrs_minisim::{ acs::{MgmSensorValues, MgtDipole, MGT_GEN_MAGNETIC_FIELD}, SimDevice, SimReply, @@ -32,7 +32,7 @@ const PHASE_Z: f32 = 0.2; /// a general purpose OS, but self self-sampling at a relatively high rate (20-40 ms) might /// stil lbe possible. pub struct MagnetometerModel { - pub switch_state: SwitchState, + pub switch_state: SwitchStateBinary, pub periodicity: Duration, pub external_mag_field: Option, pub reply_sender: mpsc::Sender, @@ -41,14 +41,14 @@ pub struct MagnetometerModel { impl MagnetometerModel { pub fn new(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { Self { - switch_state: SwitchState::Off, + switch_state: SwitchStateBinary::Off, periodicity, external_mag_field: None, reply_sender, } } - pub async fn switch_device(&mut self, switch_state: SwitchState) { + pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { self.switch_state = switch_state; } @@ -72,7 +72,7 @@ impl MagnetometerModel { } fn calculate_current_mgm_tuple(&mut self, time_ms: u64) -> MgmSensorValues { - if let SwitchState::On = self.switch_state { + if SwitchStateBinary::On == self.switch_state { if let Some(ext_field) = self.external_mag_field { return ext_field; } diff --git a/satrs-minisim/src/controller.rs b/satrs-minisim/src/controller.rs index 3c1e91a..e21247f 100644 --- a/satrs-minisim/src/controller.rs +++ b/satrs-minisim/src/controller.rs @@ -1,7 +1,10 @@ -use std::{sync::mpsc, time::Duration}; +use std::{ + sync::mpsc, + time::{Duration, SystemTime}, +}; use asynchronix::{ - simulation::{Address, Simulation}, + simulation::{Address, Mailbox, SimInit, Simulation}, time::{Clock, MonotonicTime, SystemClock}, }; use satrs_minisim::{acs::MgmRequest, SimRequest}; @@ -21,6 +24,22 @@ pub struct SimController { } impl SimController { + pub fn new( + sys_clock: SystemClock, + request_receiver: mpsc::Receiver, + simulation: Simulation, + mgm_addr: Address, + pcdu_addr: Address, + ) -> Self { + Self { + sys_clock, + request_receiver, + simulation, + mgm_addr, + pcdu_addr, + } + } + pub fn run(&mut self, start_time: MonotonicTime) { let mut t = start_time + Duration::from_millis(1); self.sys_clock.synchronize(t); @@ -39,10 +58,10 @@ impl SimController { pub fn handle_sim_requests(&mut self) { loop { match self.request_receiver.try_recv() { - Ok(request) => match request.device { - satrs_minisim::SimDevice::Mgm => self.handle_mgm_request(&request.request), - satrs_minisim::SimDevice::Mgt => self.handle_mgt_request(&request.request), - satrs_minisim::SimDevice::Pcdu => self.handle_pcdu_request(&request.request), + Ok(request) => match request.device() { + satrs_minisim::SimDevice::Mgm => self.handle_mgm_request(request.request()), + satrs_minisim::SimDevice::Mgt => self.handle_mgt_request(request.request()), + satrs_minisim::SimDevice::Pcdu => self.handle_pcdu_request(request.request()), }, Err(e) => match e { mpsc::TryRecvError::Empty => break, @@ -83,6 +102,7 @@ impl SimController { let pcdu_request = pcdu_request.unwrap(); match pcdu_request { PcduRequest::RequestSwitchInfo => todo!(), + PcduRequest::SwitchDevice => todo!(), } } diff --git a/satrs-minisim/src/eps.rs b/satrs-minisim/src/eps.rs index aa16afa..544038c 100644 --- a/satrs-minisim/src/eps.rs +++ b/satrs-minisim/src/eps.rs @@ -1,10 +1,19 @@ -use asynchronix::model::{Model, Output}; -use satrs::power::{SwitchState, SwitchStateBinary}; +use std::{sync::mpsc, time::Duration}; + +use asynchronix::{ + model::{Model, Output}, + time::Scheduler, +}; +use satrs::power::SwitchStateBinary; +use satrs_minisim::{SimDevice, SimReply}; use serde::{Deserialize, Serialize}; -#[derive(Debug, Clone, PartialEq, Serialize)] -pub struct PcduTuple {} +pub const SWITCH_INFO_DELAY_MS: u64 = 10; +#[derive(Debug, Clone, PartialEq, Serialize)] +pub struct SwitchInfo(Vec); + +#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] pub enum PcduSwitches { Mgm = 0, Mgt = 1, @@ -12,17 +21,45 @@ pub enum PcduSwitches { #[derive(Debug, Copy, Clone, Serialize, Deserialize)] pub enum PcduRequest { + SwitchDevice, RequestSwitchInfo, } pub struct PcduModel { - pub switcher_list: Output>, - pub mgm_switch: Output, - pub mgt_switch: Output, + pub current_switch_info: Vec, + pub mgm_switch: Output, + pub mgt_switch: Output, + pub reply_sender: mpsc::Sender, } impl PcduModel { - pub async fn switch_device(&mut self, switch: PcduSwitches, switch_state: SwitchState) { + pub fn new(reply_sender: mpsc::Sender) -> Self { + Self { + current_switch_info: vec![SwitchStateBinary::Off; 2], + mgm_switch: Output::new(), + mgt_switch: Output::new(), + reply_sender, + } + } + + pub async fn request_switch_info(&mut self, _: (), scheduler: &Scheduler) { + scheduler + .schedule_event( + Duration::from_millis(SWITCH_INFO_DELAY_MS), + Self::send_switch_info, + (), + ) + .expect("requesting switch info failed"); + } + + pub fn send_switch_info(&mut self) { + let switch_info = SwitchInfo(self.current_switch_info.clone()); + let reply = SimReply::new(SimDevice::Pcdu, switch_info); + self.reply_sender.send(reply).unwrap(); + } + + pub async fn switch_device(&mut self, switch: PcduSwitches, switch_state: SwitchStateBinary) { + self.current_switch_info[switch as usize] = switch_state; match switch { PcduSwitches::Mgm => { self.mgm_switch.send(switch_state).await; diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 8731a31..0723910 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -9,8 +9,25 @@ pub enum SimDevice { #[derive(Debug, Clone, Serialize, Deserialize)] pub struct SimRequest { - pub device: SimDevice, - pub request: String, + device: SimDevice, + request: String, +} + +impl SimRequest { + pub fn new(device: SimDevice, reply: T) -> Self { + Self { + device, + request: serde_json::to_string(&reply).unwrap(), + } + } + + pub fn device(&self) -> SimDevice { + self.device + } + + pub fn request(&self) -> &String { + &self.request + } } #[derive(Serialize, Deserialize)] @@ -19,6 +36,19 @@ pub struct SimReply { pub reply: String, } +impl SimReply { + pub fn new(device: SimDevice, reply: T) -> Self { + Self { + device, + reply: serde_json::to_string(&reply).unwrap(), + } + } + + pub fn reply(&self) -> &String { + &self.reply + } +} + pub mod acs { use super::*; diff --git a/satrs-minisim/src/main.rs b/satrs-minisim/src/main.rs index 4c5f1f6..3aa03ec 100644 --- a/satrs-minisim/src/main.rs +++ b/satrs-minisim/src/main.rs @@ -2,6 +2,8 @@ use acs::MagnetometerModel; use asynchronix::simulation::{Mailbox, SimInit}; use asynchronix::time::{MonotonicTime, SystemClock}; use controller::SimController; +use eps::PcduModel; +use satrs_minisim::{SimReply, SimRequest}; use std::sync::mpsc; use std::thread; use std::time::{Duration, SystemTime}; @@ -13,35 +15,57 @@ mod eps; mod time; mod udp; -fn main() { - let shared_socket_addr = SharedSocketAddr::default(); - let (request_sender, request_receiver) = mpsc::channel(); - let (reply_sender, reply_receiver) = mpsc::channel(); +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum ThreadingModel { + Default = 0, + Single = 1, +} +fn create_sim_controller( + threading_model: ThreadingModel, + start_time: MonotonicTime, + reply_sender: mpsc::Sender, + request_receiver: mpsc::Receiver, +) -> SimController { // Instantiate models and their mailboxes. - let mgm_sim = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); + let mgm_model = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); let mgm_mailbox = Mailbox::new(); let mgm_addr = mgm_mailbox.address(); let pcdu_mailbox = Mailbox::new(); let pcdu_addr = pcdu_mailbox.address(); - // Instantiate the simulator - let t0 = MonotonicTime::EPOCH; - let sys_clock = SystemClock::from_system_time(t0, SystemTime::now()); - let simulation = SimInit::new().add_model(mgm_sim, mgm_mailbox).init(t0); + let mut pcdu_model = PcduModel::new(reply_sender.clone()); - let mut sim_controller = SimController { - sys_clock, - request_receiver, - simulation, - mgm_addr, - pcdu_addr, + pcdu_model + .mgm_switch + .connect(MagnetometerModel::switch_device, &mgm_addr); + + // Instantiate the simulator + let sys_clock = SystemClock::from_system_time(start_time, SystemTime::now()); + let sim_init = if threading_model == ThreadingModel::Single { + SimInit::with_num_threads(1) + } else { + SimInit::new() }; + let simulation = sim_init + .add_model(mgm_model, mgm_mailbox) + .add_model(pcdu_model, pcdu_mailbox) + .init(start_time); + SimController::new(sys_clock, request_receiver, simulation, mgm_addr, pcdu_addr) +} + +fn main() { + let shared_socket_addr = SharedSocketAddr::default(); + let (request_sender, request_receiver) = mpsc::channel(); + let (reply_sender, reply_receiver) = mpsc::channel(); + let t0 = MonotonicTime::EPOCH; + let mut sim_ctrl = + create_sim_controller(ThreadingModel::Default, t0, reply_sender, request_receiver); // This thread schedules the simulator. let sim_thread = thread::spawn(move || { - sim_controller.run(t0); + sim_ctrl.run(t0); }); let mut server = UdpTcServer::new(request_sender, shared_socket_addr.clone()).unwrap(); @@ -69,6 +93,8 @@ mod tests { SimDevice, SimReply, SimRequest, }; + use crate::eps::PcduRequest; + use super::*; struct SimTestbench { @@ -81,29 +107,12 @@ mod tests { fn new() -> Self { let (request_sender, request_receiver) = mpsc::channel(); let (reply_sender, reply_receiver) = mpsc::channel(); - // Instantiate models and their mailboxes. - let mgm_sim = MagnetometerModel::new(Duration::from_millis(50), reply_sender.clone()); - - let mgm_mailbox = Mailbox::new(); - let mgm_addr = mgm_mailbox.address(); - let pcdu_mailbox = Mailbox::new(); - let pcdu_addr = pcdu_mailbox.address(); - - // Instantiate the simulator let t0 = MonotonicTime::EPOCH; - let sys_clock = SystemClock::from_system_time(t0, SystemTime::now()); - let simulation = SimInit::with_num_threads(1) - .add_model(mgm_sim, mgm_mailbox) - .init(t0); + let sim_ctrl = + create_sim_controller(ThreadingModel::Single, t0, reply_sender, request_receiver); Self { - sim_controller: SimController { - sys_clock, - request_receiver, - simulation, - mgm_addr, - pcdu_addr, - }, + sim_controller: sim_ctrl, reply_receiver, request_sender, } @@ -140,10 +149,7 @@ mod tests { fn test_basic_mgm_request() { let mut sim_testbench = SimTestbench::new(); let mgm_request = MgmRequest::RequestSensorData; - let request = SimRequest { - device: SimDevice::Mgm, - request: serde_json::to_string(&mgm_request).unwrap(), - }; + let request = SimRequest::new(SimDevice::Mgm, mgm_request); sim_testbench .send_request(request) .expect("sending MGM request failed"); @@ -161,5 +167,9 @@ mod tests { } #[test] - fn test_basic_mgm_request_switched_on() {} + fn test_basic_mgm_request_switched_on() { + let mut sim_testbench = SimTestbench::new(); + let pcdu_request = PcduRequest::RequestSwitchInfo; + let request = SimRequest::new(SimDevice::Pcdu, pcdu_request); + } } diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 7786651..1e1fda1 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -42,6 +42,15 @@ impl TryFrom for SwitchStateBinary { } } +impl> From for SwitchStateBinary { + fn from(value: T) -> Self { + if value.into() == 0 { + return SwitchStateBinary::Off; + } + SwitchStateBinary::On + } +} + impl From for SwitchState { fn from(value: SwitchStateBinary) -> Self { match value { -- 2.43.0 From 86275af208c25b0795fde79c155db8a62ba6a6f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Mar 2024 17:14:09 +0100 Subject: [PATCH 10/10] use upstream asynchronix again --- satrs-minisim/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/satrs-minisim/Cargo.toml b/satrs-minisim/Cargo.toml index 72fbff9..db45674 100644 --- a/satrs-minisim/Cargo.toml +++ b/satrs-minisim/Cargo.toml @@ -11,8 +11,8 @@ serde_json = "1" log = "0.4" [dependencies.asynchronix] -version = "0.2" -path = "../../asynchronix/asynchronix" +version = "0.2.1" +# path = "../../asynchronix/asynchronix" # git = "https://github.com/us-irs/asynchronix.git" # branch = "clock-not-sendable" -- 2.43.0