serial protocol for MGT
This commit is contained in:
@@ -10,6 +10,9 @@ use serde::{Deserialize, Serialize};
|
||||
use std::time::Duration;
|
||||
use types::pcdu::SwitchStateBinary;
|
||||
|
||||
/// Time the device needs to answer a command.
|
||||
const REPLY_DELAY: Duration = Duration::from_millis(15);
|
||||
|
||||
/// Simple magnetorquer simulation model.
|
||||
#[derive(Serialize, Deserialize)]
|
||||
pub struct MgtModel {
|
||||
@@ -39,6 +42,9 @@ impl MgtModel {
|
||||
duration_and_dipole: (Duration, mgt::Dipole),
|
||||
cx: &Context<Self>,
|
||||
) {
|
||||
if self.switch_state != SwitchStateBinary::On {
|
||||
return;
|
||||
}
|
||||
self.torque_dipole = duration_and_dipole.1;
|
||||
self.torquing = true;
|
||||
if cx
|
||||
@@ -48,6 +54,7 @@ impl MgtModel {
|
||||
log::warn!("torque clearing can only be set for a future time.");
|
||||
}
|
||||
self.generate_magnetic_field(()).await;
|
||||
self.schedule_reply(mgt::Reply::Ack, cx);
|
||||
}
|
||||
|
||||
#[nexosim(schedulable)]
|
||||
@@ -69,22 +76,22 @@ impl MgtModel {
|
||||
if self.switch_state != SwitchStateBinary::On {
|
||||
return;
|
||||
}
|
||||
cx.schedule_event(
|
||||
Duration::from_millis(15),
|
||||
schedulable!(Self::send_housekeeping_data),
|
||||
(),
|
||||
)
|
||||
.expect("requesting housekeeping data failed")
|
||||
// The HK is sampled when the command is processed, not when the reply is sent.
|
||||
let hk = mgt::HkSet {
|
||||
dipole: self.torque_dipole,
|
||||
torquing: self.torquing,
|
||||
};
|
||||
self.schedule_reply(mgt::Reply::Hk(hk), cx);
|
||||
}
|
||||
|
||||
fn schedule_reply(&self, reply: mgt::Reply, cx: &Context<Self>) {
|
||||
cx.schedule_event(REPLY_DELAY, schedulable!(Self::send_reply), reply)
|
||||
.expect("scheduling MGT reply failed")
|
||||
}
|
||||
|
||||
#[nexosim(schedulable)]
|
||||
async fn send_housekeeping_data(&mut self) {
|
||||
self.reply
|
||||
.send(SimReply::from(mgt::Reply::Hk(mgt::HkSet {
|
||||
dipole: self.torque_dipole,
|
||||
torquing: self.torquing,
|
||||
})))
|
||||
.await;
|
||||
async fn send_reply(&mut self, reply: mgt::Reply) {
|
||||
self.reply.send(SimReply::from(reply)).await;
|
||||
}
|
||||
|
||||
fn calc_magnetic_field(&self, _: mgt::Dipole) -> mgm::SensorValuesMicroTesla {
|
||||
@@ -118,10 +125,17 @@ mod tests {
|
||||
|
||||
use crate::{eps::tests::switch_device_on, test_helpers::SimTestbench};
|
||||
|
||||
fn decode_reply(sim_reply: SimReply) -> mgt::Reply {
|
||||
let SimReply::Mgt(frame) = sim_reply else {
|
||||
panic!("unexpected reply {sim_reply:?}");
|
||||
};
|
||||
mgt::Reply::from_frame(&frame).expect("invalid MGT reply frame")
|
||||
}
|
||||
|
||||
fn request_hk(sim_testbench: &mut SimTestbench) -> Option<mgt::HkSet> {
|
||||
let sim_reply = sim_testbench.request_reply(mgt::Request::RequestHk)?;
|
||||
let SimReply::Mgt(mgt::Reply::Hk(hk)) = sim_reply else {
|
||||
panic!("unexpected reply {sim_reply:?}");
|
||||
let mgt::Reply::Hk(hk) = decode_reply(sim_reply) else {
|
||||
panic!("unexpected MGT reply");
|
||||
};
|
||||
Some(hk)
|
||||
}
|
||||
@@ -162,7 +176,11 @@ mod tests {
|
||||
.send_request(request)
|
||||
.expect("sending MGT request failed");
|
||||
sim_testbench.handle_sim_requests_time_agnostic();
|
||||
sim_testbench.step_until(Duration::from_millis(5)).unwrap();
|
||||
sim_testbench.step_until(Duration::from_millis(20)).unwrap();
|
||||
let ack = sim_testbench
|
||||
.try_receive_next_reply()
|
||||
.expect("no torque command ack");
|
||||
assert_eq!(decode_reply(ack), mgt::Reply::Ack);
|
||||
|
||||
assert_eq!(
|
||||
request_hk(&mut sim_testbench),
|
||||
@@ -183,6 +201,25 @@ mod tests {
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_torque_command_not_acked_when_off() {
|
||||
let mut sim_testbench = SimTestbench::new();
|
||||
let reply = sim_testbench.request_reply(mgt::Request::ApplyTorque {
|
||||
duration: Duration::from_millis(100),
|
||||
dipole: mgt::Dipole { x: 1, y: 2, z: 3 },
|
||||
});
|
||||
assert!(reply.is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_invalid_frame_is_dropped() {
|
||||
let mut sim_testbench = SimTestbench::new();
|
||||
switch_device_on(&mut sim_testbench, SwitchId::Mgt);
|
||||
assert!(sim_testbench
|
||||
.request_reply(SimRequest::Mgt(vec![0x01, 0x00]))
|
||||
.is_none());
|
||||
}
|
||||
|
||||
/// Processes the request without stepping, so scheduled events like the torque clearing do
|
||||
/// not fire.
|
||||
fn process_without_step(sim_testbench: &mut SimTestbench, request: impl Into<SimRequest>) {
|
||||
@@ -200,13 +237,15 @@ mod tests {
|
||||
request: mgm::Request::RequestSensorData,
|
||||
},
|
||||
);
|
||||
let sim_reply = sim_testbench
|
||||
.try_receive_next_reply()
|
||||
.expect("no MGM reply received");
|
||||
let SimReply::Mgm { reply, .. } = sim_reply else {
|
||||
panic!("unexpected reply {sim_reply:?}");
|
||||
};
|
||||
reply.sensor_values
|
||||
// Skips pending MGT replies, for example torque command acks.
|
||||
loop {
|
||||
let sim_reply = sim_testbench
|
||||
.try_receive_next_reply()
|
||||
.expect("no MGM reply received");
|
||||
if let SimReply::Mgm { reply, .. } = sim_reply {
|
||||
return reply.sensor_values;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn start_torquing(sim_testbench: &mut SimTestbench, duration: Duration) {
|
||||
|
||||
@@ -268,7 +268,14 @@ impl SimController {
|
||||
}
|
||||
}
|
||||
|
||||
fn handle_mgt_request(&mut self, mgt_request: mgt::Request) {
|
||||
fn handle_mgt_request(&mut self, frame: Vec<u8>) {
|
||||
let mgt_request = match mgt::Request::from_frame(&frame) {
|
||||
Ok(request) => request,
|
||||
Err(e) => {
|
||||
log::warn!("dropping invalid MGT frame {frame:02x?}: {e}");
|
||||
return;
|
||||
}
|
||||
};
|
||||
if MGT_REQ_WIRETAPPING {
|
||||
log::info!("received MGT request: {mgt_request:?}");
|
||||
}
|
||||
|
||||
@@ -19,8 +19,12 @@ pub enum ComponentId {
|
||||
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
|
||||
pub enum SimRequest {
|
||||
SimCtrl(SimCtrlRequest),
|
||||
Mgm { id: mgm::Id, request: mgm::Request },
|
||||
Mgt(mgt::Request),
|
||||
Mgm {
|
||||
id: mgm::Id,
|
||||
request: mgm::Request,
|
||||
},
|
||||
/// Raw frame of the MGT serial protocol.
|
||||
Mgt(Vec<u8>),
|
||||
Pcdu(PcduRequest),
|
||||
}
|
||||
|
||||
@@ -32,7 +36,7 @@ impl From<SimCtrlRequest> for SimRequest {
|
||||
|
||||
impl From<mgt::Request> for SimRequest {
|
||||
fn from(request: mgt::Request) -> Self {
|
||||
Self::Mgt(request)
|
||||
Self::Mgt(request.to_frame())
|
||||
}
|
||||
}
|
||||
|
||||
@@ -64,8 +68,12 @@ impl SimRequestWithTime {
|
||||
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
|
||||
pub enum SimReply {
|
||||
SimCtrl(SimCtrlReply),
|
||||
Mgm { id: mgm::Id, reply: mgm::Reply },
|
||||
Mgt(mgt::Reply),
|
||||
Mgm {
|
||||
id: mgm::Id,
|
||||
reply: mgm::Reply,
|
||||
},
|
||||
/// Raw frame of the MGT serial protocol.
|
||||
Mgt(Vec<u8>),
|
||||
Pcdu(PcduReply),
|
||||
}
|
||||
|
||||
@@ -88,7 +96,7 @@ impl From<SimCtrlReply> for SimReply {
|
||||
|
||||
impl From<mgt::Reply> for SimReply {
|
||||
fn from(reply: mgt::Reply) -> Self {
|
||||
Self::Mgt(reply)
|
||||
Self::Mgt(reply.to_frame())
|
||||
}
|
||||
}
|
||||
|
||||
@@ -263,11 +271,40 @@ pub mod acs {
|
||||
}
|
||||
}
|
||||
|
||||
/// Simple serial protocol of the magnetorquer.
|
||||
///
|
||||
/// The first byte of each frame is the packet ID. The high bit of the ID is set for replies.
|
||||
/// All fields are big endian. Every command is answered with exactly one reply, but only
|
||||
/// if the device is powered. The device drops invalid frames.
|
||||
///
|
||||
/// A data link layer is deliberately skipped for simplicity. A real serial link would need
|
||||
/// framing and error detection, for example COBS encoding and a CRC. Here, the transport
|
||||
/// always delivers complete and intact frames.
|
||||
pub mod mgt {
|
||||
use std::time::Duration;
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
pub mod packet_id {
|
||||
pub const REQUEST_HK: u8 = 0x01;
|
||||
/// Payload: dipole (3 x i16), duration in milliseconds (u32).
|
||||
pub const APPLY_TORQUE: u8 = 0x02;
|
||||
/// Payload: dipole (3 x i16), torquing flag (u8).
|
||||
pub const HK: u8 = 0x81;
|
||||
/// Reply to [APPLY_TORQUE].
|
||||
pub const ACK: u8 = 0x82;
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone, PartialEq, Eq, thiserror::Error)]
|
||||
pub enum FrameError {
|
||||
#[error("empty frame")]
|
||||
Empty,
|
||||
#[error("unknown packet ID {0:#04x}")]
|
||||
UnknownPacketId(u8),
|
||||
#[error("invalid length {len} for packet ID {id:#04x}")]
|
||||
InvalidLength { id: u8, len: usize },
|
||||
}
|
||||
|
||||
// Simple model using i16 values.
|
||||
#[derive(Default, Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub struct Dipole {
|
||||
@@ -276,12 +313,84 @@ pub mod acs {
|
||||
pub z: i16,
|
||||
}
|
||||
|
||||
impl Dipole {
|
||||
const LEN: usize = 6;
|
||||
|
||||
fn write_to(&self, frame: &mut Vec<u8>) {
|
||||
frame.extend_from_slice(&self.x.to_be_bytes());
|
||||
frame.extend_from_slice(&self.y.to_be_bytes());
|
||||
frame.extend_from_slice(&self.z.to_be_bytes());
|
||||
}
|
||||
|
||||
fn read_from(buf: &[u8]) -> Self {
|
||||
Self {
|
||||
x: i16::from_be_bytes([buf[0], buf[1]]),
|
||||
y: i16::from_be_bytes([buf[2], buf[3]]),
|
||||
z: i16::from_be_bytes([buf[4], buf[5]]),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Checks the frame length and returns the packet ID and the payload.
|
||||
fn split_frame(
|
||||
frame: &[u8],
|
||||
payload_len: impl Fn(u8) -> Option<usize>,
|
||||
) -> Result<(u8, &[u8]), FrameError> {
|
||||
let (&id, payload) = frame.split_first().ok_or(FrameError::Empty)?;
|
||||
let expected_len = payload_len(id).ok_or(FrameError::UnknownPacketId(id))?;
|
||||
if payload.len() != expected_len {
|
||||
return Err(FrameError::InvalidLength {
|
||||
id,
|
||||
len: frame.len(),
|
||||
});
|
||||
}
|
||||
Ok((id, payload))
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub enum Request {
|
||||
ApplyTorque { duration: Duration, dipole: Dipole },
|
||||
/// The duration has millisecond resolution on the wire.
|
||||
ApplyTorque {
|
||||
duration: Duration,
|
||||
dipole: Dipole,
|
||||
},
|
||||
RequestHk,
|
||||
}
|
||||
|
||||
impl Request {
|
||||
pub fn to_frame(&self) -> Vec<u8> {
|
||||
match self {
|
||||
Request::RequestHk => vec![packet_id::REQUEST_HK],
|
||||
Request::ApplyTorque { duration, dipole } => {
|
||||
let mut frame = vec![packet_id::APPLY_TORQUE];
|
||||
dipole.write_to(&mut frame);
|
||||
let duration_ms = u32::try_from(duration.as_millis()).unwrap_or(u32::MAX);
|
||||
frame.extend_from_slice(&duration_ms.to_be_bytes());
|
||||
frame
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_frame(frame: &[u8]) -> Result<Self, FrameError> {
|
||||
let (id, payload) = split_frame(frame, |id| match id {
|
||||
packet_id::REQUEST_HK => Some(0),
|
||||
packet_id::APPLY_TORQUE => Some(Dipole::LEN + 4),
|
||||
_ => None,
|
||||
})?;
|
||||
Ok(match id {
|
||||
packet_id::REQUEST_HK => Request::RequestHk,
|
||||
_ => {
|
||||
let duration_ms =
|
||||
u32::from_be_bytes(payload[Dipole::LEN..].try_into().unwrap());
|
||||
Request::ApplyTorque {
|
||||
duration: Duration::from_millis(duration_ms.into()),
|
||||
dipole: Dipole::read_from(payload),
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub struct HkSet {
|
||||
pub dipole: Dipole,
|
||||
@@ -291,6 +400,95 @@ pub mod acs {
|
||||
#[derive(Debug, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub enum Reply {
|
||||
Hk(HkSet),
|
||||
Ack,
|
||||
}
|
||||
|
||||
impl Reply {
|
||||
pub fn to_frame(&self) -> Vec<u8> {
|
||||
match self {
|
||||
Reply::Hk(hk) => {
|
||||
let mut frame = vec![packet_id::HK];
|
||||
hk.dipole.write_to(&mut frame);
|
||||
frame.push(hk.torquing as u8);
|
||||
frame
|
||||
}
|
||||
Reply::Ack => vec![packet_id::ACK],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_frame(frame: &[u8]) -> Result<Self, FrameError> {
|
||||
let (id, payload) = split_frame(frame, |id| match id {
|
||||
packet_id::HK => Some(Dipole::LEN + 1),
|
||||
packet_id::ACK => Some(0),
|
||||
_ => None,
|
||||
})?;
|
||||
Ok(match id {
|
||||
packet_id::ACK => Reply::Ack,
|
||||
_ => Reply::Hk(HkSet {
|
||||
dipole: Dipole::read_from(payload),
|
||||
torquing: payload[Dipole::LEN] != 0,
|
||||
}),
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_apply_torque_frame() {
|
||||
let request = Request::ApplyTorque {
|
||||
duration: Duration::from_millis(0x0102_0304),
|
||||
dipole: Dipole {
|
||||
x: -2,
|
||||
y: 0x0506,
|
||||
z: 0x0708,
|
||||
},
|
||||
};
|
||||
let frame = request.to_frame();
|
||||
assert_eq!(
|
||||
frame,
|
||||
[0x02, 0xff, 0xfe, 0x05, 0x06, 0x07, 0x08, 0x01, 0x02, 0x03, 0x04]
|
||||
);
|
||||
assert_eq!(Request::from_frame(&frame), Ok(request));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_request_hk_frame() {
|
||||
assert_eq!(Request::RequestHk.to_frame(), [0x01]);
|
||||
assert_eq!(Request::from_frame(&[0x01]), Ok(Request::RequestHk));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_reply_frames() {
|
||||
let hk = Reply::Hk(HkSet {
|
||||
dipole: Dipole { x: 1, y: 2, z: 3 },
|
||||
torquing: true,
|
||||
});
|
||||
let frame = hk.to_frame();
|
||||
assert_eq!(frame, [0x81, 0, 1, 0, 2, 0, 3, 1]);
|
||||
assert_eq!(Reply::from_frame(&frame), Ok(hk));
|
||||
assert_eq!(Reply::Ack.to_frame(), [0x82]);
|
||||
assert_eq!(Reply::from_frame(&[0x82]), Ok(Reply::Ack));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_invalid_frames() {
|
||||
assert_eq!(Request::from_frame(&[]), Err(FrameError::Empty));
|
||||
assert_eq!(
|
||||
Request::from_frame(&[0x81]),
|
||||
Err(FrameError::UnknownPacketId(0x81))
|
||||
);
|
||||
assert_eq!(
|
||||
Request::from_frame(&[0x01, 0x00]),
|
||||
Err(FrameError::InvalidLength { id: 0x01, len: 2 })
|
||||
);
|
||||
assert_eq!(
|
||||
Reply::from_frame(&[0x81, 0, 1]),
|
||||
Err(FrameError::InvalidLength { id: 0x81, len: 3 })
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,7 +5,7 @@ use std::time::{Duration, Instant};
|
||||
use satrs::spacepackets::CcsdsPacketIdAndPsc;
|
||||
use satrs_example::{HkHelperSingleSet, TmtcQueues};
|
||||
use satrs_minisim::acs::mgt as sim_mgt;
|
||||
use satrs_minisim::{SimReply, SimRequestWithTime};
|
||||
use satrs_minisim::{SimReply, SimRequest, SimRequestWithTime};
|
||||
use types::acs::mgt::{
|
||||
self, HkSet,
|
||||
request::{ModeRequest, Request},
|
||||
@@ -23,41 +23,41 @@ use crate::eps::PowerSwitchHelper;
|
||||
pub struct DummyInterface {
|
||||
dipole: sim_mgt::Dipole,
|
||||
torque_end: Option<Instant>,
|
||||
hk_requested: bool,
|
||||
replies: VecDeque<Vec<u8>>,
|
||||
}
|
||||
|
||||
impl DummyInterface {
|
||||
fn send(&mut self, request: sim_mgt::Request) {
|
||||
match request {
|
||||
fn send(&mut self, frame: &[u8]) {
|
||||
let Ok(request) = sim_mgt::Request::from_frame(frame) else {
|
||||
return;
|
||||
};
|
||||
let reply = match request {
|
||||
sim_mgt::Request::ApplyTorque { duration, dipole } => {
|
||||
self.dipole = dipole;
|
||||
self.torque_end = Some(Instant::now() + duration);
|
||||
sim_mgt::Reply::Ack
|
||||
}
|
||||
sim_mgt::Request::RequestHk => self.hk_requested = true,
|
||||
}
|
||||
}
|
||||
|
||||
fn try_recv_hk(&mut self) -> Option<sim_mgt::HkSet> {
|
||||
if !std::mem::take(&mut self.hk_requested) {
|
||||
return None;
|
||||
}
|
||||
let torquing = self.torque_end.is_some_and(|end| Instant::now() < end);
|
||||
Some(sim_mgt::HkSet {
|
||||
dipole: if torquing {
|
||||
self.dipole
|
||||
} else {
|
||||
sim_mgt::Dipole::default()
|
||||
},
|
||||
torquing,
|
||||
})
|
||||
sim_mgt::Request::RequestHk => {
|
||||
let torquing = self.torque_end.is_some_and(|end| Instant::now() < end);
|
||||
sim_mgt::Reply::Hk(sim_mgt::HkSet {
|
||||
dipole: if torquing {
|
||||
self.dipole
|
||||
} else {
|
||||
sim_mgt::Dipole::default()
|
||||
},
|
||||
torquing,
|
||||
})
|
||||
}
|
||||
};
|
||||
self.replies.push_back(reply.to_frame());
|
||||
}
|
||||
}
|
||||
|
||||
/// Records all requests and returns injected HK replies.
|
||||
/// Records all sent frames and returns injected reply frames.
|
||||
#[derive(Default)]
|
||||
pub struct TestInterface {
|
||||
pub sent_requests: Vec<sim_mgt::Request>,
|
||||
pub hk_replies: VecDeque<sim_mgt::HkSet>,
|
||||
pub sent_frames: Vec<Vec<u8>>,
|
||||
pub replies: VecDeque<Vec<u8>>,
|
||||
}
|
||||
|
||||
pub struct SimInterface {
|
||||
@@ -66,19 +66,21 @@ pub struct SimInterface {
|
||||
}
|
||||
|
||||
impl SimInterface {
|
||||
fn send(&mut self, request: sim_mgt::Request) {
|
||||
fn send(&mut self, frame: &[u8]) {
|
||||
if let Err(e) = self
|
||||
.sim_request_tx
|
||||
.send(SimRequestWithTime::new_with_epoch_time(request))
|
||||
.send(SimRequestWithTime::new_with_epoch_time(SimRequest::Mgt(
|
||||
frame.to_vec(),
|
||||
)))
|
||||
{
|
||||
log::error!("failed to send MGT SIM request: {e}");
|
||||
}
|
||||
}
|
||||
|
||||
fn try_recv_hk(&mut self) -> Option<sim_mgt::HkSet> {
|
||||
fn try_recv(&mut self) -> Option<Vec<u8>> {
|
||||
let sim_reply = self.sim_reply_rx.try_recv().ok()?;
|
||||
match sim_reply {
|
||||
SimReply::Mgt(sim_mgt::Reply::Hk(hk)) => Some(hk),
|
||||
SimReply::Mgt(frame) => Some(frame),
|
||||
_ => {
|
||||
log::warn!("unexpected MGT SIM reply: {sim_reply:?}");
|
||||
None
|
||||
@@ -87,6 +89,7 @@ impl SimInterface {
|
||||
}
|
||||
}
|
||||
|
||||
/// Frame based transport to the device. The handler implements the protocol on top of it.
|
||||
pub enum MgtCommunication {
|
||||
Dummy(DummyInterface),
|
||||
Sim(SimInterface),
|
||||
@@ -95,19 +98,19 @@ pub enum MgtCommunication {
|
||||
}
|
||||
|
||||
impl MgtCommunication {
|
||||
fn send(&mut self, request: sim_mgt::Request) {
|
||||
fn send(&mut self, frame: &[u8]) {
|
||||
match self {
|
||||
MgtCommunication::Dummy(dummy) => dummy.send(request),
|
||||
MgtCommunication::Sim(sim) => sim.send(request),
|
||||
MgtCommunication::Test(test) => test.sent_requests.push(request),
|
||||
MgtCommunication::Dummy(dummy) => dummy.send(frame),
|
||||
MgtCommunication::Sim(sim) => sim.send(frame),
|
||||
MgtCommunication::Test(test) => test.sent_frames.push(frame.to_vec()),
|
||||
}
|
||||
}
|
||||
|
||||
fn try_recv_hk(&mut self) -> Option<sim_mgt::HkSet> {
|
||||
fn try_recv(&mut self) -> Option<Vec<u8>> {
|
||||
match self {
|
||||
MgtCommunication::Dummy(dummy) => dummy.try_recv_hk(),
|
||||
MgtCommunication::Sim(sim) => sim.try_recv_hk(),
|
||||
MgtCommunication::Test(test) => test.hk_replies.pop_front(),
|
||||
MgtCommunication::Dummy(dummy) => dummy.replies.pop_front(),
|
||||
MgtCommunication::Sim(sim) => sim.try_recv(),
|
||||
MgtCommunication::Test(test) => test.replies.pop_front(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -183,25 +186,39 @@ impl MgtHandler {
|
||||
}
|
||||
|
||||
if self.ready_for_commanding() {
|
||||
self.com.send(sim_mgt::Request::RequestHk);
|
||||
}
|
||||
while let Some(hk) = self.com.try_recv_hk() {
|
||||
self.hk_set = HkSet {
|
||||
valid: true,
|
||||
dipole: types::acs::mgt::Dipole {
|
||||
x: hk.dipole.x,
|
||||
y: hk.dipole.y,
|
||||
z: hk.dipole.z,
|
||||
},
|
||||
torquing: hk.torquing,
|
||||
};
|
||||
self.send_request(sim_mgt::Request::RequestHk);
|
||||
}
|
||||
self.handle_replies();
|
||||
|
||||
if self.hk_helper.needs_generation() {
|
||||
self.send_telemetry(None, Response::Hk(self.hk_set));
|
||||
}
|
||||
}
|
||||
|
||||
fn send_request(&mut self, request: sim_mgt::Request) {
|
||||
self.com.send(&request.to_frame());
|
||||
}
|
||||
|
||||
fn handle_replies(&mut self) {
|
||||
while let Some(frame) = self.com.try_recv() {
|
||||
match sim_mgt::Reply::from_frame(&frame) {
|
||||
Ok(sim_mgt::Reply::Hk(hk)) => {
|
||||
self.hk_set = HkSet {
|
||||
valid: true,
|
||||
dipole: types::acs::mgt::Dipole {
|
||||
x: hk.dipole.x,
|
||||
y: hk.dipole.y,
|
||||
z: hk.dipole.z,
|
||||
},
|
||||
torquing: hk.torquing,
|
||||
};
|
||||
}
|
||||
Ok(sim_mgt::Reply::Ack) => (),
|
||||
Err(e) => log::warn!("MGT: invalid reply frame {frame:02x?}: {e}"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn ready_for_commanding(&self) -> bool {
|
||||
self.mode() == DeviceMode::Normal && self.switch_and_mode_helper.target().is_none()
|
||||
}
|
||||
@@ -271,7 +288,7 @@ impl MgtHandler {
|
||||
self.send_telemetry(Some(tc_id), Response::NotInNormalMode);
|
||||
return;
|
||||
}
|
||||
self.com.send(sim_mgt::Request::ApplyTorque {
|
||||
self.send_request(sim_mgt::Request::ApplyTorque {
|
||||
duration,
|
||||
dipole: sim_mgt::Dipole {
|
||||
x: dipole.x,
|
||||
@@ -355,6 +372,19 @@ mod tests {
|
||||
|
||||
use super::*;
|
||||
|
||||
impl TestInterface {
|
||||
fn sent_requests(&self) -> Vec<sim_mgt::Request> {
|
||||
self.sent_frames
|
||||
.iter()
|
||||
.map(|frame| sim_mgt::Request::from_frame(frame).unwrap())
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn push_reply(&mut self, reply: sim_mgt::Reply) {
|
||||
self.replies.push_back(reply.to_frame());
|
||||
}
|
||||
}
|
||||
|
||||
struct MgtTestbench {
|
||||
parent_request_tx: mpsc::SyncSender<ModeRequest>,
|
||||
parent_report_rx: mpsc::Receiver<ModeResponse>,
|
||||
@@ -442,7 +472,7 @@ mod tests {
|
||||
let mut testbench = MgtTestbench::new();
|
||||
testbench.handler.periodic_operation();
|
||||
assert_eq!(testbench.handler.mode(), DeviceMode::Off);
|
||||
assert!(testbench.test_interface().sent_requests.is_empty());
|
||||
assert!(testbench.test_interface().sent_frames.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
@@ -504,14 +534,14 @@ mod tests {
|
||||
});
|
||||
testbench.handler.periodic_operation();
|
||||
assert_eq!(testbench.next_response(), Response::NotInNormalMode);
|
||||
assert!(testbench.test_interface().sent_requests.is_empty());
|
||||
assert!(testbench.test_interface().sent_frames.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_torque_command_forwarded_in_normal_mode() {
|
||||
let mut testbench = MgtTestbench::new();
|
||||
testbench.switch_to_normal();
|
||||
testbench.test_interface().sent_requests.clear();
|
||||
testbench.test_interface().sent_frames.clear();
|
||||
|
||||
testbench.send_tc(Request::ApplyTorque {
|
||||
dipole: mgt::Dipole { x: 1, y: 2, z: 3 },
|
||||
@@ -520,7 +550,7 @@ mod tests {
|
||||
testbench.handler.periodic_operation();
|
||||
assert_eq!(testbench.next_response(), Response::Ok);
|
||||
assert_eq!(
|
||||
testbench.test_interface().sent_requests.first(),
|
||||
testbench.test_interface().sent_requests().first(),
|
||||
Some(&sim_mgt::Request::ApplyTorque {
|
||||
duration: Duration::from_millis(100),
|
||||
dipole: sim_mgt::Dipole { x: 1, y: 2, z: 3 },
|
||||
@@ -535,17 +565,16 @@ mod tests {
|
||||
assert!(
|
||||
testbench
|
||||
.test_interface()
|
||||
.sent_requests
|
||||
.sent_requests()
|
||||
.contains(&sim_mgt::Request::RequestHk)
|
||||
);
|
||||
|
||||
testbench
|
||||
.test_interface()
|
||||
.hk_replies
|
||||
.push_back(sim_mgt::HkSet {
|
||||
.push_reply(sim_mgt::Reply::Hk(sim_mgt::HkSet {
|
||||
dipole: sim_mgt::Dipole { x: 1, y: 2, z: 3 },
|
||||
torquing: true,
|
||||
});
|
||||
}));
|
||||
testbench.handler.periodic_operation();
|
||||
testbench.send_tc(Request::Hk(HkRequestType::OneShot));
|
||||
testbench.handler.periodic_operation();
|
||||
@@ -565,11 +594,10 @@ mod tests {
|
||||
testbench.switch_to_normal();
|
||||
testbench
|
||||
.test_interface()
|
||||
.hk_replies
|
||||
.push_back(sim_mgt::HkSet {
|
||||
.push_reply(sim_mgt::Reply::Hk(sim_mgt::HkSet {
|
||||
dipole: sim_mgt::Dipole::default(),
|
||||
torquing: false,
|
||||
});
|
||||
}));
|
||||
testbench.handler.periodic_operation();
|
||||
|
||||
testbench.send_tc(Request::Mode(ModeRequest::SetMode(DeviceMode::Off)));
|
||||
|
||||
Reference in New Issue
Block a user