From db814189a0b879ccdf9544b594fb54017a2aef52 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 5 Mar 2024 00:43:01 +0100 Subject: [PATCH] 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()); }