rust/sat-rs
rust/sat-rs
5
1
Fork 0
Code Issues 11 Pull Requests 1 Projects Releases 20 Wiki Activity

add first MGM device unittests

Browse Source
This commit is contained in:
Robin Müller 2024-05-09 21:38:56 +02:00
parent 6e5b70af34
commit a4888bce01
Signed by: muellerr
GPG Key ID: A649FB78196E3849
3 changed files with 227 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

266
satrs-example/src/acs/mgm.rs
View File

@ -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.
}
}

8
satrs-example/src/main.rs
View File

@ -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() {

2
satrs-minisim/src/lib.rs
View File

@ -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,