add first MGM device unittests
This commit is contained in:
parent
6e5b70af34
commit
a4888bce01
@ -5,11 +5,14 @@ use satrs::spacepackets::ecss::hk;
|
||||
use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader};
|
||||
use satrs::spacepackets::SpHeader;
|
||||
use satrs_example::{DeviceMode, TimestampHelper};
|
||||
use satrs_minisim::acs::lis3mdl::{FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR};
|
||||
use satrs_minisim::acs::lis3mdl::{
|
||||
MgmLis3MdlReply, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR,
|
||||
};
|
||||
use satrs_minisim::acs::MgmRequestLis3Mdl;
|
||||
use satrs_minisim::{SimReply, SimRequest};
|
||||
use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest};
|
||||
use std::sync::mpsc::{self};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::time::Duration;
|
||||
|
||||
use satrs::mode::{
|
||||
ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler,
|
||||
@ -62,13 +65,20 @@ 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> {
|
||||
fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
|
||||
let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData;
|
||||
self.sim_request_tx
|
||||
.send(SimRequest::new_with_epoch_time(mgm_sensor_request))
|
||||
.expect("failed to send request");
|
||||
self.sim_reply_rx.recv().expect("reply timeout");
|
||||
// TODO: Write the sensor data to the raw buffer.
|
||||
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_be_bytes());
|
||||
rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_be_bytes());
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
@ -99,8 +109,8 @@ pub struct MgmData {
|
||||
|
||||
pub struct MpscModeLeafInterface {
|
||||
pub request_rx: mpsc::Receiver<GenericMessage<ModeRequest>>,
|
||||
pub reply_tx_to_pus: mpsc::Sender<GenericMessage<ModeReply>>,
|
||||
pub reply_tx_to_parent: mpsc::Sender<GenericMessage<ModeReply>>,
|
||||
pub reply_to_pus_tx: mpsc::Sender<GenericMessage<ModeReply>>,
|
||||
pub reply_to_parent_tx: mpsc::Sender<GenericMessage<ModeReply>>,
|
||||
}
|
||||
|
||||
/// Example MGM device handler strongly based on the LIS3MDL MEMS device.
|
||||
@ -110,8 +120,8 @@ pub struct MgmHandlerLis3Mdl<ComInterface: SpiInterface, TmSender: EcssTmSender>
|
||||
id: UniqueApidTargetId,
|
||||
dev_str: &'static str,
|
||||
mode_interface: MpscModeLeafInterface,
|
||||
composite_request_receiver: mpsc::Receiver<GenericMessage<CompositeRequest>>,
|
||||
hk_reply_sender: mpsc::Sender<GenericMessage<HkReply>>,
|
||||
composite_request_rx: mpsc::Receiver<GenericMessage<CompositeRequest>>,
|
||||
hk_reply_tx: mpsc::Sender<GenericMessage<HkReply>>,
|
||||
tm_sender: TmSender,
|
||||
com_interface: ComInterface,
|
||||
shared_mgm_set: Arc<Mutex<MgmData>>,
|
||||
@ -135,43 +145,13 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> MgmHandlerLis3Mdl<ComIn
|
||||
self.handle_mode_requests();
|
||||
if self.mode() == DeviceMode::Normal as u32 {
|
||||
log::trace!("polling LIS3MDL sensor {}", self.dev_str);
|
||||
// Communicate with the device.
|
||||
let result = self.com_interface.transfer(
|
||||
&self.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
|
||||
&mut self.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
|
||||
);
|
||||
assert!(result.is_ok());
|
||||
// Actual data begins on the second byte, similarly to how a lot of SPI devices behave.
|
||||
let x_raw = i16::from_le_bytes(
|
||||
self.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
|
||||
.try_into()
|
||||
.unwrap(),
|
||||
);
|
||||
let y_raw = i16::from_le_bytes(
|
||||
self.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
|
||||
.try_into()
|
||||
.unwrap(),
|
||||
);
|
||||
let z_raw = i16::from_le_bytes(
|
||||
self.rx_buf[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
|
||||
.try_into()
|
||||
.unwrap(),
|
||||
);
|
||||
// Simple scaling to retrieve the float value, assuming a sensor resolution of
|
||||
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;
|
||||
drop(mgm_guard);
|
||||
self.poll_sensor();
|
||||
}
|
||||
}
|
||||
|
||||
pub fn handle_composite_requests(&mut self) {
|
||||
loop {
|
||||
match self.composite_request_receiver.try_recv() {
|
||||
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)
|
||||
@ -199,7 +179,7 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> MgmHandlerLis3Mdl<ComIn
|
||||
pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) {
|
||||
match hk_request.variant {
|
||||
HkRequestVariant::OneShot => {
|
||||
self.hk_reply_sender
|
||||
self.hk_reply_tx
|
||||
.send(GenericMessage::new(
|
||||
*requestor_info,
|
||||
HkReply::new(hk_request.unique_id, HkReplyVariant::Ack),
|
||||
@ -234,6 +214,38 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> MgmHandlerLis3Mdl<ComIn
|
||||
}
|
||||
}
|
||||
|
||||
pub fn poll_sensor(&mut self) {
|
||||
// Communicate with the device. This is actually how to read the data from the LIS3 device
|
||||
// SPI interface.
|
||||
let result = self.com_interface.transfer(
|
||||
&self.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
|
||||
&mut self.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
|
||||
);
|
||||
assert!(result.is_ok());
|
||||
let x_raw = i16::from_le_bytes(
|
||||
self.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
|
||||
.try_into()
|
||||
.unwrap(),
|
||||
);
|
||||
let y_raw = i16::from_le_bytes(
|
||||
self.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
|
||||
.try_into()
|
||||
.unwrap(),
|
||||
);
|
||||
let z_raw = i16::from_le_bytes(
|
||||
self.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_requests(&mut self) {
|
||||
loop {
|
||||
// TODO: Only allow one set mode request per cycle?
|
||||
@ -284,6 +296,9 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> ModeRequestHandler
|
||||
mode_and_submode
|
||||
);
|
||||
self.mode_and_submode = 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))?;
|
||||
Ok(())
|
||||
}
|
||||
@ -326,7 +341,7 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> ModeRequestHandler
|
||||
);
|
||||
}
|
||||
self.mode_interface
|
||||
.reply_tx_to_pus
|
||||
.reply_to_pus_tx
|
||||
.send(GenericMessage::new(requestor, reply))
|
||||
.map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
|
||||
Ok(())
|
||||
@ -343,5 +358,168 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> ModeRequestHandler
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
// TODO: Add some basic tests for the modes of the device.
|
||||
use std::sync::{atomic::AtomicU32, mpsc, Arc, Mutex};
|
||||
|
||||
use satrs::{
|
||||
mode::{ModeReply, ModeRequest},
|
||||
request::{GenericMessage, UniqueApidTargetId},
|
||||
tmtc::PacketAsVec,
|
||||
};
|
||||
use satrs_example::config::components::Apid;
|
||||
use satrs_minisim::acs::lis3mdl::MgmLis3RawValues;
|
||||
|
||||
use crate::{pus::hk::HkReply, requests::CompositeRequest};
|
||||
|
||||
use super::*;
|
||||
|
||||
pub struct TestInterface {
|
||||
pub call_count: Arc<AtomicU32>,
|
||||
pub next_mgm_data: Arc<Mutex<MgmLis3RawValues>>,
|
||||
}
|
||||
|
||||
impl TestInterface {
|
||||
pub fn new(
|
||||
call_count: Arc<AtomicU32>,
|
||||
next_mgm_data: Arc<Mutex<MgmLis3RawValues>>,
|
||||
) -> Self {
|
||||
Self {
|
||||
call_count,
|
||||
next_mgm_data,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SpiInterface for TestInterface {
|
||||
type Error = ();
|
||||
|
||||
fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
|
||||
let mgm_data = *self.next_mgm_data.lock().unwrap();
|
||||
rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.x.to_le_bytes());
|
||||
rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.y.to_be_bytes());
|
||||
rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.z.to_be_bytes());
|
||||
self.call_count
|
||||
.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
pub struct MgmTestbench {
|
||||
pub spi_interface_call_count: Arc<AtomicU32>,
|
||||
pub next_mgm_data: Arc<Mutex<MgmLis3RawValues>>,
|
||||
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<TestInterface, mpsc::Sender<PacketAsVec>>,
|
||||
}
|
||||
|
||||
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::channel();
|
||||
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();
|
||||
let next_mgm_data = Arc::new(Mutex::default());
|
||||
let spi_interface_call_count = Arc::new(AtomicU32::new(0));
|
||||
let test_interface =
|
||||
TestInterface::new(spi_interface_call_count.clone(), next_mgm_data.clone());
|
||||
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,
|
||||
spi_interface_call_count,
|
||||
next_mgm_data,
|
||||
handler: MgmHandlerLis3Mdl::new(
|
||||
UniqueApidTargetId::new(Apid::Acs as u16, 1),
|
||||
"test-mgm",
|
||||
mode_interface,
|
||||
composite_request_rx,
|
||||
hk_reply_tx,
|
||||
tm_tx,
|
||||
test_interface,
|
||||
shared_mgm_set,
|
||||
),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_basic_handler() {
|
||||
let mut testbench = MgmTestbench::new();
|
||||
assert_eq!(
|
||||
testbench
|
||||
.spi_interface_call_count
|
||||
.load(std::sync::atomic::Ordering::Relaxed),
|
||||
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
|
||||
.spi_interface_call_count
|
||||
.load(std::sync::atomic::Ordering::Relaxed),
|
||||
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);
|
||||
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
|
||||
.spi_interface_call_count
|
||||
.load(std::sync::atomic::Ordering::Relaxed),
|
||||
1
|
||||
);
|
||||
// TODO: Check shared MGM set. The field values should be 0, but the entry should be valid.
|
||||
// TODO: Set non-zero raw values for the next polled MGM set.
|
||||
}
|
||||
}
|
||||
|
@ -209,8 +209,8 @@ fn static_tmtc_pool_main() {
|
||||
let shared_mgm_set = Arc::default();
|
||||
let mode_leaf_interface = MpscModeLeafInterface {
|
||||
request_rx: mgm_handler_mode_rx,
|
||||
reply_tx_to_pus: pus_mode_reply_tx,
|
||||
reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx,
|
||||
reply_to_pus_tx: pus_mode_reply_tx,
|
||||
reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
|
||||
};
|
||||
|
||||
let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
|
||||
@ -448,8 +448,8 @@ fn dyn_tmtc_pool_main() {
|
||||
let shared_mgm_set = Arc::default();
|
||||
let mode_leaf_interface = MpscModeLeafInterface {
|
||||
request_rx: mgm_handler_mode_rx,
|
||||
reply_tx_to_pus: pus_mode_reply_tx,
|
||||
reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx,
|
||||
reply_to_pus_tx: pus_mode_reply_tx,
|
||||
reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
|
||||
};
|
||||
|
||||
let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
|
||||
|
@ -247,7 +247,7 @@ pub mod acs {
|
||||
pub const FIELD_LSB_PER_GAUSS_12_SENS: f32 = 1.0 / 2281.0;
|
||||
pub const FIELD_LSB_PER_GAUSS_16_SENS: f32 = 1.0 / 1711.0;
|
||||
|
||||
#[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)]
|
||||
#[derive(Default, Debug, Copy, Clone, PartialEq, Serialize, Deserialize)]
|
||||
pub struct MgmLis3RawValues {
|
||||
pub x: i16,
|
||||
pub y: i16,
|
||||
|
Loading…
x
Reference in New Issue
Block a user