sat-rs/satrs-minisim/src/acs.rs

use std::{f32::consts::PI, sync::mpsc, time::Duration};

use asynchronix::{
    model::{Model, Output},
    time::Scheduler,
};
use satrs::power::{SwitchState, SwitchStateBinary};
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: SwitchStateBinary,
    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: SwitchStateBinary::Off,
            periodicity,
            external_mag_field: None,
            reply_sender,
        }
    }

    pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) {
        self.switch_state = switch_state;
    }

    pub async fn send_sensor_values(&mut self, _: (), scheduler: &Scheduler<Self>) {
        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,
            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 SwitchStateBinary::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 {}