add MGT and PCDU model
This commit is contained in:
parent
96f0c90838
commit
db814189a0
@ -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"
|
||||
|
@ -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<MgmTuple>,
|
||||
pub struct MagnetometerModel {
|
||||
pub switch_state: SwitchState,
|
||||
pub external_mag_field: Option<MgmTuple>,
|
||||
pub sensor_values: Output<MgmTuple>,
|
||||
}
|
||||
|
||||
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<Self>) {
|
||||
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<SwitchState>,
|
||||
pub mgt_switch: Output<SwitchState>,
|
||||
}
|
||||
|
||||
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<Dipole>,
|
||||
gen_magnetic_field: Output<MgmTuple>,
|
||||
}
|
||||
|
||||
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<Self>) {
|
||||
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<Self>,
|
||||
) -> std::pin::Pin<
|
||||
Box<
|
||||
dyn std::future::Future<Output = asynchronix::model::InitializedModel<Self>>
|
||||
+ 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());
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user