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(); + }); +}