655 lines
24 KiB
Rust
655 lines
24 KiB
Rust
use derive_new::new;
|
|
use satrs::hk::{HkRequest, HkRequestVariant};
|
|
use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender};
|
|
use satrs::queue::{GenericSendError, GenericTargetedMessagingError};
|
|
use satrs_example::{DeviceMode, TimestampHelper};
|
|
use satrs_minisim::acs::lis3mdl::{
|
|
MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR,
|
|
};
|
|
use satrs_minisim::acs::MgmRequestLis3Mdl;
|
|
use satrs_minisim::eps::PcduSwitch;
|
|
use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest};
|
|
use std::fmt::Debug;
|
|
use std::sync::mpsc::{self};
|
|
use std::sync::{Arc, Mutex};
|
|
use std::time::Duration;
|
|
|
|
use satrs::mode::{
|
|
ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler,
|
|
};
|
|
use satrs::pus::{EcssTmSender, PusTmVariant};
|
|
use satrs::request::{GenericMessage, MessageMetadata, UniqueApidTargetId};
|
|
use satrs_example::config::components::{NO_SENDER, PUS_MODE_SERVICE};
|
|
|
|
use crate::hk::PusHkHelper;
|
|
use crate::pus::hk::{HkReply, HkReplyVariant};
|
|
use crate::requests::CompositeRequest;
|
|
|
|
use serde::{Deserialize, Serialize};
|
|
|
|
pub const NR_OF_DATA_AND_CFG_REGISTERS: usize = 14;
|
|
|
|
// Register adresses to access various bytes from the raw reply.
|
|
pub const X_LOWBYTE_IDX: usize = 9;
|
|
pub const Y_LOWBYTE_IDX: usize = 11;
|
|
pub const Z_LOWBYTE_IDX: usize = 13;
|
|
|
|
#[derive(Debug, Copy, Clone, Serialize, Deserialize)]
|
|
#[repr(u32)]
|
|
pub enum SetId {
|
|
SensorData = 0,
|
|
}
|
|
|
|
#[derive(Default, Debug, PartialEq, Eq)]
|
|
pub enum TransitionState {
|
|
#[default]
|
|
Idle,
|
|
PowerSwitching,
|
|
Done,
|
|
}
|
|
|
|
pub trait SpiInterface {
|
|
type Error: Debug;
|
|
fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>;
|
|
}
|
|
|
|
#[derive(Default)]
|
|
pub struct SpiDummyInterface {
|
|
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_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(())
|
|
}
|
|
}
|
|
|
|
pub struct SpiSimInterface {
|
|
pub sim_request_tx: mpsc::Sender<SimRequest>,
|
|
pub sim_reply_rx: mpsc::Receiver<SimReply>,
|
|
}
|
|
|
|
impl SpiInterface for SpiSimInterface {
|
|
type Error = ();
|
|
|
|
// 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;
|
|
if let Err(e) = self
|
|
.sim_request_tx
|
|
.send(SimRequest::new_with_epoch_time(mgm_sensor_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");
|
|
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(())
|
|
}
|
|
}
|
|
|
|
pub enum SpiSimInterfaceWrapper {
|
|
Dummy(SpiDummyInterface),
|
|
Sim(SpiSimInterface),
|
|
}
|
|
|
|
impl SpiInterface for SpiSimInterfaceWrapper {
|
|
type Error = ();
|
|
|
|
fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
|
|
match self {
|
|
SpiSimInterfaceWrapper::Dummy(dummy) => dummy.transfer(tx, rx),
|
|
SpiSimInterfaceWrapper::Sim(sim_if) => sim_if.transfer(tx, rx),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Default, Debug, Copy, Clone, Serialize, Deserialize)]
|
|
pub struct MgmData {
|
|
pub valid: bool,
|
|
pub x: f32,
|
|
pub y: f32,
|
|
pub z: f32,
|
|
}
|
|
|
|
pub struct MpscModeLeafInterface {
|
|
pub request_rx: mpsc::Receiver<GenericMessage<ModeRequest>>,
|
|
pub reply_to_pus_tx: mpsc::Sender<GenericMessage<ModeReply>>,
|
|
pub reply_to_parent_tx: mpsc::SyncSender<GenericMessage<ModeReply>>,
|
|
}
|
|
|
|
#[derive(Default)]
|
|
pub struct BufWrapper {
|
|
tx_buf: [u8; 32],
|
|
rx_buf: [u8; 32],
|
|
tm_buf: [u8; 32],
|
|
}
|
|
|
|
pub struct ModeHelpers {
|
|
current: ModeAndSubmode,
|
|
target: Option<ModeAndSubmode>,
|
|
requestor_info: Option<MessageMetadata>,
|
|
transition_state: TransitionState,
|
|
}
|
|
|
|
impl Default for ModeHelpers {
|
|
fn default() -> Self {
|
|
Self {
|
|
current: ModeAndSubmode::new(DeviceMode::Off as u32, 0),
|
|
target: Default::default(),
|
|
requestor_info: Default::default(),
|
|
transition_state: Default::default(),
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Example MGM device handler strongly based on the LIS3MDL MEMS device.
|
|
#[derive(new)]
|
|
#[allow(clippy::too_many_arguments)]
|
|
pub struct MgmHandlerLis3Mdl<
|
|
ComInterface: SpiInterface,
|
|
TmSender: EcssTmSender,
|
|
SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
|
|
> {
|
|
id: UniqueApidTargetId,
|
|
dev_str: &'static str,
|
|
mode_interface: MpscModeLeafInterface,
|
|
composite_request_rx: mpsc::Receiver<GenericMessage<CompositeRequest>>,
|
|
hk_reply_tx: mpsc::Sender<GenericMessage<HkReply>>,
|
|
switch_helper: SwitchHelper,
|
|
tm_sender: TmSender,
|
|
pub com_interface: ComInterface,
|
|
shared_mgm_set: Arc<Mutex<MgmData>>,
|
|
#[new(value = "PusHkHelper::new(id)")]
|
|
hk_helper: PusHkHelper,
|
|
#[new(default)]
|
|
mode_helpers: ModeHelpers,
|
|
#[new(default)]
|
|
bufs: BufWrapper,
|
|
#[new(default)]
|
|
stamp_helper: TimestampHelper,
|
|
}
|
|
|
|
impl<
|
|
ComInterface: SpiInterface,
|
|
TmSender: EcssTmSender,
|
|
SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
|
|
> MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
|
|
{
|
|
pub fn periodic_operation(&mut self) {
|
|
self.stamp_helper.update_from_now();
|
|
// Handle requests.
|
|
self.handle_composite_requests();
|
|
self.handle_mode_requests();
|
|
if let Some(target_mode_submode) = self.mode_helpers.target {
|
|
self.handle_mode_transition(target_mode_submode);
|
|
}
|
|
if self.mode() == DeviceMode::Normal as u32 {
|
|
log::trace!("polling LIS3MDL sensor {}", self.dev_str);
|
|
self.poll_sensor();
|
|
}
|
|
}
|
|
|
|
pub fn handle_composite_requests(&mut self) {
|
|
loop {
|
|
match self.composite_request_rx.try_recv() {
|
|
Ok(ref msg) => match &msg.message {
|
|
CompositeRequest::Hk(hk_request) => {
|
|
self.handle_hk_request(&msg.requestor_info, hk_request)
|
|
}
|
|
// TODO: This object does not have actions (yet).. Still send back completion failure
|
|
// reply.
|
|
CompositeRequest::Action(_action_req) => {}
|
|
},
|
|
|
|
Err(e) => {
|
|
if e != mpsc::TryRecvError::Empty {
|
|
log::warn!(
|
|
"{}: failed to receive composite request: {:?}",
|
|
self.dev_str,
|
|
e
|
|
);
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) {
|
|
match hk_request.variant {
|
|
HkRequestVariant::OneShot => {
|
|
let mgm_snapshot = *self.shared_mgm_set.lock().unwrap();
|
|
if let Ok(hk_tm) = self.hk_helper.generate_hk_report_packet(
|
|
self.stamp_helper.stamp(),
|
|
SetId::SensorData as u32,
|
|
&mut |hk_buf| {
|
|
hk_buf[0] = mgm_snapshot.valid as u8;
|
|
hk_buf[1..5].copy_from_slice(&mgm_snapshot.x.to_be_bytes());
|
|
hk_buf[5..9].copy_from_slice(&mgm_snapshot.y.to_be_bytes());
|
|
hk_buf[9..13].copy_from_slice(&mgm_snapshot.z.to_be_bytes());
|
|
Ok(13)
|
|
},
|
|
&mut self.bufs.tm_buf,
|
|
) {
|
|
// TODO: If sending the TM fails, we should also send a failure reply.
|
|
self.tm_sender
|
|
.send_tm(self.id.id(), PusTmVariant::Direct(hk_tm))
|
|
.expect("failed to send HK TM");
|
|
self.hk_reply_tx
|
|
.send(GenericMessage::new(
|
|
*requestor_info,
|
|
HkReply::new(hk_request.unique_id, HkReplyVariant::Ack),
|
|
))
|
|
.expect("failed to send HK reply");
|
|
} else {
|
|
// TODO: Send back failure reply. Need result code for this.
|
|
log::error!("TM buffer too small to generate HK data");
|
|
}
|
|
}
|
|
HkRequestVariant::EnablePeriodic => todo!(),
|
|
HkRequestVariant::DisablePeriodic => todo!(),
|
|
HkRequestVariant::ModifyCollectionInterval(_) => todo!(),
|
|
}
|
|
}
|
|
|
|
pub fn handle_mode_requests(&mut self) {
|
|
loop {
|
|
// TODO: Only allow one set mode request per cycle?
|
|
match self.mode_interface.request_rx.try_recv() {
|
|
Ok(msg) => {
|
|
let result = self.handle_mode_request(msg);
|
|
// TODO: Trigger event?
|
|
if result.is_err() {
|
|
log::warn!(
|
|
"{}: mode request failed with error {:?}",
|
|
self.dev_str,
|
|
result.err().unwrap()
|
|
);
|
|
}
|
|
}
|
|
Err(e) => {
|
|
if e != mpsc::TryRecvError::Empty {
|
|
log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e);
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn poll_sensor(&mut self) {
|
|
// Communicate with the device. This is actually how to read the data from the LIS3 device
|
|
// SPI interface.
|
|
self.com_interface
|
|
.transfer(
|
|
&self.bufs.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
|
|
&mut self.bufs.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
|
|
)
|
|
.expect("failed to transfer data");
|
|
let x_raw = i16::from_le_bytes(
|
|
self.bufs.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
|
|
.try_into()
|
|
.unwrap(),
|
|
);
|
|
let y_raw = i16::from_le_bytes(
|
|
self.bufs.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
|
|
.try_into()
|
|
.unwrap(),
|
|
);
|
|
let z_raw = i16::from_le_bytes(
|
|
self.bufs.rx_buf[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
|
|
.try_into()
|
|
.unwrap(),
|
|
);
|
|
// Simple scaling to retrieve the float value, assuming the best sensor resolution.
|
|
let mut mgm_guard = self.shared_mgm_set.lock().unwrap();
|
|
mgm_guard.x = x_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
|
|
mgm_guard.y = y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
|
|
mgm_guard.z = z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
|
|
mgm_guard.valid = true;
|
|
drop(mgm_guard);
|
|
}
|
|
|
|
pub fn handle_mode_transition(&mut self, target_mode_submode: ModeAndSubmode) {
|
|
if target_mode_submode.mode() == DeviceMode::On as u32
|
|
|| target_mode_submode.mode() == DeviceMode::Normal as u32
|
|
{
|
|
if self.mode_helpers.transition_state == TransitionState::Idle {
|
|
let result = self
|
|
.switch_helper
|
|
.send_switch_on_cmd(MessageMetadata::new(0, self.id.id()), PcduSwitch::Mgm);
|
|
if result.is_err() {
|
|
// Could not send switch command.. still continue with transition.
|
|
log::error!("failed to send switch on command");
|
|
}
|
|
self.mode_helpers.transition_state = TransitionState::PowerSwitching;
|
|
}
|
|
if self.mode_helpers.transition_state == TransitionState::PowerSwitching
|
|
&& self
|
|
.switch_helper
|
|
.is_switch_on(PcduSwitch::Mgm)
|
|
.expect("switch info error")
|
|
{
|
|
self.mode_helpers.transition_state = TransitionState::Done;
|
|
}
|
|
if self.mode_helpers.transition_state == TransitionState::Done {
|
|
self.mode_helpers.current = self.mode_helpers.target.unwrap();
|
|
self.handle_mode_reached(self.mode_helpers.requestor_info)
|
|
.expect("failed to handle mode reached");
|
|
self.mode_helpers.transition_state = TransitionState::Idle;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<
|
|
ComInterface: SpiInterface,
|
|
TmSender: EcssTmSender,
|
|
SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
|
|
> ModeProvider for MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
|
|
{
|
|
fn mode_and_submode(&self) -> ModeAndSubmode {
|
|
self.mode_helpers.current
|
|
}
|
|
}
|
|
|
|
impl<
|
|
ComInterface: SpiInterface,
|
|
TmSender: EcssTmSender,
|
|
SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
|
|
> ModeRequestHandler for MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
|
|
{
|
|
type Error = ModeError;
|
|
|
|
fn start_transition(
|
|
&mut self,
|
|
requestor: MessageMetadata,
|
|
mode_and_submode: ModeAndSubmode,
|
|
) -> Result<(), satrs::mode::ModeError> {
|
|
log::info!(
|
|
"{}: transitioning to mode {:?}",
|
|
self.dev_str,
|
|
mode_and_submode
|
|
);
|
|
self.mode_helpers.current = mode_and_submode;
|
|
if mode_and_submode.mode() == DeviceMode::Off as u32 {
|
|
self.shared_mgm_set.lock().unwrap().valid = false;
|
|
self.handle_mode_reached(Some(requestor))?;
|
|
} else if mode_and_submode.mode() == DeviceMode::Normal as u32
|
|
|| mode_and_submode.mode() == DeviceMode::On as u32
|
|
{
|
|
// TODO: Write helper method for the struct? Might help for other handlers as well..
|
|
self.mode_helpers.transition_state = TransitionState::Idle;
|
|
self.mode_helpers.requestor_info = Some(requestor);
|
|
self.mode_helpers.target = Some(mode_and_submode);
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn announce_mode(&self, _requestor_info: Option<MessageMetadata>, _recursive: bool) {
|
|
log::info!(
|
|
"{} announcing mode: {:?}",
|
|
self.dev_str,
|
|
self.mode_and_submode()
|
|
);
|
|
}
|
|
|
|
fn handle_mode_reached(
|
|
&mut self,
|
|
requestor: Option<MessageMetadata>,
|
|
) -> Result<(), Self::Error> {
|
|
self.mode_helpers.target = None;
|
|
self.announce_mode(requestor, false);
|
|
if let Some(requestor) = requestor {
|
|
if requestor.sender_id() == NO_SENDER {
|
|
return Ok(());
|
|
}
|
|
if requestor.sender_id() != PUS_MODE_SERVICE.id() {
|
|
log::warn!(
|
|
"can not send back mode reply to sender {:x}",
|
|
requestor.sender_id()
|
|
);
|
|
} else {
|
|
self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode()))?;
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn send_mode_reply(
|
|
&self,
|
|
requestor: MessageMetadata,
|
|
reply: ModeReply,
|
|
) -> Result<(), Self::Error> {
|
|
if requestor.sender_id() != PUS_MODE_SERVICE.id() {
|
|
log::warn!(
|
|
"can not send back mode reply to sender {}",
|
|
requestor.sender_id()
|
|
);
|
|
}
|
|
self.mode_interface
|
|
.reply_to_pus_tx
|
|
.send(GenericMessage::new(requestor, reply))
|
|
.map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
|
|
Ok(())
|
|
}
|
|
|
|
fn handle_mode_info(
|
|
&mut self,
|
|
_requestor_info: MessageMetadata,
|
|
_info: ModeAndSubmode,
|
|
) -> Result<(), Self::Error> {
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use std::sync::{mpsc, Arc};
|
|
|
|
use satrs::{
|
|
mode::{ModeReply, ModeRequest},
|
|
power::SwitchStateBinary,
|
|
request::{GenericMessage, UniqueApidTargetId},
|
|
tmtc::PacketAsVec,
|
|
};
|
|
use satrs_example::config::components::Apid;
|
|
use satrs_minisim::acs::lis3mdl::MgmLis3RawValues;
|
|
|
|
use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest};
|
|
|
|
use super::*;
|
|
|
|
#[derive(Default)]
|
|
pub struct TestSpiInterface {
|
|
pub call_count: u32,
|
|
pub next_mgm_data: MgmLis3RawValues,
|
|
}
|
|
|
|
impl SpiInterface for TestSpiInterface {
|
|
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.next_mgm_data.x.to_le_bytes());
|
|
rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
|
|
.copy_from_slice(&self.next_mgm_data.y.to_le_bytes());
|
|
rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
|
|
.copy_from_slice(&self.next_mgm_data.z.to_le_bytes());
|
|
self.call_count += 1;
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
pub struct MgmTestbench {
|
|
pub mode_request_tx: mpsc::Sender<GenericMessage<ModeRequest>>,
|
|
pub mode_reply_rx_to_pus: mpsc::Receiver<GenericMessage<ModeReply>>,
|
|
pub mode_reply_rx_to_parent: mpsc::Receiver<GenericMessage<ModeReply>>,
|
|
pub composite_request_tx: mpsc::Sender<GenericMessage<CompositeRequest>>,
|
|
pub hk_reply_rx: mpsc::Receiver<GenericMessage<HkReply>>,
|
|
pub tm_rx: mpsc::Receiver<PacketAsVec>,
|
|
pub handler:
|
|
MgmHandlerLis3Mdl<TestSpiInterface, mpsc::Sender<PacketAsVec>, TestSwitchHelper>,
|
|
}
|
|
|
|
impl MgmTestbench {
|
|
pub fn new() -> Self {
|
|
let (request_tx, request_rx) = mpsc::channel();
|
|
let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel();
|
|
let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5);
|
|
let mode_interface = MpscModeLeafInterface {
|
|
request_rx,
|
|
reply_to_pus_tx: reply_tx_to_pus,
|
|
reply_to_parent_tx: reply_tx_to_parent,
|
|
};
|
|
let (composite_request_tx, composite_request_rx) = mpsc::channel();
|
|
let (hk_reply_tx, hk_reply_rx) = mpsc::channel();
|
|
let (tm_tx, tm_rx) = mpsc::channel::<PacketAsVec>();
|
|
let shared_mgm_set = Arc::default();
|
|
Self {
|
|
mode_request_tx: request_tx,
|
|
mode_reply_rx_to_pus: reply_rx_to_pus,
|
|
mode_reply_rx_to_parent: reply_rx_to_parent,
|
|
composite_request_tx,
|
|
tm_rx,
|
|
hk_reply_rx,
|
|
handler: MgmHandlerLis3Mdl::new(
|
|
UniqueApidTargetId::new(Apid::Acs as u16, 1),
|
|
"TEST_MGM",
|
|
mode_interface,
|
|
composite_request_rx,
|
|
hk_reply_tx,
|
|
TestSwitchHelper::default(),
|
|
tm_tx,
|
|
TestSpiInterface::default(),
|
|
shared_mgm_set,
|
|
),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_basic_handler() {
|
|
let mut testbench = MgmTestbench::new();
|
|
assert_eq!(testbench.handler.com_interface.call_count, 0);
|
|
assert_eq!(
|
|
testbench.handler.mode_and_submode().mode(),
|
|
DeviceMode::Off as u32
|
|
);
|
|
assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16);
|
|
testbench.handler.periodic_operation();
|
|
// Handler is OFF, no changes expected.
|
|
assert_eq!(testbench.handler.com_interface.call_count, 0);
|
|
assert_eq!(
|
|
testbench.handler.mode_and_submode().mode(),
|
|
DeviceMode::Off as u32
|
|
);
|
|
assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16);
|
|
}
|
|
|
|
#[test]
|
|
fn test_normal_handler() {
|
|
let mut testbench = MgmTestbench::new();
|
|
testbench
|
|
.mode_request_tx
|
|
.send(GenericMessage::new(
|
|
MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
|
|
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
|
|
))
|
|
.expect("failed to send mode request");
|
|
testbench.handler.periodic_operation();
|
|
assert_eq!(
|
|
testbench.handler.mode_and_submode().mode(),
|
|
DeviceMode::Normal as u32
|
|
);
|
|
assert_eq!(testbench.handler.mode_and_submode().submode(), 0);
|
|
|
|
// Verify power switch handling.
|
|
let mut switch_requests = testbench.handler.switch_helper.switch_requests.borrow_mut();
|
|
assert_eq!(switch_requests.len(), 1);
|
|
let switch_req = switch_requests.pop_front().expect("no switch request");
|
|
assert_eq!(switch_req.target_state, SwitchStateBinary::On);
|
|
assert_eq!(switch_req.switch_id, PcduSwitch::Mgm);
|
|
let mut switch_info_requests = testbench
|
|
.handler
|
|
.switch_helper
|
|
.switch_info_requests
|
|
.borrow_mut();
|
|
assert_eq!(switch_info_requests.len(), 1);
|
|
let switch_info_req = switch_info_requests.pop_front().expect("no switch request");
|
|
assert_eq!(switch_info_req, PcduSwitch::Mgm);
|
|
|
|
let mode_reply = testbench
|
|
.mode_reply_rx_to_pus
|
|
.try_recv()
|
|
.expect("no mode reply generated");
|
|
match mode_reply.message {
|
|
ModeReply::ModeReply(mode) => {
|
|
assert_eq!(mode.mode(), DeviceMode::Normal as u32);
|
|
assert_eq!(mode.submode(), 0);
|
|
}
|
|
_ => panic!("unexpected mode reply"),
|
|
}
|
|
// The device should have been polled once.
|
|
assert_eq!(testbench.handler.com_interface.call_count, 1);
|
|
let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap();
|
|
assert!(mgm_set.x < 0.001);
|
|
assert!(mgm_set.y < 0.001);
|
|
assert!(mgm_set.z < 0.001);
|
|
assert!(mgm_set.valid);
|
|
}
|
|
|
|
#[test]
|
|
fn test_normal_handler_mgm_set_conversion() {
|
|
let mut testbench = MgmTestbench::new();
|
|
let raw_values = MgmLis3RawValues {
|
|
x: 1000,
|
|
y: -1000,
|
|
z: 1000,
|
|
};
|
|
testbench.handler.com_interface.next_mgm_data = raw_values;
|
|
testbench
|
|
.mode_request_tx
|
|
.send(GenericMessage::new(
|
|
MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
|
|
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
|
|
))
|
|
.expect("failed to send mode request");
|
|
testbench.handler.periodic_operation();
|
|
let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap();
|
|
let expected_x =
|
|
raw_values.x as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
|
|
let expected_y =
|
|
raw_values.y as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
|
|
let expected_z =
|
|
raw_values.z as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
|
|
let x_diff = (mgm_set.x - expected_x).abs();
|
|
let y_diff = (mgm_set.y - expected_y).abs();
|
|
let z_diff = (mgm_set.z - expected_z).abs();
|
|
assert!(x_diff < 0.001, "x diff too large: {}", x_diff);
|
|
assert!(y_diff < 0.001, "y diff too large: {}", y_diff);
|
|
assert!(z_diff < 0.001, "z diff too large: {}", z_diff);
|
|
assert!(mgm_set.valid);
|
|
}
|
|
}
|