modularized the mini simulator
Some checks failed
Rust/sat-rs/pipeline/pr-main There was a failure building this commit
Some checks failed
Rust/sat-rs/pipeline/pr-main There was a failure building this commit
This commit is contained in:
parent
0a41de5e70
commit
3ad06f63c7
148
satrs-minisim/src/acs.rs
Normal file
148
satrs-minisim/src/acs.rs
Normal file
@ -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<MgmSensorValues>,
|
||||
pub reply_sender: mpsc::Sender<SimReply>,
|
||||
}
|
||||
|
||||
impl MagnetometerModel {
|
||||
pub fn new(periodicity: Duration, reply_sender: mpsc::Sender<SimReply>) -> 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<Self>) {
|
||||
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<MgtDipole>,
|
||||
gen_magnetic_field: Output<MgmSensorValues>,
|
||||
}
|
||||
|
||||
impl MagnetorquerModel {
|
||||
pub async fn apply_torque(
|
||||
&mut self,
|
||||
dipole: MgtDipole,
|
||||
torque_duration: Duration,
|
||||
scheduler: &Scheduler<Self>,
|
||||
) {
|
||||
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 {}
|
70
satrs-minisim/src/controller.rs
Normal file
70
satrs-minisim/src/controller.rs
Normal file
@ -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<SimRequest>,
|
||||
pub simulation: Simulation,
|
||||
pub mgm_addr: Address<MagnetometerModel>,
|
||||
pub pcdu_addr: Address<PcduModel>,
|
||||
}
|
||||
|
||||
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<MgmRequest> = 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<PcduRequest> = 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!(),
|
||||
}
|
||||
}
|
||||
}
|
37
satrs-minisim/src/eps.rs
Normal file
37
satrs-minisim/src/eps.rs
Normal file
@ -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<Vec<SwitchStateBinary>>,
|
||||
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 {}
|
53
satrs-minisim/src/lib.rs
Normal file
53
satrs-minisim/src/lib.rs
Normal file
@ -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,
|
||||
}
|
||||
}
|
@ -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<MgmTuple>,
|
||||
pub sensor_values: Output<MgmTuple>,
|
||||
}
|
||||
|
||||
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>) {
|
||||
self.generate_output_self_scheduling((), scheduler).await;
|
||||
}
|
||||
|
||||
pub async fn switch_device(&mut self, switch_state: SwitchState, scheduler: &Scheduler<Self>) {
|
||||
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>,
|
||||
) {
|
||||
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<Self>,
|
||||
) -> impl Future<Output = ()> + 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<Self>) {
|
||||
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<Vec<SwitchStateBinary>>,
|
||||
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 {}
|
||||
|
||||
// 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<Dipole>,
|
||||
gen_magnetic_field: Output<MgmTuple>,
|
||||
}
|
||||
|
||||
impl MagnetorquerModel {
|
||||
pub async fn apply_torque(
|
||||
&mut self,
|
||||
dipole: Dipole,
|
||||
torque_duration: Duration,
|
||||
scheduler: &Scheduler<Self>,
|
||||
) {
|
||||
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<SimReply>,
|
||||
last_sender: Arc<Mutex<Option<SocketAddr>>>,
|
||||
}
|
||||
|
||||
#[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<Mutex<Option<SocketAddr>>>;
|
||||
|
||||
// A UDP server which handles all TC received by a client application.
|
||||
pub struct UdpTcServer {
|
||||
socket: UdpSocket,
|
||||
request_sender: mpsc::Sender<SimRequest>,
|
||||
last_sender: SharedSocketAddr,
|
||||
}
|
||||
|
||||
impl UdpTcServer {
|
||||
pub fn new(
|
||||
request_sender: mpsc::Sender<SimRequest>,
|
||||
last_sender: SharedSocketAddr,
|
||||
) -> io::Result<Self> {
|
||||
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<SimRequest> = 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<PcduRequest> = 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<SimRequest>,
|
||||
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");
|
||||
}
|
||||
|
5
satrs-minisim/src/time.rs
Normal file
5
satrs-minisim/src/time.rs
Normal file
@ -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)
|
||||
}
|
152
satrs-minisim/src/udp.rs
Normal file
152
satrs-minisim/src/udp.rs
Normal file
@ -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<Mutex<Option<SocketAddr>>>;
|
||||
|
||||
// A UDP server which handles all TC received by a client application.
|
||||
pub struct UdpTcServer {
|
||||
socket: UdpSocket,
|
||||
request_sender: mpsc::Sender<SimRequest>,
|
||||
shared_last_sender: SharedSocketAddr,
|
||||
}
|
||||
|
||||
impl UdpTcServer {
|
||||
pub fn new(
|
||||
request_sender: mpsc::Sender<SimRequest>,
|
||||
shared_last_sender: SharedSocketAddr,
|
||||
) -> std::io::Result<Self> {
|
||||
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<SimRequest> = 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<SimReply>,
|
||||
reply_queue: VecDeque<SimReply>,
|
||||
max_num_replies: usize,
|
||||
socket: UdpSocket,
|
||||
last_sender: SharedSocketAddr,
|
||||
}
|
||||
|
||||
impl UdpTmClient {
|
||||
pub fn new(
|
||||
reply_receiver: mpsc::Receiver<SimReply>,
|
||||
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
|
||||
}
|
||||
}
|
Loading…
x
Reference in New Issue
Block a user