diff --git a/satrs-example/client/src/main.rs b/satrs-example/client/src/main.rs index 9cd9bc8..f0c2eff 100644 --- a/satrs-example/client/src/main.rs +++ b/satrs-example/client/src/main.rs @@ -4,7 +4,8 @@ use clap::Parser as _; use satrs_example::config::{OBSW_SERVER_ADDR, SERVER_PORT}; use satrs_minisim::{ SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, - SimReply, SimRequest, acs, acs::MgmRequestLis3Mdl, acs::SpiFault, udp::SIM_CTRL_PORT, + SimReply, SimRequest, acs, acs::MgmRequestLis3Mdl, acs::MgmRequestLis3MdlMgm0, + acs::MgmRequestLis3MdlMgm1, acs::SpiFault, udp::SIM_CTRL_PORT, }; use spacepackets::{CcsdsPacketIdAndPsc, SpacePacketHeader}; use std::{ @@ -202,13 +203,13 @@ fn handle_mgm_command( args: MgmArgs, ) -> anyhow::Result<()> { if let Some(mode) = args.fault { - if target_id != types::ComponentId::AcsMgm0 { - bail!("SPI fault injection is only supported for MGM0 right now (minisim limitation)"); - } - inject_mgm_failure(SpiFault { - mode: mode.into(), - cleared_by_power_cycle: args.fault_kind == FaultKind::Transient, - })?; + inject_mgm_failure( + target_id, + SpiFault { + mode: mode.into(), + cleared_by_power_cycle: args.fault_kind == FaultKind::Transient, + }, + )?; } if args.ping { let request = types::ccsds::CcsdsTcPacketOwned::new_with_request( @@ -494,12 +495,12 @@ fn main() -> anyhow::Result<()> { Ok(()) } -/// Injects the given SPI fault directly into minisim's MGM0 model, bypassing the OBSW. +/// Injects the given SPI fault directly into minisim's MGM model, bypassing the OBSW. /// /// Confirms the simulator is actually reachable first (same ping/pong check the OBSW's own /// internal sim client does, see `SimClientUdp::attempt_connection`), since a fire-and-forget /// UDP send would otherwise silently do nothing if minisim is not running. -fn inject_mgm_failure(fault: SpiFault) -> anyhow::Result<()> { +fn inject_mgm_failure(target_id: types::ComponentId, fault: SpiFault) -> anyhow::Result<()> { let sim_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), SIM_CTRL_PORT); let sim_socket = UdpSocket::bind("127.0.0.1:0")?; sim_socket.set_read_timeout(Some(Duration::from_millis(200)))?; @@ -531,9 +532,18 @@ fn inject_mgm_failure(fault: SpiFault) -> anyhow::Result<()> { Err(e) => return Err(e.into()), } - let request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::SetSpiFault(fault)); + let fault_request = MgmRequestLis3Mdl::SetSpiFault(fault); + let request = match target_id { + types::ComponentId::AcsMgm0 => { + SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0(fault_request)) + } + types::ComponentId::AcsMgm1 => { + SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm1(fault_request)) + } + _ => bail!("SPI fault injection is not supported for {target_id:?}"), + }; sim_socket.send_to(&serde_json::to_vec(&request)?, sim_addr)?; - log::info!("injected SPI fault {fault:?} into minisim MGM0"); + log::info!("injected SPI fault {fault:?} into minisim {target_id:?}"); Ok(()) } diff --git a/satrs-example/minisim/src/acs.rs b/satrs-example/minisim/src/acs.rs deleted file mode 100644 index 69e2620..0000000 --- a/satrs-example/minisim/src/acs.rs +++ /dev/null @@ -1,446 +0,0 @@ -use std::{f32::consts::PI, sync::mpsc, time::Duration}; - -use nexosim::{ - model::{Context, Model}, - ports::Output, -}; -use satrs_minisim::{ - acs::{ - lis3mdl::MgmLis3MdlReply, MgmReplyCommon, MgmReplyProvider, MgmSensorValuesMicroTesla, - MgtDipole, MgtHkSet, MgtReply, SpiFault, MGT_GEN_MAGNETIC_FIELD, - }, - SimReply, -}; -use types::pcdu::SwitchStateBinary; - -use crate::time::current_millis; - -// Earth magnetic field varies between roughly -30 uT and 30 uT -const AMPLITUDE_MGM_UT: f32 = 30.0; -// 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. -/// -/// An ideal sensor 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 still be possible and is probably sufficient for many OBSW needs. -pub struct MagnetometerModel { - pub switch_state: SwitchStateBinary, - #[allow(dead_code)] - pub periodicity: Duration, - pub external_mag_field: Option, - pub spi_fault: SpiFault, - pub reply_sender: mpsc::Sender, - pub phatom: std::marker::PhantomData, -} - -impl MagnetometerModel { - pub fn new_for_lis3mdl(periodicity: Duration, reply_sender: mpsc::Sender) -> Self { - Self { - switch_state: SwitchStateBinary::Off, - periodicity, - external_mag_field: None, - spi_fault: SpiFault::default(), - reply_sender, - phatom: std::marker::PhantomData, - } - } -} - -impl MagnetometerModel { - pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { - self.switch_state = switch_state; - if switch_state == SwitchStateBinary::Off && self.spi_fault.cleared_by_power_cycle { - self.spi_fault = SpiFault::default(); - } - } - - /// Force (or clear) a stuck-bus SPI fault, for FDIR testing purposes. - pub async fn set_spi_fault(&mut self, fault: SpiFault) { - self.spi_fault = fault; - } - - pub async fn send_sensor_values(&mut self, _: (), scheduler: &mut Context) { - self.reply_sender - .send(ReplyProvider::create_mgm_reply( - MgmReplyCommon { - switch_state: self.switch_state, - sensor_values: self - .calculate_current_mgm_tuple(current_millis(scheduler.time())), - }, - self.spi_fault.mode, - )) - .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: MgmSensorValuesMicroTesla) { - self.external_mag_field = Some(field); - } - - fn calculate_current_mgm_tuple(&self, time_ms: u64) -> MgmSensorValuesMicroTesla { - 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 * FREQUENCY_MGM * (time_ms as f32 / 1000.0); - return MgmSensorValuesMicroTesla { - x: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_X).sin(), - y: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Y).sin(), - z: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Z).sin(), - }; - } - MgmSensorValuesMicroTesla { - x: 0.0, - y: 0.0, - z: 0.0, - } - } -} - -impl Model for MagnetometerModel {} - -pub struct MagnetorquerModel { - switch_state: SwitchStateBinary, - torquing: bool, - torque_dipole: MgtDipole, - pub gen_magnetic_field: Output, - reply_sender: mpsc::Sender, -} - -impl MagnetorquerModel { - pub fn new(reply_sender: mpsc::Sender) -> Self { - Self { - switch_state: SwitchStateBinary::Off, - torquing: false, - torque_dipole: MgtDipole::default(), - gen_magnetic_field: Output::new(), - reply_sender, - } - } - - pub async fn apply_torque( - &mut self, - duration_and_dipole: (Duration, MgtDipole), - cx: &mut Context, - ) { - self.torque_dipole = duration_and_dipole.1; - self.torquing = true; - if cx - .schedule_event(duration_and_dipole.0, 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 = MgtDipole::default(); - self.torquing = false; - self.generate_magnetic_field(()).await; - } - - pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { - self.switch_state = switch_state; - self.generate_magnetic_field(()).await; - } - - pub async fn request_housekeeping_data(&mut self, _: (), cx: &mut Context) { - if self.switch_state != SwitchStateBinary::On { - return; - } - cx.schedule_event(Duration::from_millis(15), Self::send_housekeeping_data, ()) - .expect("requesting housekeeping data failed") - } - - pub fn send_housekeeping_data(&mut self) { - self.reply_sender - .send(SimReply::new(&MgtReply::Hk(MgtHkSet { - dipole: self.torque_dipole, - torquing: self.torquing, - }))) - .unwrap(); - } - - fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValuesMicroTesla { - // 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 != SwitchStateBinary::On || !self.torquing { - return; - } - self.gen_magnetic_field - .send(self.calc_magnetic_field(self.torque_dipole)) - .await; - } -} - -impl Model for MagnetorquerModel {} - -#[cfg(test)] -pub mod tests { - use std::time::Duration; - - use satrs_minisim::{ - acs::{ - lis3mdl::{self, MgmLis3MdlReply}, - MgmRequestLis3Mdl, MgtDipole, MgtHkSet, MgtReply, MgtRequest, SpiFault, SpiFaultMode, - }, - SerializableSimMsgPayload, SimComponent, SimMessageProvider, SimRequest, - }; - use types::pcdu::{SwitchId, SwitchStateBinary}; - - use crate::{ - eps::tests::{switch_device_off, switch_device_on}, - test_helpers::SimTestbench, - }; - - #[test] - fn test_basic_mgm_request() { - let mut sim_testbench = SimTestbench::new(); - let request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - let sim_reply = sim_testbench.try_receive_next_reply(); - assert!(sim_reply.is_some()); - let sim_reply = sim_reply.unwrap(); - assert_eq!(sim_reply.component(), SimComponent::Mgm0Lis3Mdl); - let reply = MgmLis3MdlReply::from_sim_message(&sim_reply) - .expect("failed to deserialize MGM sensor values"); - assert_eq!(reply.common.switch_state, SwitchStateBinary::Off); - assert_eq!(reply.common.sensor_values.x, 0.0); - assert_eq!(reply.common.sensor_values.y, 0.0); - assert_eq!(reply.common.sensor_values.z, 0.0); - } - - fn inject_spi_fault(sim_testbench: &mut SimTestbench, cleared_by_power_cycle: bool) { - let fault_request = - SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::SetSpiFault(SpiFault { - mode: SpiFaultMode::AllOnes, - cleared_by_power_cycle, - })); - sim_testbench - .send_request(fault_request) - .expect("sending MGM fault injection request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - } - - fn request_mgm_reply(sim_testbench: &mut SimTestbench) -> MgmLis3MdlReply { - let data_request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); - sim_testbench - .send_request(data_request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - let sim_reply = sim_testbench - .try_receive_next_reply() - .expect("no MGM reply received"); - MgmLis3MdlReply::from_sim_message(&sim_reply) - .expect("failed to deserialize MGM sensor values") - } - - fn is_stuck_bus_reply(reply: &MgmLis3MdlReply) -> bool { - reply.raw.x == -1 && reply.raw.y == -1 && reply.raw.z == -1 - } - - #[test] - fn test_mgm_spi_fault_injection_all_ones() { - let mut sim_testbench = SimTestbench::new(); - switch_device_on(&mut sim_testbench, SwitchId::Mgm0); - inject_spi_fault(&mut sim_testbench, false); - - let reply = request_mgm_reply(&mut sim_testbench); - // Even though the device is switched on, the injected fault forces a stuck-bus reply. - assert_eq!(reply.common.switch_state, SwitchStateBinary::On); - assert!(is_stuck_bus_reply(&reply)); - } - - #[test] - fn test_mgm_spi_fault_cleared_by_power_cycle() { - let mut sim_testbench = SimTestbench::new(); - switch_device_on(&mut sim_testbench, SwitchId::Mgm0); - inject_spi_fault(&mut sim_testbench, true); - assert!(is_stuck_bus_reply(&request_mgm_reply(&mut sim_testbench))); - - switch_device_off(&mut sim_testbench, SwitchId::Mgm0); - switch_device_on(&mut sim_testbench, SwitchId::Mgm0); - sim_testbench.step_until(Duration::from_millis(50)).unwrap(); - assert!(!is_stuck_bus_reply(&request_mgm_reply(&mut sim_testbench))); - } - - #[test] - fn test_mgm_spi_fault_persists_after_power_cycle() { - let mut sim_testbench = SimTestbench::new(); - switch_device_on(&mut sim_testbench, SwitchId::Mgm0); - inject_spi_fault(&mut sim_testbench, false); - - switch_device_off(&mut sim_testbench, SwitchId::Mgm0); - switch_device_on(&mut sim_testbench, SwitchId::Mgm0); - let reply = request_mgm_reply(&mut sim_testbench); - assert_eq!(reply.common.switch_state, SwitchStateBinary::On); - assert!(is_stuck_bus_reply(&reply)); - } - - #[test] - fn test_basic_mgm_request_switched_on() { - let mut sim_testbench = SimTestbench::new(); - switch_device_on(&mut sim_testbench, SwitchId::Mgm0); - - let mut request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - let mut sim_reply_res = sim_testbench.try_receive_next_reply(); - assert!(sim_reply_res.is_some()); - let mut sim_reply = sim_reply_res.unwrap(); - assert_eq!(sim_reply.component(), SimComponent::Mgm0Lis3Mdl); - let first_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) - .expect("failed to deserialize MGM sensor values"); - sim_testbench.step_until(Duration::from_millis(50)).unwrap(); - - request = SimRequest::new_with_epoch_time(MgmRequestLis3Mdl::RequestSensorData); - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - sim_reply_res = sim_testbench.try_receive_next_reply(); - assert!(sim_reply_res.is_some()); - sim_reply = sim_reply_res.unwrap(); - - let second_reply = MgmLis3MdlReply::from_sim_message(&sim_reply) - .expect("failed to deserialize MGM sensor values"); - let x_conv_back = second_reply.raw.x as f32 - * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS - * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; - let y_conv_back = second_reply.raw.y as f32 - * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS - * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; - let z_conv_back = second_reply.raw.z as f32 - * lis3mdl::FIELD_LSB_PER_GAUSS_4_SENS - * lis3mdl::GAUSS_TO_MICROTESLA_FACTOR as f32; - let diff_x = (second_reply.common.sensor_values.x - x_conv_back).abs(); - assert!(diff_x < 0.01, "diff x too large: {}", diff_x); - let diff_y = (second_reply.common.sensor_values.y - y_conv_back).abs(); - assert!(diff_y < 0.01, "diff y too large: {}", diff_y); - let diff_z = (second_reply.common.sensor_values.z - z_conv_back).abs(); - assert!(diff_z < 0.01, "diff z too large: {}", diff_z); - // assert_eq!(second_reply.raw_reply, SwitchStateBinary::On); - // Check that the values are changing. - assert!(first_reply != second_reply); - } - - #[test] - fn test_basic_mgt_request_is_off() { - let mut sim_testbench = SimTestbench::new(); - let request = SimRequest::new_with_epoch_time(MgtRequest::RequestHk); - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - let sim_reply_res = sim_testbench.try_receive_next_reply(); - assert!(sim_reply_res.is_none()); - } - - #[test] - fn test_basic_mgt_request_is_on() { - let mut sim_testbench = SimTestbench::new(); - switch_device_on(&mut sim_testbench, SwitchId::Mgt); - let request = SimRequest::new_with_epoch_time(MgtRequest::RequestHk); - - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - let sim_reply_res = sim_testbench.try_receive_next_reply(); - assert!(sim_reply_res.is_some()); - let sim_reply = sim_reply_res.unwrap(); - let mgt_reply = MgtReply::from_sim_message(&sim_reply) - .expect("failed to deserialize MGM sensor values"); - match mgt_reply { - MgtReply::Hk(hk) => { - assert_eq!(hk.dipole, MgtDipole::default()); - assert!(!hk.torquing); - } - _ => panic!("unexpected reply"), - } - } - - fn check_mgt_hk(sim_testbench: &mut SimTestbench, expected_hk_set: MgtHkSet) { - let request = SimRequest::new_with_epoch_time(MgtRequest::RequestHk); - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step().unwrap(); - let sim_reply_res = sim_testbench.try_receive_next_reply(); - assert!(sim_reply_res.is_some()); - let sim_reply = sim_reply_res.unwrap(); - let mgt_reply = MgtReply::from_sim_message(&sim_reply) - .expect("failed to deserialize MGM sensor values"); - match mgt_reply { - MgtReply::Hk(hk) => { - assert_eq!(hk, expected_hk_set); - } - _ => panic!("unexpected reply"), - } - } - - #[test] - fn test_basic_mgt_request_is_on_and_torquing() { - let mut sim_testbench = SimTestbench::new(); - switch_device_on(&mut sim_testbench, SwitchId::Mgt); - let commanded_dipole = MgtDipole { - x: -200, - y: 200, - z: 1000, - }; - let request = SimRequest::new_with_epoch_time(MgtRequest::ApplyTorque { - duration: Duration::from_millis(100), - dipole: commanded_dipole, - }); - sim_testbench - .send_request(request) - .expect("sending MGM request failed"); - sim_testbench.handle_sim_requests_time_agnostic(); - sim_testbench.step_until(Duration::from_millis(5)).unwrap(); - - check_mgt_hk( - &mut sim_testbench, - MgtHkSet { - dipole: commanded_dipole, - torquing: true, - }, - ); - sim_testbench - .step_until(Duration::from_millis(100)) - .unwrap(); - check_mgt_hk( - &mut sim_testbench, - MgtHkSet { - dipole: MgtDipole::default(), - torquing: false, - }, - ); - } -} diff --git a/satrs-example/minisim/src/acs/mgm.rs b/satrs-example/minisim/src/acs/mgm.rs new file mode 100644 index 0000000..94ab3c9 --- /dev/null +++ b/satrs-example/minisim/src/acs/mgm.rs @@ -0,0 +1,315 @@ +use std::{f32::consts::PI, sync::mpsc, time::Duration}; + +use nexosim::model::{Context, Model}; +use satrs_minisim::{ + acs::{ + mgm::{MgmId, MgmReply, MgmReplyWrapper}, + MgmSensorValuesMicroTesla, SpiFault, + }, + SimReply, +}; +use types::pcdu::SwitchStateBinary; + +use crate::time::current_millis; +// Earth magnetic field varies between roughly -30 uT and 30 uT +const AMPLITUDE_MGM_UT: f32 = 30.0; +// 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. +/// +/// An ideal sensor 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 still be possible and is probably sufficient for many OBSW needs. +pub struct MagnetometerModel { + pub id: MgmId, + pub switch_state: SwitchStateBinary, + #[allow(dead_code)] + pub periodicity: Duration, + pub external_mag_field: Option, + pub spi_fault: SpiFault, + pub reply_sender: mpsc::Sender, +} + +impl MagnetometerModel { + pub fn new(mgm_id: MgmId, periodicity: Duration, reply_sender: mpsc::Sender) -> Self { + Self { + id: mgm_id, + switch_state: SwitchStateBinary::Off, + periodicity, + external_mag_field: None, + spi_fault: SpiFault::default(), + reply_sender, + } + } + + pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { + self.switch_state = switch_state; + if switch_state == SwitchStateBinary::Off && self.spi_fault.cleared_by_power_cycle { + self.spi_fault = SpiFault::default(); + } + } + + /// Force (or clear) a stuck-bus SPI fault, for FDIR testing purposes. + pub async fn set_spi_fault(&mut self, fault: SpiFault) { + self.spi_fault = fault; + } + + pub async fn send_sensor_values(&mut self, _: (), scheduler: &mut Context) { + let reply = MgmReplyWrapper { + mgm_id: self.id, + reply: MgmReply::new( + self.switch_state, + self.calculate_current_mgm_tuple(current_millis(scheduler.time())), + self.spi_fault.mode, + ), + }; + self.reply_sender + .send(reply.to_sim_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: MgmSensorValuesMicroTesla) { + self.external_mag_field = Some(field); + } + + fn calculate_current_mgm_tuple(&self, time_ms: u64) -> MgmSensorValuesMicroTesla { + 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 * FREQUENCY_MGM * (time_ms as f32 / 1000.0); + return MgmSensorValuesMicroTesla { + x: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_X).sin(), + y: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Y).sin(), + z: AMPLITUDE_MGM_UT * (base_sin_val + PHASE_Z).sin(), + }; + } + MgmSensorValuesMicroTesla { + x: 0.0, + y: 0.0, + z: 0.0, + } + } +} + +impl Model for MagnetometerModel {} + +#[cfg(test)] +mod tests { + use std::time::Duration; + + use satrs_minisim::{ + acs::{ + mgm::{self, MgmId, MgmReply, MgmReplyWrapper}, + MgmRequestLis3Mdl, MgmRequestLis3MdlMgm0, MgmRequestLis3MdlMgm1, SpiFault, + SpiFaultMode, + }, + SimComponent, SimMessageProvider, SimRequest, + }; + use types::pcdu::{SwitchId, SwitchStateBinary}; + + use crate::{ + eps::tests::{switch_device_off, switch_device_on}, + test_helpers::SimTestbench, + }; + + #[test] + fn test_basic_mgm_request() { + let mut sim_testbench = SimTestbench::new(); + let request = SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0( + MgmRequestLis3Mdl::RequestSensorData, + )); + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + let sim_reply = sim_testbench.try_receive_next_reply(); + assert!(sim_reply.is_some()); + let sim_reply = sim_reply.unwrap(); + assert_eq!(sim_reply.component(), SimComponent::Mgm0Lis3Mdl); + let wrapper = MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to deserialize MGM sensor values"); + assert_eq!(wrapper.mgm_id, MgmId::Mgm0); + assert_eq!(wrapper.reply.switch_state, SwitchStateBinary::Off); + assert_eq!(wrapper.reply.sensor_values.x, 0.0); + assert_eq!(wrapper.reply.sensor_values.y, 0.0); + assert_eq!(wrapper.reply.sensor_values.z, 0.0); + } + + fn inject_spi_fault(sim_testbench: &mut SimTestbench, cleared_by_power_cycle: bool) { + let fault_request = SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0( + MgmRequestLis3Mdl::SetSpiFault(SpiFault { + mode: SpiFaultMode::AllOnes, + cleared_by_power_cycle, + }), + )); + sim_testbench + .send_request(fault_request) + .expect("sending MGM fault injection request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + } + + fn request_mgm_reply(sim_testbench: &mut SimTestbench) -> MgmReply { + let data_request = SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0( + MgmRequestLis3Mdl::RequestSensorData, + )); + sim_testbench + .send_request(data_request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + let sim_reply = sim_testbench + .try_receive_next_reply() + .expect("no MGM reply received"); + MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to deserialize MGM sensor values") + .reply + } + + fn is_stuck_bus_reply(reply: &MgmReply) -> bool { + reply.raw.x == -1 && reply.raw.y == -1 && reply.raw.z == -1 + } + + #[test] + fn test_mgm_spi_fault_injection_all_ones() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgm0); + inject_spi_fault(&mut sim_testbench, false); + + let reply = request_mgm_reply(&mut sim_testbench); + // Even though the device is switched on, the injected fault forces a stuck-bus reply. + assert_eq!(reply.switch_state, SwitchStateBinary::On); + assert!(is_stuck_bus_reply(&reply)); + } + + #[test] + fn test_mgm_spi_fault_cleared_by_power_cycle() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgm0); + inject_spi_fault(&mut sim_testbench, true); + assert!(is_stuck_bus_reply(&request_mgm_reply(&mut sim_testbench))); + + switch_device_off(&mut sim_testbench, SwitchId::Mgm0); + switch_device_on(&mut sim_testbench, SwitchId::Mgm0); + sim_testbench.step_until(Duration::from_millis(50)).unwrap(); + assert!(!is_stuck_bus_reply(&request_mgm_reply(&mut sim_testbench))); + } + + #[test] + fn test_mgm_spi_fault_persists_after_power_cycle() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgm0); + inject_spi_fault(&mut sim_testbench, false); + + switch_device_off(&mut sim_testbench, SwitchId::Mgm0); + switch_device_on(&mut sim_testbench, SwitchId::Mgm0); + let reply = request_mgm_reply(&mut sim_testbench); + assert_eq!(reply.switch_state, SwitchStateBinary::On); + assert!(is_stuck_bus_reply(&reply)); + } + + #[test] + fn test_basic_mgm_request_switched_on() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgm0); + + let mut request = SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0( + MgmRequestLis3Mdl::RequestSensorData, + )); + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + let mut sim_reply_res = sim_testbench.try_receive_next_reply(); + assert!(sim_reply_res.is_some()); + let mut sim_reply = sim_reply_res.unwrap(); + assert_eq!(sim_reply.component(), SimComponent::Mgm0Lis3Mdl); + let first_reply = MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to deserialize MGM sensor values") + .reply; + sim_testbench.step_until(Duration::from_millis(50)).unwrap(); + + request = SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0( + MgmRequestLis3Mdl::RequestSensorData, + )); + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + sim_reply_res = sim_testbench.try_receive_next_reply(); + assert!(sim_reply_res.is_some()); + sim_reply = sim_reply_res.unwrap(); + + let second_reply = MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to deserialize MGM sensor values") + .reply; + let x_conv_back = second_reply.raw.x as f32 + * mgm::FIELD_LSB_PER_GAUSS_4_SENS + * mgm::GAUSS_TO_MICROTESLA_FACTOR as f32; + let y_conv_back = second_reply.raw.y as f32 + * mgm::FIELD_LSB_PER_GAUSS_4_SENS + * mgm::GAUSS_TO_MICROTESLA_FACTOR as f32; + let z_conv_back = second_reply.raw.z as f32 + * mgm::FIELD_LSB_PER_GAUSS_4_SENS + * mgm::GAUSS_TO_MICROTESLA_FACTOR as f32; + let diff_x = (second_reply.sensor_values.x - x_conv_back).abs(); + assert!(diff_x < 0.01, "diff x too large: {}", diff_x); + let diff_y = (second_reply.sensor_values.y - y_conv_back).abs(); + assert!(diff_y < 0.01, "diff y too large: {}", diff_y); + let diff_z = (second_reply.sensor_values.z - z_conv_back).abs(); + assert!(diff_z < 0.01, "diff z too large: {}", diff_z); + // assert_eq!(second_reply.raw_reply, SwitchStateBinary::On); + // Check that the values are changing. + assert!(first_reply != second_reply); + } + + #[test] + fn test_mgm_1_request_switched_on() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgm1); + + for request in [ + SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0( + MgmRequestLis3Mdl::RequestSensorData, + )), + SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm1( + MgmRequestLis3Mdl::RequestSensorData, + )), + ] { + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + } + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + + let sim_reply = sim_testbench + .try_receive_next_reply() + .expect("no MGM0 reply received"); + assert_eq!(sim_reply.component(), SimComponent::Mgm0Lis3Mdl); + let mgm_0_reply = MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to deserialize MGM0 sensor values"); + assert_eq!(mgm_0_reply.mgm_id, MgmId::Mgm0); + assert_eq!(mgm_0_reply.reply.switch_state, SwitchStateBinary::Off); + + let sim_reply = sim_testbench + .try_receive_next_reply() + .expect("no MGM1 reply received"); + assert_eq!(sim_reply.component(), SimComponent::Mgm1Lis3Mdl); + let mgm_1_reply = MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to deserialize MGM1 sensor values"); + assert_eq!(mgm_1_reply.mgm_id, MgmId::Mgm1); + assert_eq!(mgm_1_reply.reply.switch_state, SwitchStateBinary::On); + } +} diff --git a/satrs-example/minisim/src/acs/mgt.rs b/satrs-example/minisim/src/acs/mgt.rs new file mode 100644 index 0000000..661fe49 --- /dev/null +++ b/satrs-example/minisim/src/acs/mgt.rs @@ -0,0 +1,202 @@ +use nexosim::{ + model::{Context, Model}, + ports::Output, +}; +use satrs_minisim::{ + acs::{MgmSensorValuesMicroTesla, MgtDipole, MgtHkSet, MgtReply, MGT_GEN_MAGNETIC_FIELD}, + SimReply, +}; +use std::{sync::mpsc, time::Duration}; +use types::pcdu::SwitchStateBinary; + +pub struct MagnetorquerModel { + switch_state: SwitchStateBinary, + torquing: bool, + torque_dipole: MgtDipole, + pub gen_magnetic_field: Output, + reply_sender: mpsc::Sender, +} + +impl MagnetorquerModel { + pub fn new(reply_sender: mpsc::Sender) -> Self { + Self { + switch_state: SwitchStateBinary::Off, + torquing: false, + torque_dipole: MgtDipole::default(), + gen_magnetic_field: Output::new(), + reply_sender, + } + } + + pub async fn apply_torque( + &mut self, + duration_and_dipole: (Duration, MgtDipole), + cx: &mut Context, + ) { + self.torque_dipole = duration_and_dipole.1; + self.torquing = true; + if cx + .schedule_event(duration_and_dipole.0, 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 = MgtDipole::default(); + self.torquing = false; + self.generate_magnetic_field(()).await; + } + + pub async fn switch_device(&mut self, switch_state: SwitchStateBinary) { + self.switch_state = switch_state; + self.generate_magnetic_field(()).await; + } + + pub async fn request_housekeeping_data(&mut self, _: (), cx: &mut Context) { + if self.switch_state != SwitchStateBinary::On { + return; + } + cx.schedule_event(Duration::from_millis(15), Self::send_housekeeping_data, ()) + .expect("requesting housekeeping data failed") + } + + pub fn send_housekeeping_data(&mut self) { + self.reply_sender + .send(SimReply::new(&MgtReply::Hk(MgtHkSet { + dipole: self.torque_dipole, + torquing: self.torquing, + }))) + .unwrap(); + } + + fn calc_magnetic_field(&self, _: MgtDipole) -> MgmSensorValuesMicroTesla { + // 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 != SwitchStateBinary::On || !self.torquing { + return; + } + self.gen_magnetic_field + .send(self.calc_magnetic_field(self.torque_dipole)) + .await; + } +} + +impl Model for MagnetorquerModel {} + +#[cfg(test)] +mod tests { + use std::time::Duration; + + use satrs_minisim::{ + acs::{MgtDipole, MgtHkSet, MgtReply, MgtRequest}, + SerializableSimMsgPayload, SimRequest, + }; + use types::pcdu::SwitchId; + + use crate::{eps::tests::switch_device_on, test_helpers::SimTestbench}; + + #[test] + fn test_basic_mgt_request_is_off() { + let mut sim_testbench = SimTestbench::new(); + let request = SimRequest::new_with_epoch_time(MgtRequest::RequestHk); + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + let sim_reply_res = sim_testbench.try_receive_next_reply(); + assert!(sim_reply_res.is_none()); + } + + #[test] + fn test_basic_mgt_request_is_on() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgt); + let request = SimRequest::new_with_epoch_time(MgtRequest::RequestHk); + + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + let sim_reply_res = sim_testbench.try_receive_next_reply(); + assert!(sim_reply_res.is_some()); + let sim_reply = sim_reply_res.unwrap(); + let mgt_reply = MgtReply::from_sim_message(&sim_reply) + .expect("failed to deserialize MGM sensor values"); + match mgt_reply { + MgtReply::Hk(hk) => { + assert_eq!(hk.dipole, MgtDipole::default()); + assert!(!hk.torquing); + } + _ => panic!("unexpected reply"), + } + } + + fn check_mgt_hk(sim_testbench: &mut SimTestbench, expected_hk_set: MgtHkSet) { + let request = SimRequest::new_with_epoch_time(MgtRequest::RequestHk); + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step().unwrap(); + let sim_reply_res = sim_testbench.try_receive_next_reply(); + assert!(sim_reply_res.is_some()); + let sim_reply = sim_reply_res.unwrap(); + let mgt_reply = MgtReply::from_sim_message(&sim_reply) + .expect("failed to deserialize MGM sensor values"); + match mgt_reply { + MgtReply::Hk(hk) => { + assert_eq!(hk, expected_hk_set); + } + _ => panic!("unexpected reply"), + } + } + + #[test] + fn test_basic_mgt_request_is_on_and_torquing() { + let mut sim_testbench = SimTestbench::new(); + switch_device_on(&mut sim_testbench, SwitchId::Mgt); + let commanded_dipole = MgtDipole { + x: -200, + y: 200, + z: 1000, + }; + let request = SimRequest::new_with_epoch_time(MgtRequest::ApplyTorque { + duration: Duration::from_millis(100), + dipole: commanded_dipole, + }); + sim_testbench + .send_request(request) + .expect("sending MGM request failed"); + sim_testbench.handle_sim_requests_time_agnostic(); + sim_testbench.step_until(Duration::from_millis(5)).unwrap(); + + check_mgt_hk( + &mut sim_testbench, + MgtHkSet { + dipole: commanded_dipole, + torquing: true, + }, + ); + sim_testbench + .step_until(Duration::from_millis(100)) + .unwrap(); + check_mgt_hk( + &mut sim_testbench, + MgtHkSet { + dipole: MgtDipole::default(), + torquing: false, + }, + ); + } +} diff --git a/satrs-example/minisim/src/acs/mod.rs b/satrs-example/minisim/src/acs/mod.rs new file mode 100644 index 0000000..af30ef2 --- /dev/null +++ b/satrs-example/minisim/src/acs/mod.rs @@ -0,0 +1,2 @@ +pub mod mgm; +pub mod mgt; diff --git a/satrs-example/minisim/src/controller.rs b/satrs-example/minisim/src/controller.rs index c805128..15b23dd 100644 --- a/satrs-example/minisim/src/controller.rs +++ b/satrs-example/minisim/src/controller.rs @@ -5,14 +5,14 @@ use nexosim::{ time::{Clock, MonotonicTime, SystemClock}, }; use satrs_minisim::{ - acs::{lis3mdl::MgmLis3MdlReply, MgmRequestLis3Mdl, MgtRequest}, + acs::{MgmRequestLis3Mdl, MgmRequestLis3MdlMgm0, MgmRequestLis3MdlMgm1, MgtRequest}, eps::PcduRequest, SerializableSimMsgPayload, SimComponent, SimCtrlReply, SimCtrlRequest, SimMessageProvider, SimReply, SimRequest, SimRequestError, }; use crate::{ - acs::{MagnetometerModel, MagnetorquerModel}, + acs::{mgm::MagnetometerModel, mgt::MagnetorquerModel}, eps::PcduModel, }; @@ -24,8 +24,8 @@ const PCDU_REQ_WIRETAPPING: bool = false; const MGT_REQ_WIRETAPPING: bool = false; pub struct ModelAddrWrapper { - mgm_0_addr: Address>, - mgm_1_addr: Address>, + mgm_0_addr: Address, + mgm_1_addr: Address, pcdu_addr: Address, mgt_addr: Address, } @@ -43,8 +43,8 @@ pub struct SimController { impl ModelAddrWrapper { pub fn new( - mgm_0_addr: Address>, - mgm_1_addr: Address>, + mgm_0_addr: Address, + mgm_1_addr: Address, pcdu_addr: Address, mgt_addr: Address, ) -> Self { @@ -138,29 +138,27 @@ impl SimController { mgm_idx: usize, request: &SimRequest, ) -> Result<(), SimRequestError> { - let mgm_request = MgmRequestLis3Mdl::from_sim_message(request)?; + let (mgm_request, addr) = match mgm_idx { + 0 => ( + MgmRequestLis3MdlMgm0::from_sim_message(request)?.0, + &self.addr_wrapper.mgm_0_addr, + ), + 1 => ( + MgmRequestLis3MdlMgm1::from_sim_message(request)?.0, + &self.addr_wrapper.mgm_1_addr, + ), + _ => panic!("invalid mgm index"), + }; if MGM_REQ_WIRETAPPING { - log::info!("received MGM request: {mgm_request:?}"); + log::info!("received MGM{mgm_idx} request: {mgm_request:?}"); } match mgm_request { MgmRequestLis3Mdl::RequestSensorData => { - let addr = match mgm_idx { - 0 => &self.addr_wrapper.mgm_0_addr, - 1 => &self.addr_wrapper.mgm_1_addr, - - _ => panic!("invalid mgm index"), - }; self.simulation .process_event(MagnetometerModel::send_sensor_values, (), addr) .expect("event execution error for mgm"); } MgmRequestLis3Mdl::SetSpiFault(fault_mode) => { - let addr = match mgm_idx { - 0 => &self.addr_wrapper.mgm_0_addr, - 1 => &self.addr_wrapper.mgm_1_addr, - - _ => panic!("invalid mgm index"), - }; log::info!("MGM{mgm_idx}: setting SPI fault mode to {fault_mode:?}"); self.simulation .process_event(MagnetometerModel::set_spi_fault, fault_mode, addr) diff --git a/satrs-example/minisim/src/eps.rs b/satrs-example/minisim/src/eps.rs index b3626ee..d8360a2 100644 --- a/satrs-example/minisim/src/eps.rs +++ b/satrs-example/minisim/src/eps.rs @@ -58,10 +58,12 @@ impl PcduModel { SwitchId::Mgm0 => { self.mgm_0_switch.send(switch_and_target_state.1).await; } + SwitchId::Mgm1 => { + self.mgm_1_switch.send(switch_and_target_state.1).await; + } SwitchId::Mgt => { self.mgt_switch.send(switch_and_target_state.1).await; } - SwitchId::Mgm1 => todo!(), } } } diff --git a/satrs-example/minisim/src/lib.rs b/satrs-example/minisim/src/lib.rs index 33e21fb..826329a 100644 --- a/satrs-example/minisim/src/lib.rs +++ b/satrs-example/minisim/src/lib.rs @@ -105,6 +105,16 @@ impl SimReply { }, } } + + /// For payloads where the target is only known at runtime. + pub fn new_with_target(target: SimComponent, reply: &T) -> Self { + Self { + inner: SimMessage { + target, + payload: serde_json::to_string(reply).unwrap(), + }, + } + } } impl SimMessageProvider for SimReply { @@ -201,10 +211,6 @@ pub mod acs { use super::*; - pub trait MgmReplyProvider: Send + 'static { - fn create_mgm_reply(common: MgmReplyCommon, fault_mode: SpiFaultMode) -> SimReply; - } - /// Fault mode injected on the simulated SPI bus, independent of the switch state. /// /// Models the classic symptom of a stuck SPI bus: an undriven MISO line commonly reads @@ -233,10 +239,20 @@ pub mod acs { SetSpiFault(SpiFault), } - impl SerializableSimMsgPayload for MgmRequestLis3Mdl { + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub struct MgmRequestLis3MdlMgm0(pub MgmRequestLis3Mdl); + + impl SerializableSimMsgPayload for MgmRequestLis3MdlMgm0 { const TARGET: SimComponent = SimComponent::Mgm0Lis3Mdl; } + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub struct MgmRequestLis3MdlMgm1(pub MgmRequestLis3Mdl); + + impl SerializableSimMsgPayload for MgmRequestLis3MdlMgm1 { + const TARGET: SimComponent = SimComponent::Mgm1Lis3Mdl; + } + // 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. The unit is micro tesla. @@ -248,10 +264,7 @@ pub mod acs { } #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmReplyCommon { - pub switch_state: SwitchStateBinary, - pub sensor_values: MgmSensorValuesMicroTesla, - } + pub struct MgmReplyCommon {} pub const MGT_GEN_MAGNETIC_FIELD: MgmSensorValuesMicroTesla = MgmSensorValuesMicroTesla { x: 30.0, @@ -261,7 +274,9 @@ pub mod acs { pub const ALL_ONES_SENSOR_VAL: i16 = 0xffff_u16 as i16; pub const ALL_ZEROS_SENSOR_VAL: i16 = 0; - pub mod lis3mdl { + /// MGM module strongly based on the LIS3MDL device. + pub mod mgm { + use super::*; // Field data register scaling @@ -272,27 +287,70 @@ pub mod acs { pub const FIELD_LSB_PER_GAUSS_16_SENS: f32 = 1.0 / 1711.0; #[derive(Default, Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmLis3RawValues { + pub struct RawValues { pub x: i16, pub y: i16, pub z: i16, } #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] - pub struct MgmLis3MdlReply { - pub common: MgmReplyCommon, + pub struct MgmReply { + pub switch_state: SwitchStateBinary, + pub sensor_values: MgmSensorValuesMicroTesla, // Raw sensor values which are transmitted by the LIS3 device in little-endian // order. - pub raw: MgmLis3RawValues, + pub raw: RawValues, } - impl MgmLis3MdlReply { - pub fn new(common: MgmReplyCommon, fault_mode: SpiFaultMode) -> Self { + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub enum MgmId { + Mgm0, + Mgm1, + } + + impl MgmId { + pub const fn sim_component(&self) -> SimComponent { + match self { + MgmId::Mgm0 => SimComponent::Mgm0Lis3Mdl, + MgmId::Mgm1 => SimComponent::Mgm1Lis3Mdl, + } + } + } + + /// Does not implement [SerializableSimMsgPayload] because the target depends on the + /// MGM ID, which is only known at runtime. + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub struct MgmReplyWrapper { + pub mgm_id: MgmId, + pub reply: MgmReply, + } + + impl MgmReplyWrapper { + pub fn to_sim_reply(&self) -> SimReply { + SimReply::new_with_target(self.mgm_id.sim_component(), self) + } + + pub fn from_sim_reply(sim_reply: &SimReply) -> Result { + let wrapper: Self = serde_json::from_str(sim_reply.payload())?; + if wrapper.mgm_id.sim_component() != sim_reply.component() { + return Err(SimMessageError::TargetRequestMissmatch(sim_reply.clone())); + } + Ok(wrapper) + } + } + + impl MgmReply { + pub fn new( + switch_state: SwitchStateBinary, + sensor_values: MgmSensorValuesMicroTesla, + fault_mode: SpiFaultMode, + ) -> Self { match fault_mode { SpiFaultMode::AllZeros => { return Self { - common, - raw: MgmLis3RawValues { + switch_state, + sensor_values, + raw: RawValues { x: ALL_ZEROS_SENSOR_VAL, y: ALL_ZEROS_SENSOR_VAL, z: ALL_ZEROS_SENSOR_VAL, @@ -301,8 +359,9 @@ pub mod acs { } SpiFaultMode::AllOnes => { return Self { - common, - raw: MgmLis3RawValues { + switch_state, + sensor_values, + raw: RawValues { x: ALL_ONES_SENSOR_VAL, y: ALL_ONES_SENSOR_VAL, z: ALL_ONES_SENSOR_VAL, @@ -311,10 +370,11 @@ pub mod acs { } SpiFaultMode::None => (), } - match common.switch_state { + match switch_state { SwitchStateBinary::Off => Self { - common, - raw: MgmLis3RawValues { + switch_state, + sensor_values, + raw: RawValues { x: ALL_ONES_SENSOR_VAL, y: ALL_ONES_SENSOR_VAL, z: ALL_ONES_SENSOR_VAL, @@ -322,13 +382,13 @@ pub mod acs { }, SwitchStateBinary::On => { let mut raw_reply: [u8; 7] = [0; 7]; - let raw_x: i16 = (common.sensor_values.x + let raw_x: i16 = (sensor_values.x / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) .round() as i16; - let raw_y: i16 = (common.sensor_values.y + let raw_y: i16 = (sensor_values.y / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) .round() as i16; - let raw_z: i16 = (common.sensor_values.z + let raw_z: i16 = (sensor_values.z / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) .round() as i16; // The first byte is a dummy byte. @@ -336,8 +396,9 @@ pub mod acs { raw_reply[3..5].copy_from_slice(&raw_y.to_be_bytes()); raw_reply[5..7].copy_from_slice(&raw_z.to_be_bytes()); Self { - common, - raw: MgmLis3RawValues { + switch_state, + sensor_values, + raw: RawValues { x: raw_x, y: raw_y, z: raw_z, @@ -347,16 +408,6 @@ pub mod acs { } } } - - impl SerializableSimMsgPayload for MgmLis3MdlReply { - const TARGET: SimComponent = SimComponent::Mgm0Lis3Mdl; - } - - impl MgmReplyProvider for MgmLis3MdlReply { - fn create_mgm_reply(common: MgmReplyCommon, fault_mode: SpiFaultMode) -> SimReply { - SimReply::new(&Self::new(common, fault_mode)) - } - } } // Simple model using i16 values. diff --git a/satrs-example/minisim/src/main.rs b/satrs-example/minisim/src/main.rs index 5e03ee7..3c754b1 100644 --- a/satrs-example/minisim/src/main.rs +++ b/satrs-example/minisim/src/main.rs @@ -1,8 +1,9 @@ -use acs::{MagnetometerModel, MagnetorquerModel}; +use acs::{mgm::MagnetometerModel, mgt::MagnetorquerModel}; use controller::{ModelAddrWrapper, SimController}; use eps::PcduModel; use nexosim::simulation::{Mailbox, SimInit}; use nexosim::time::{MonotonicTime, SystemClock}; +use satrs_minisim::acs::mgm::MgmId; use satrs_minisim::udp::SIM_CTRL_PORT; use satrs_minisim::{SimReply, SimRequest}; use std::sync::mpsc; @@ -32,9 +33,9 @@ fn create_sim_controller( ) -> SimController { // Instantiate models and their mailboxes. let mgm_0_model = - MagnetometerModel::new_for_lis3mdl(Duration::from_millis(50), reply_sender.clone()); + MagnetometerModel::new(MgmId::Mgm0, Duration::from_millis(50), reply_sender.clone()); let mgm_1_model = - MagnetometerModel::new_for_lis3mdl(Duration::from_millis(50), reply_sender.clone()); + MagnetometerModel::new(MgmId::Mgm1, Duration::from_millis(50), reply_sender.clone()); let mgm_0_mailbox = Mailbox::new(); let mgm_0_addr = mgm_0_mailbox.address(); diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 565bc0b..a059784 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -2,11 +2,11 @@ use satrs::fdir::{FaultCounterStd, FaultResponse, RecoveryEvent, RecoveryFdir}; use satrs::health::HealthTableMapSync; use satrs::spacepackets::CcsdsPacketIdAndPsc; use satrs_example::{HkHelperSingleSet, TimestampHelper, TmtcQueues}; -use satrs_minisim::acs::MgmRequestLis3Mdl; -use satrs_minisim::acs::lis3mdl::{ - FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, MgmLis3MdlReply, MgmLis3RawValues, +use satrs_minisim::acs::mgm::{ + FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, MgmReplyWrapper, RawValues, }; -use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; +use satrs_minisim::acs::{MgmRequestLis3Mdl, MgmRequestLis3MdlMgm0, MgmRequestLis3MdlMgm1}; +use satrs_minisim::{SimReply, SimRequest}; use std::sync::mpsc; use std::sync::{Arc, Mutex}; use std::time::Duration; @@ -76,7 +76,7 @@ impl MgmId { #[derive(Default)] pub struct SpiDummyInterface { - pub dummy_values: MgmLis3RawValues, + pub dummy_values: RawValues, } impl SpiDummyInterface { @@ -90,7 +90,7 @@ impl SpiDummyInterface { #[derive(Default)] pub struct TestSpiInterface { pub call_count: u32, - pub next_mgm_data: MgmLis3RawValues, + pub next_mgm_data: RawValues, } impl TestSpiInterface { @@ -103,6 +103,7 @@ impl TestSpiInterface { } pub struct SpiSimInterface { + pub id: MgmId, pub sim_request_tx: mpsc::Sender, pub sim_reply_rx: mpsc::Receiver, } @@ -111,16 +112,18 @@ impl SpiSimInterface { // Right now, we only support requesting sensor data and not configuration of the sensor. fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) { let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; - if let Err(e) = self - .sim_request_tx - .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) - { + let sim_request = match self.id { + MgmId::_0 => SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm0(mgm_sensor_request)), + MgmId::_1 => SimRequest::new_with_epoch_time(MgmRequestLis3MdlMgm1(mgm_sensor_request)), + }; + if let Err(e) = self.sim_request_tx.send(sim_request) { log::error!("failed to send MGM LIS3 request: {e}"); } match self.sim_reply_rx.recv_timeout(Duration::from_millis(50)) { Ok(sim_reply) => { - let sim_reply_lis3 = MgmLis3MdlReply::from_sim_message(&sim_reply) - .expect("failed to parse LIS3 reply"); + let sim_reply_lis3 = MgmReplyWrapper::from_sim_reply(&sim_reply) + .expect("failed to parse LIS3 reply") + .reply; rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2] .copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes()); rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2] @@ -644,7 +647,7 @@ mod tests { use arbitrary_int::u11; use satrs::health::{HealthState, HealthTableProvider}; use satrs::spacepackets::SpacePacketHeader; - use satrs_minisim::acs::lis3mdl::MgmLis3RawValues; + use satrs_minisim::acs::mgm::RawValues; use types::{ Apid, ComponentId, TcHeader, acs::mgm::request::HkRequest, @@ -763,7 +766,7 @@ mod tests { } pub fn inject_stuck_bus(&mut self) { - self.test_spi_interface().next_mgm_data = MgmLis3RawValues { + self.test_spi_interface().next_mgm_data = RawValues { x: -1, y: -1, z: -1, @@ -904,7 +907,7 @@ mod tests { #[test] fn test_normal_handler_mgm_set_conversion() { let mut testbench = MgmTestbench::new(); - let raw_values = MgmLis3RawValues { + let raw_values = RawValues { x: 1000, y: -1000, z: 1000, @@ -1057,7 +1060,7 @@ mod tests { fn test_spi_fault_below_threshold_stays_healthy() { let mut testbench = MgmTestbench::new(); testbench.switch_to_normal(); - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues { + testbench.test_spi_interface().next_mgm_data = RawValues { x: -1, y: -1, z: -1, @@ -1098,7 +1101,7 @@ mod tests { testbench.drain_switch_requests(); testbench.mode_report_rx.try_iter().for_each(drop); testbench.exceed_spi_fault_threshold(); - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues::default(); + testbench.test_spi_interface().next_mgm_data = RawValues::default(); let call_count = testbench.test_spi_interface().call_count; testbench.complete_power_cycle(); @@ -1271,7 +1274,7 @@ mod tests { testbench.drain_events(); testbench.mode_report_rx.try_iter().for_each(drop); testbench.exceed_spi_fault_threshold(); - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues::default(); + testbench.test_spi_interface().next_mgm_data = RawValues::default(); // The switch never turns off. Every failed power cycle costs a recovery attempt. for _ in 0..RECOVERY_THRESHOLD { @@ -1309,7 +1312,7 @@ mod tests { let mut testbench = MgmTestbench::new(); testbench.switch_to_normal(); testbench.exceed_spi_fault_threshold(); - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues::default(); + testbench.test_spi_interface().next_mgm_data = RawValues::default(); testbench .tc_tx .send(create_request_tc( @@ -1374,7 +1377,7 @@ mod tests { testbench .health_table .set_health(ComponentId::AcsMgm0.into(), HealthState::ExternalControl); - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues { + testbench.test_spi_interface().next_mgm_data = RawValues { x: -1, y: -1, z: -1, @@ -1393,7 +1396,7 @@ mod tests { fn test_recovering_from_spi_fault_clears_invalid_data_flag() { let mut testbench = MgmTestbench::new(); testbench.switch_to_normal(); - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues { + testbench.test_spi_interface().next_mgm_data = RawValues { x: -1, y: -1, z: -1, @@ -1402,7 +1405,7 @@ mod tests { assert!(!testbench.handler.shared_mgm_set.lock().unwrap().valid); // Bus recovers before the threshold is exceeded. - testbench.test_spi_interface().next_mgm_data = MgmLis3RawValues::default(); + testbench.test_spi_interface().next_mgm_data = RawValues::default(); testbench.handler.periodic_operation(); assert_eq!( testbench.health_table.health(ComponentId::AcsMgm0.into()), diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 2adef6f..8b56378 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -181,10 +181,12 @@ fn main() { .add_reply_recipient(satrs_minisim::SimComponent::Mgm1Lis3Mdl, mgm_1_sim_reply_tx); ( mgm::SpiCommunication::Sim(mgm::SpiSimInterface { + id: mgm::MgmId::_0, sim_request_tx: sim_request_tx.clone(), sim_reply_rx: mgm_0_sim_reply_rx, }), mgm::SpiCommunication::Sim(mgm::SpiSimInterface { + id: mgm::MgmId::_1, sim_request_tx: sim_request_tx.clone(), sim_reply_rx: mgm_1_sim_reply_rx, }),