From 7606767f634a7c90bb35ef4f101bc8f99c4c3918 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 11 May 2024 19:11:41 +0200 Subject: [PATCH] the PCDU handler is already required --- satrs-example/src/acs/mgm.rs | 43 ++--- satrs-example/src/eps/mod.rs | 1 + satrs-example/src/eps/pcdu.rs | 44 ++++++ satrs-example/src/main.rs | 1 + satrs-minisim/src/acs.rs | 13 +- satrs-minisim/src/lib.rs | 53 ++++--- satrs/src/power.rs | 290 +++++++++++++++++++++++++++------- 7 files changed, 339 insertions(+), 106 deletions(-) create mode 100644 satrs-example/src/eps/mod.rs create mode 100644 satrs-example/src/eps/pcdu.rs diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 21424b5..8d0c03d 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -6,7 +6,7 @@ use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; use satrs::spacepackets::SpHeader; use satrs_example::{DeviceMode, TimestampHelper}; use satrs_minisim::acs::lis3mdl::{ - MgmLis3MdlReply, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, + MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR, }; use satrs_minisim::acs::MgmRequestLis3Mdl; use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest}; @@ -47,18 +47,16 @@ pub trait SpiInterface { #[derive(Default)] pub struct SpiDummyInterface { - pub dummy_val_0: i16, - pub dummy_val_1: i16, - pub dummy_val_2: i16, + pub dummy_values: MgmLis3RawValues, } impl SpiInterface for SpiDummyInterface { type Error = (); fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { - rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_0.to_le_bytes()); - rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_1.to_be_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_val_2.to_be_bytes()); + rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.x.to_le_bytes()); + rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.y.to_be_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.z.to_be_bytes()); Ok(()) } } @@ -74,18 +72,27 @@ impl SpiInterface for SpiSimInterface { // Right now, we only support requesting sensor data and not configuration of the sensor. fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData; - self.sim_request_tx + if let Err(e) = self + .sim_request_tx .send(SimRequest::new_with_epoch_time(mgm_sensor_request)) - .expect("failed to send request"); - let sim_reply = self - .sim_reply_rx - .recv_timeout(Duration::from_millis(100)) - .expect("reply timeout"); - let sim_reply_lis3 = - MgmLis3MdlReply::from_sim_message(&sim_reply).expect("failed to parse LIS3 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].copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes()); - rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes()); + { + 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"); + 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] + .copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes()); + rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2] + .copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes()); + } + Err(e) => { + log::warn!("MGM LIS3 SIM reply timeout: {}", e); + } + } Ok(()) } } diff --git a/satrs-example/src/eps/mod.rs b/satrs-example/src/eps/mod.rs new file mode 100644 index 0000000..055b3ab --- /dev/null +++ b/satrs-example/src/eps/mod.rs @@ -0,0 +1 @@ +pub mod pcdu; diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs new file mode 100644 index 0000000..e572cf5 --- /dev/null +++ b/satrs-example/src/eps/pcdu.rs @@ -0,0 +1,44 @@ +use std::{ + collections::HashMap, + sync::{mpsc, Arc, Mutex}, +}; + +use derive_new::new; +use satrs::{ + mode::ModeAndSubmode, + power::SwitchState, + pus::EcssTmSender, + request::{GenericMessage, UniqueApidTargetId}, +}; +use satrs_example::TimestampHelper; + +use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest}; + +pub trait SerialInterface {} + +#[derive(Copy, Clone, PartialEq, Eq, Hash)] +#[repr(u32)] +pub enum SwitchId { + Mgm0 = 0, + Mgt = 1, +} + +pub type SwitchMap = HashMap; + +/// Example PCDU device handler. +#[derive(new)] +#[allow(clippy::too_many_arguments)] +pub struct PcduHandler { + id: UniqueApidTargetId, + dev_str: &'static str, + mode_interface: MpscModeLeafInterface, + composite_request_rx: mpsc::Receiver>, + hk_reply_tx: mpsc::Sender>, + tm_sender: TmSender, + pub com_interface: ComInterface, + shared_switch_map: Arc>, + #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] + mode_and_submode: ModeAndSubmode, + #[new(default)] + stamp_helper: TimestampHelper, +} diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs index 80ce229..712e895 100644 --- a/satrs-example/src/main.rs +++ b/satrs-example/src/main.rs @@ -1,4 +1,5 @@ mod acs; +mod eps; mod events; mod hk; mod interface; diff --git a/satrs-minisim/src/acs.rs b/satrs-minisim/src/acs.rs index 299d09d..2403487 100644 --- a/satrs-minisim/src/acs.rs +++ b/satrs-minisim/src/acs.rs @@ -15,7 +15,7 @@ use satrs_minisim::{ use crate::time::current_millis; -// Earth magnetic field varies between -30 uT and 30 uT +// 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; @@ -26,14 +26,9 @@ 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. +/// 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, pub periodicity: Duration, diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs index 361508e..124ef32 100644 --- a/satrs-minisim/src/lib.rs +++ b/satrs-minisim/src/lib.rs @@ -236,6 +236,7 @@ pub mod acs { y: -30.0, z: 30.0, }; + pub const ALL_ONES_SENSOR_VAL: i16 = 0xffff_u16 as i16; pub mod lis3mdl { use super::*; @@ -264,27 +265,39 @@ pub mod acs { impl MgmLis3MdlReply { pub fn new(common: MgmReplyCommon) -> Self { - let mut raw_reply: [u8; 7] = [0; 7]; - let raw_x: i16 = (common.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 - / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) - .round() as i16; - let raw_z: i16 = (common.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. - raw_reply[1..3].copy_from_slice(&raw_x.to_be_bytes()); - 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 { - x: raw_x, - y: raw_y, - z: raw_z, + match common.switch_state { + SwitchStateBinary::Off => Self { + common, + raw: MgmLis3RawValues { + x: ALL_ONES_SENSOR_VAL, + y: ALL_ONES_SENSOR_VAL, + z: ALL_ONES_SENSOR_VAL, + }, }, + SwitchStateBinary::On => { + let mut raw_reply: [u8; 7] = [0; 7]; + let raw_x: i16 = (common.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 + / (GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS)) + .round() as i16; + let raw_z: i16 = (common.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. + raw_reply[1..3].copy_from_slice(&raw_x.to_be_bytes()); + 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 { + x: raw_x, + y: raw_y, + z: raw_z, + }, + } + } } } } diff --git a/satrs/src/power.rs b/satrs/src/power.rs index 1e1fda1..48eae0e 100644 --- a/satrs/src/power.rs +++ b/satrs/src/power.rs @@ -1,22 +1,15 @@ +use derive_new::new; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +#[cfg(feature = "std")] +#[allow(unused_imports)] +pub use std_mod::*; -/// Generic trait for a device capable of switching itself on or off. -pub trait PowerSwitch { - type Error; - - fn switch_on(&mut self) -> Result<(), Self::Error>; - fn switch_off(&mut self) -> Result<(), Self::Error>; - - fn is_switch_on(&self) -> bool { - self.switch_state() == SwitchState::On - } - - fn switch_state(&self) -> SwitchState; -} +use crate::request::MessageMetadata; #[derive(Debug, Eq, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum SwitchState { Off = 0, On = 1, @@ -26,6 +19,7 @@ pub enum SwitchState { #[derive(Debug, Eq, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum SwitchStateBinary { Off = 0, On = 1, @@ -66,18 +60,26 @@ pub type SwitchId = u16; pub trait PowerSwitcherCommandSender { type Error; - fn send_switch_on_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error>; - fn send_switch_off_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error>; + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error>; + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error>; } pub trait PowerSwitchInfo { type Error; /// Retrieve the switch state - fn get_switch_state(&mut self, switch_id: SwitchId) -> Result; + fn switch_state(&self, switch_id: SwitchId) -> Result; - fn get_is_switch_on(&mut self, switch_id: SwitchId) -> Result { - Ok(self.get_switch_state(switch_id)? == SwitchState::On) + fn is_switch_on(&self, switch_id: SwitchId) -> Result { + Ok(self.switch_state(switch_id)? == SwitchState::On) } /// The maximum delay it will take to change a switch. @@ -87,52 +89,222 @@ pub trait PowerSwitchInfo { fn switch_delay_ms(&self) -> u32; } -#[cfg(test)] -mod tests { - #![allow(dead_code)] +#[derive(new)] +pub struct SwitchRequest { + switch_id: SwitchId, + target_state: SwitchStateBinary, +} + +impl SwitchRequest { + pub fn switch_id(&self) -> SwitchId { + self.switch_id + } + + pub fn target_state(&self) -> SwitchStateBinary { + self.target_state + } +} + +#[cfg(feature = "std")] +pub mod std_mod { + use std::sync::mpsc; + + use crate::{ + queue::GenericSendError, + request::{GenericMessage, MessageMetadata}, + }; + use super::*; - use std::boxed::Box; - struct Pcdu { - switch_rx: std::sync::mpsc::Receiver<(SwitchId, u16)>, + pub type MpscSwitchCmdSender = mpsc::Sender>; + pub type MpscSwitchCmdSenderBounded = mpsc::SyncSender>; + + impl PowerSwitcherCommandSender for MpscSwitchCmdSender { + type Error = GenericSendError; + + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::On), + )) + .map_err(|_| GenericSendError::RxDisconnected) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::Off), + )) + .map_err(|_| GenericSendError::RxDisconnected) + } } - #[derive(Eq, PartialEq)] - enum DeviceState { - OFF, - SwitchingPower, - ON, - SETUP, - IDLE, - } - struct MyComplexDevice { - power_switcher: Box>, - power_info: Box>, - switch_id: SwitchId, - some_state: u16, - dev_state: DeviceState, - mode: u32, - submode: u16, - } + impl PowerSwitcherCommandSender for MpscSwitchCmdSenderBounded { + type Error = GenericSendError; - impl MyComplexDevice { - pub fn periodic_op(&mut self) { - // .. mode command coming in - let mode = 1; - if mode == 1 { - if self.dev_state == DeviceState::OFF { - self.power_switcher - .send_switch_on_cmd(self.switch_id) - .expect("sending siwthc cmd failed"); - self.dev_state = DeviceState::SwitchingPower; - } - if self.dev_state == DeviceState::SwitchingPower { - if self.power_info.get_is_switch_on(0).unwrap() { - self.dev_state = DeviceState::ON; - self.mode = 1; - } - } - } + fn send_switch_on_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.try_send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::On), + )) + .map_err(|e| match e { + mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), + mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected, + }) + } + + fn send_switch_off_cmd( + &self, + requestor_info: MessageMetadata, + switch_id: SwitchId, + ) -> Result<(), Self::Error> { + self.try_send(GenericMessage::new( + requestor_info, + SwitchRequest::new(switch_id, SwitchStateBinary::Off), + )) + .map_err(|e| match e { + mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None), + mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected, + }) } } } + +#[cfg(test)] +mod tests { + // TODO: Add unittests for PowerSwitcherCommandSender impls for mpsc. + + use std::sync::mpsc::{self, TryRecvError}; + + use crate::{queue::GenericSendError, request::GenericMessage, ComponentId}; + + use super::*; + + const TEST_REQ_ID: u32 = 2; + const TEST_SENDER_ID: ComponentId = 5; + + const TEST_SWITCH_ID: u16 = 0x1ff; + + fn common_checks(request: &GenericMessage) { + assert_eq!(request.requestor_info.sender_id(), TEST_SENDER_ID); + assert_eq!(request.requestor_info.request_id(), TEST_REQ_ID); + assert_eq!(request.message.switch_id(), TEST_SWITCH_ID); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_on_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + switch_cmd_tx + .send_switch_on_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::On); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_off_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + switch_cmd_tx + .send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::Off); + } + + #[test] + fn test_comand_switch_sending_mpsc_regular_rx_disconnected() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::channel::>(); + drop(switch_cmd_rx); + let result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::RxDisconnected); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_on_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(3); + switch_cmd_tx + .send_switch_on_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::On); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_off_cmd() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(3); + switch_cmd_tx + .send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ) + .expect("sending switch cmd failed"); + let request = switch_cmd_rx + .recv() + .expect("receiving switch request failed"); + common_checks(&request); + assert_eq!(request.message.target_state(), SwitchStateBinary::Off); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_rx_disconnected() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(1); + drop(switch_cmd_rx); + let result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::RxDisconnected); + } + + #[test] + fn test_comand_switch_sending_mpsc_sync_queue_full() { + let (switch_cmd_tx, switch_cmd_rx) = mpsc::sync_channel::>(1); + let mut result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_ok()); + result = switch_cmd_tx.send_switch_off_cmd( + MessageMetadata::new(TEST_REQ_ID, TEST_SENDER_ID), + TEST_SWITCH_ID, + ); + assert!(result.is_err()); + matches!(result.unwrap_err(), GenericSendError::QueueFull(None)); + matches!(switch_cmd_rx.try_recv(), Err(TryRecvError::Empty)); + } +}