rust/sat-rs
6
1
Fork 0
Code Issues 9 Pull Requests Projects Releases 24 Wiki Activity

Re-worked TMTC modules
All checks were successful
Rust/sat-rs/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2024-04-16 11:04:22 +02:00
parent 8cfe3b81e7
commit 63f37f0917
53 changed files with 2030 additions and 2939 deletions
Download Patch File Download Diff File Expand all files Collapse all files

403
satrs/src/tmtc/ccsds_distrib.rs
View File

@ -1,403 +0,0 @@
//! CCSDS packet routing components.
//!
//! The routing components consist of two core components:
//! 1. [CcsdsDistributor] component which dispatches received packets to a user-provided handler
//! 2. [CcsdsPacketHandler] trait which should be implemented by the user-provided packet handler.
//!
//! The [CcsdsDistributor] implements the [ReceivesCcsdsTc] and [ReceivesTcCore] trait which allows to
//! pass raw or CCSDS packets to it. Upon receiving a packet, it performs the following steps:
//!
//! 1. It tries to identify the target Application Process Identifier (APID) based on the
//! respective CCSDS space packet header field. If that process fails, a [ByteConversionError] is
//! returned to the user
//! 2. If a valid APID is found and matches one of the APIDs provided by
//! [CcsdsPacketHandler::valid_apids], it will pass the packet to the user provided
//! [CcsdsPacketHandler::handle_known_apid] function. If no valid APID is found, the packet
//! will be passed to the [CcsdsPacketHandler::handle_unknown_apid] function.
//!
//! # Example
//!
//! ```rust
//! use satrs::ValidatorU16Id;
//! use satrs::tmtc::ccsds_distrib::{CcsdsPacketHandler, CcsdsDistributor};
//! use satrs::tmtc::{ReceivesTc, ReceivesTcCore};
//! use spacepackets::{CcsdsPacket, SpHeader};
//! use spacepackets::ecss::WritablePusPacket;
//! use spacepackets::ecss::tc::PusTcCreator;
//!
//! #[derive (Default)]
//! struct ConcreteApidHandler {
//! known_call_count: u32,
//! unknown_call_count: u32
//! }
//!
//! impl ConcreteApidHandler {
//! fn mutable_foo(&mut self) {}
//! }
//!
//! impl ValidatorU16Id for ConcreteApidHandler {
//! fn validate(&self, apid: u16) -> bool { apid == 0x0002 }
//! }
//!
//! impl CcsdsPacketHandler for ConcreteApidHandler {
//! type Error = ();
//! fn handle_packet_with_valid_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
//! assert_eq!(sp_header.apid(), 0x002);
//! assert_eq!(tc_raw.len(), 13);
//! self.known_call_count += 1;
//! Ok(())
//! }
//! fn handle_packet_with_unknown_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
//! assert_eq!(sp_header.apid(), 0x003);
//! assert_eq!(tc_raw.len(), 13);
//! self.unknown_call_count += 1;
//! Ok(())
//! }
//! }
//!
//! let apid_handler = ConcreteApidHandler::default();
//! let mut ccsds_distributor = CcsdsDistributor::new(apid_handler);
//!
//! // Create and pass PUS telecommand with a valid APID
//! let sp_header = SpHeader::new_for_unseg_tc(0x002, 0x34, 0);
//! let mut pus_tc = PusTcCreator::new_simple(sp_header, 17, 1, &[], true);
//! let mut test_buf: [u8; 32] = [0; 32];
//! let mut size = pus_tc
//! .write_to_bytes(test_buf.as_mut_slice())
//! .expect("Error writing TC to buffer");
//! let tc_slice = &test_buf[0..size];
//! ccsds_distributor.pass_tc(&tc_slice).expect("Passing TC slice failed");
//!
//! // Now pass a packet with an unknown APID to the distributor
//! pus_tc.set_apid(0x003);
//! size = pus_tc
//! .write_to_bytes(test_buf.as_mut_slice())
//! .expect("Error writing TC to buffer");
//! let tc_slice = &test_buf[0..size];
//! ccsds_distributor.pass_tc(&tc_slice).expect("Passing TC slice failed");
//!
//! // Retrieve the APID handler.
//! let handler_ref = ccsds_distributor.packet_handler();
//! assert_eq!(handler_ref.known_call_count, 1);
//! assert_eq!(handler_ref.unknown_call_count, 1);
//!
//! // Mutable access to the handler.
//! let mutable_handler_ref = ccsds_distributor.packet_handler_mut();
//! mutable_handler_ref.mutable_foo();
//! ```
use crate::{
tmtc::{ReceivesCcsdsTc, ReceivesTcCore},
ValidatorU16Id,
};
use core::fmt::{Display, Formatter};
use spacepackets::{ByteConversionError, CcsdsPacket, SpHeader};
#[cfg(feature = "std")]
use std::error::Error;
/// Generic trait for a handler or dispatcher object handling CCSDS packets.
///
/// Users should implement this trait on their custom CCSDS packet handler and then pass a boxed
/// instance of this handler to the [CcsdsDistributor]. The distributor will use the trait
/// interface to dispatch received packets to the user based on the Application Process Identifier
/// (APID) field of the CCSDS packet. The APID will be checked using the generic [ValidatorU16Id]
/// trait.
pub trait CcsdsPacketHandler: ValidatorU16Id {
type Error;
fn handle_packet_with_valid_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error>;
fn handle_packet_with_unknown_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error>;
}
/// The CCSDS distributor dispatches received CCSDS packets to a user provided packet handler.
pub struct CcsdsDistributor<PacketHandler: CcsdsPacketHandler<Error = E>, E> {
/// User provided APID handler stored as a generic trait object.
/// It can be cast back to the original concrete type using [Self::packet_handler] or
/// the [Self::packet_handler_mut] method.
packet_handler: PacketHandler,
}
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum CcsdsError<E> {
CustomError(E),
ByteConversionError(ByteConversionError),
}
impl<E: Display> Display for CcsdsError<E> {
fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
match self {
Self::CustomError(e) => write!(f, "{e}"),
Self::ByteConversionError(e) => write!(f, "{e}"),
}
}
}
#[cfg(feature = "std")]
impl<E: Error> Error for CcsdsError<E> {
fn source(&self) -> Option<&(dyn Error + 'static)> {
match self {
Self::CustomError(e) => e.source(),
Self::ByteConversionError(e) => e.source(),
}
}
}
impl<PacketHandler: CcsdsPacketHandler<Error = E>, E: 'static> ReceivesCcsdsTc
for CcsdsDistributor<PacketHandler, E>
{
type Error = CcsdsError<E>;
fn pass_ccsds(&mut self, header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
self.dispatch_ccsds(header, tc_raw)
}
}
impl<PacketHandler: CcsdsPacketHandler<Error = E>, E: 'static> ReceivesTcCore
for CcsdsDistributor<PacketHandler, E>
{
type Error = CcsdsError<E>;
fn pass_tc(&mut self, tc_raw: &[u8]) -> Result<(), Self::Error> {
if tc_raw.len() < 7 {
return Err(CcsdsError::ByteConversionError(
ByteConversionError::FromSliceTooSmall {
found: tc_raw.len(),
expected: 7,
},
));
}
let (sp_header, _) =
SpHeader::from_be_bytes(tc_raw).map_err(|e| CcsdsError::ByteConversionError(e))?;
self.dispatch_ccsds(&sp_header, tc_raw)
}
}
impl<PacketHandler: CcsdsPacketHandler<Error = E>, E: 'static> CcsdsDistributor<PacketHandler, E> {
pub fn new(packet_handler: PacketHandler) -> Self {
CcsdsDistributor { packet_handler }
}
pub fn packet_handler(&self) -> &PacketHandler {
&self.packet_handler
}
pub fn packet_handler_mut(&mut self) -> &mut PacketHandler {
&mut self.packet_handler
}
fn dispatch_ccsds(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), CcsdsError<E>> {
let valid_apid = self.packet_handler().validate(sp_header.apid());
if valid_apid {
self.packet_handler
.handle_packet_with_valid_apid(sp_header, tc_raw)
.map_err(|e| CcsdsError::CustomError(e))?;
return Ok(());
}
self.packet_handler
.handle_packet_with_unknown_apid(sp_header, tc_raw)
.map_err(|e| CcsdsError::CustomError(e))
}
}
#[cfg(test)]
pub(crate) mod tests {
use super::*;
use crate::tmtc::ccsds_distrib::{CcsdsDistributor, CcsdsPacketHandler};
use spacepackets::ecss::tc::PusTcCreator;
use spacepackets::ecss::WritablePusPacket;
use spacepackets::CcsdsPacket;
use std::collections::VecDeque;
use std::sync::{Arc, Mutex};
use std::vec::Vec;
fn is_send<T: Send>(_: &T) {}
pub fn generate_ping_tc(buf: &mut [u8]) -> &[u8] {
let sph = SpHeader::new_for_unseg_tc(0x002, 0x34, 0);
let pus_tc = PusTcCreator::new_simple(sph, 17, 1, &[], true);
let size = pus_tc
.write_to_bytes(buf)
.expect("Error writing TC to buffer");
assert_eq!(size, 13);
&buf[0..size]
}
pub fn generate_ping_tc_as_vec() -> Vec<u8> {
let sph = SpHeader::new_for_unseg_tc(0x002, 0x34, 0);
PusTcCreator::new_simple(sph, 17, 1, &[], true)
.to_vec()
.unwrap()
}
type SharedPacketQueue = Arc<Mutex<VecDeque<(u16, Vec<u8>)>>>;
pub struct BasicApidHandlerSharedQueue {
pub known_packet_queue: SharedPacketQueue,
pub unknown_packet_queue: SharedPacketQueue,
}
#[derive(Default)]
pub struct BasicApidHandlerOwnedQueue {
pub known_packet_queue: VecDeque<(u16, Vec<u8>)>,
pub unknown_packet_queue: VecDeque<(u16, Vec<u8>)>,
}
impl ValidatorU16Id for BasicApidHandlerSharedQueue {
fn validate(&self, packet_id: u16) -> bool {
[0x000, 0x002].contains(&packet_id)
}
}
impl CcsdsPacketHandler for BasicApidHandlerSharedQueue {
type Error = ();
fn handle_packet_with_valid_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
let mut vec = Vec::new();
vec.extend_from_slice(tc_raw);
self.known_packet_queue
.lock()
.unwrap()
.push_back((sp_header.apid(), vec));
Ok(())
}
fn handle_packet_with_unknown_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
let mut vec = Vec::new();
vec.extend_from_slice(tc_raw);
self.unknown_packet_queue
.lock()
.unwrap()
.push_back((sp_header.apid(), vec));
Ok(())
}
}
impl ValidatorU16Id for BasicApidHandlerOwnedQueue {
fn validate(&self, packet_id: u16) -> bool {
[0x000, 0x002].contains(&packet_id)
}
}
impl CcsdsPacketHandler for BasicApidHandlerOwnedQueue {
type Error = ();
fn handle_packet_with_valid_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
let mut vec = Vec::new();
vec.extend_from_slice(tc_raw);
self.known_packet_queue.push_back((sp_header.apid(), vec));
Ok(())
}
fn handle_packet_with_unknown_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
let mut vec = Vec::new();
vec.extend_from_slice(tc_raw);
self.unknown_packet_queue.push_back((sp_header.apid(), vec));
Ok(())
}
}
#[test]
fn test_distribs_known_apid() {
let known_packet_queue = Arc::new(Mutex::default());
let unknown_packet_queue = Arc::new(Mutex::default());
let apid_handler = BasicApidHandlerSharedQueue {
known_packet_queue: known_packet_queue.clone(),
unknown_packet_queue: unknown_packet_queue.clone(),
};
let mut ccsds_distrib = CcsdsDistributor::new(apid_handler);
is_send(&ccsds_distrib);
let mut test_buf: [u8; 32] = [0; 32];
let tc_slice = generate_ping_tc(test_buf.as_mut_slice());
ccsds_distrib.pass_tc(tc_slice).expect("Passing TC failed");
let recvd = known_packet_queue.lock().unwrap().pop_front();
assert!(unknown_packet_queue.lock().unwrap().is_empty());
assert!(recvd.is_some());
let (apid, packet) = recvd.unwrap();
assert_eq!(apid, 0x002);
assert_eq!(packet, tc_slice);
}
#[test]
fn test_unknown_apid_handling() {
let apid_handler = BasicApidHandlerOwnedQueue::default();
let mut ccsds_distrib = CcsdsDistributor::new(apid_handler);
let sph = SpHeader::new_for_unseg_tc(0x004, 0x34, 0);
let pus_tc = PusTcCreator::new_simple(sph, 17, 1, &[], true);
let mut test_buf: [u8; 32] = [0; 32];
pus_tc
.write_to_bytes(test_buf.as_mut_slice())
.expect("Error writing TC to buffer");
ccsds_distrib.pass_tc(&test_buf).expect("Passing TC failed");
assert!(ccsds_distrib.packet_handler().known_packet_queue.is_empty());
let apid_handler = ccsds_distrib.packet_handler_mut();
let recvd = apid_handler.unknown_packet_queue.pop_front();
assert!(recvd.is_some());
let (apid, packet) = recvd.unwrap();
assert_eq!(apid, 0x004);
assert_eq!(packet.as_slice(), test_buf);
}
#[test]
fn test_ccsds_distribution() {
let mut ccsds_distrib = CcsdsDistributor::new(BasicApidHandlerOwnedQueue::default());
let sph = SpHeader::new_for_unseg_tc(0x002, 0x34, 0);
let pus_tc = PusTcCreator::new_simple(sph, 17, 1, &[], true);
let tc_vec = pus_tc.to_vec().unwrap();
ccsds_distrib
.pass_ccsds(&sph, &tc_vec)
.expect("passing CCSDS TC failed");
let recvd = ccsds_distrib
.packet_handler_mut()
.known_packet_queue
.pop_front();
assert!(recvd.is_some());
let recvd = recvd.unwrap();
assert_eq!(recvd.0, 0x002);
assert_eq!(recvd.1, tc_vec);
}
#[test]
fn test_distribution_short_packet_fails() {
let mut ccsds_distrib = CcsdsDistributor::new(BasicApidHandlerOwnedQueue::default());
let sph = SpHeader::new_for_unseg_tc(0x002, 0x34, 0);
let pus_tc = PusTcCreator::new_simple(sph, 17, 1, &[], true);
let tc_vec = pus_tc.to_vec().unwrap();
let result = ccsds_distrib.pass_tc(&tc_vec[0..6]);
assert!(result.is_err());
let error = result.unwrap_err();
if let CcsdsError::ByteConversionError(ByteConversionError::FromSliceTooSmall {
found,
expected,
}) = error
{
assert_eq!(found, 6);
assert_eq!(expected, 7);
} else {
panic!("Unexpected error variant");
}
}
}

640
satrs/src/tmtc/mod.rs
View File

@ -1,115 +1,651 @@
//! Telemetry and Telecommanding (TMTC) module. Contains packet routing components with special
//! support for CCSDS and ECSS packets.
//!
//! The distributor modules provided by this module use trait objects provided by the user to
//! directly dispatch received packets to packet listeners based on packet fields like the CCSDS
//! Application Process ID (APID) or the ECSS PUS service type. This allows for fast packet
//! routing without the overhead and complication of using message queues. However, it also requires
//! It is recommended to read the [sat-rs book chapter](https://absatsw.irs.uni-stuttgart.de/projects/sat-rs/book/communication.html)
//! about communication first. The TMTC abstractions provided by this framework are based on the
//! assumption that all telemetry is sent to a special handler object called the TM sink while
//! all received telecommands are sent to a special handler object called TC source. Using
//! a design like this makes it simpler to add new TC packet sources or new telemetry generators:
//! They only need to send the received and generated data to these objects.
use crate::queue::GenericSendError;
use crate::{
pool::{PoolAddr, PoolError},
ComponentId,
};
#[cfg(feature = "std")]
pub use alloc_mod::*;
#[cfg(feature = "alloc")]
use downcast_rs::{impl_downcast, Downcast};
use spacepackets::SpHeader;
use spacepackets::{
ecss::{
tc::PusTcReader,
tm::{PusTmCreator, PusTmReader},
},
SpHeader,
};
#[cfg(feature = "std")]
use std::sync::mpsc;
#[cfg(feature = "std")]
pub use std_mod::*;
#[cfg(feature = "alloc")]
pub mod ccsds_distrib;
#[cfg(feature = "alloc")]
pub mod pus_distrib;
pub mod tm_helper;
#[cfg(feature = "alloc")]
pub use ccsds_distrib::{CcsdsDistributor, CcsdsError, CcsdsPacketHandler};
#[cfg(feature = "alloc")]
pub use pus_distrib::{PusDistributor, PusServiceDistributor};
/// Simple type modelling packet stored inside a pool structure. This structure is intended to
/// be used when sending a packet via a message queue, so it also contains the sender ID.
#[derive(Debug, PartialEq, Eq, Clone)]
pub struct PacketInPool {
pub sender_id: ComponentId,
pub store_addr: PoolAddr,
}
/// Generic trait for object which can receive any telecommands in form of a raw bytestream, with
impl PacketInPool {
pub fn new(sender_id: ComponentId, store_addr: PoolAddr) -> Self {
Self {
sender_id,
store_addr,
}
}
}
/// Generic trait for object which can send any packets in form of a raw bytestream, with
/// no assumptions about the received protocol.
///
/// This trait is implemented by both the [crate::tmtc::pus_distrib::PusDistributor] and the
/// [crate::tmtc::ccsds_distrib::CcsdsDistributor] which allows to pass the respective packets in
/// raw byte format into them.
pub trait ReceivesTcCore {
pub trait PacketSenderRaw: Send {
type Error;
fn pass_tc(&mut self, tc_raw: &[u8]) -> Result<(), Self::Error>;
fn send_packet(&self, sender_id: ComponentId, packet: &[u8]) -> Result<(), Self::Error>;
}
/// Extension trait of [ReceivesTcCore] which allows downcasting by implementing [Downcast] and
/// is also sendable.
/// Extension trait of [PacketSenderRaw] which allows downcasting by implementing [Downcast].
#[cfg(feature = "alloc")]
pub trait ReceivesTc: ReceivesTcCore + Downcast + Send {
pub trait PacketSenderRawExt: PacketSenderRaw + Downcast {
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast(&self) -> &dyn ReceivesTcCore<Error = Self::Error>;
fn upcast(&self) -> &dyn PacketSenderRaw<Error = Self::Error>;
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast_mut(&mut self) -> &mut dyn ReceivesTcCore<Error = Self::Error>;
fn upcast_mut(&mut self) -> &mut dyn PacketSenderRaw<Error = Self::Error>;
}
/// Blanket implementation to automatically implement [ReceivesTc] when the [alloc] feature
/// is enabled.
/// Blanket implementation to automatically implement [PacketSenderRawExt] when the [alloc]
/// feature is enabled.
#[cfg(feature = "alloc")]
impl<T> ReceivesTc for T
impl<T> PacketSenderRawExt for T
where
T: ReceivesTcCore + Send + 'static,
T: PacketSenderRaw + Send + 'static,
{
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast(&self) -> &dyn ReceivesTcCore<Error = Self::Error> {
fn upcast(&self) -> &dyn PacketSenderRaw<Error = Self::Error> {
self
}
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast_mut(&mut self) -> &mut dyn ReceivesTcCore<Error = Self::Error> {
fn upcast_mut(&mut self) -> &mut dyn PacketSenderRaw<Error = Self::Error> {
self
}
}
#[cfg(feature = "alloc")]
impl_downcast!(ReceivesTc assoc Error);
impl_downcast!(PacketSenderRawExt assoc Error);
/// Generic trait for object which can receive CCSDS space packets, for example ECSS PUS packets
/// for CCSDS File Delivery Protocol (CFDP) packets.
///
/// This trait is implemented by both the [crate::tmtc::pus_distrib::PusDistributor] and the
/// [crate::tmtc::ccsds_distrib::CcsdsDistributor] which allows
/// to pass the respective packets in raw byte format or in CCSDS format into them.
pub trait ReceivesCcsdsTc {
/// Generic trait for object which can send CCSDS space packets, for example ECSS PUS packets
/// or CCSDS File Delivery Protocol (CFDP) packets wrapped in space packets.
pub trait PacketSenderCcsds: Send {
type Error;
fn pass_ccsds(&mut self, header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error>;
fn send_ccsds(
&self,
sender_id: ComponentId,
header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error>;
}
/// Generic trait for a TM packet source, with no restrictions on the type of TM.
#[cfg(feature = "std")]
impl PacketSenderCcsds for mpsc::Sender<PacketAsVec> {
type Error = GenericSendError;
fn send_ccsds(
&self,
sender_id: ComponentId,
_: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
self.send(PacketAsVec::new(sender_id, tc_raw.to_vec()))
.map_err(|_| GenericSendError::RxDisconnected)
}
}
#[cfg(feature = "std")]
impl PacketSenderCcsds for mpsc::SyncSender<PacketAsVec> {
type Error = GenericSendError;
fn send_ccsds(
&self,
sender_id: ComponentId,
_: &SpHeader,
packet_raw: &[u8],
) -> Result<(), Self::Error> {
self.try_send(PacketAsVec::new(sender_id, packet_raw.to_vec()))
.map_err(|e| match e {
mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None),
mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected,
})
}
}
/// Generic trait for a packet receiver, with no restrictions on the type of packet.
/// Implementors write the telemetry into the provided buffer and return the size of the telemetry.
pub trait TmPacketSourceCore {
pub trait PacketSource: Send {
type Error;
fn retrieve_packet(&mut self, buffer: &mut [u8]) -> Result<usize, Self::Error>;
}
/// Extension trait of [TmPacketSourceCore] which allows downcasting by implementing [Downcast] and
/// is also sendable.
/// Extension trait of [PacketSource] which allows downcasting by implementing [Downcast].
#[cfg(feature = "alloc")]
pub trait TmPacketSource: TmPacketSourceCore + Downcast + Send {
pub trait PacketSourceExt: PacketSource + Downcast {
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast(&self) -> &dyn TmPacketSourceCore<Error = Self::Error>;
fn upcast(&self) -> &dyn PacketSource<Error = Self::Error>;
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast_mut(&mut self) -> &mut dyn TmPacketSourceCore<Error = Self::Error>;
fn upcast_mut(&mut self) -> &mut dyn PacketSource<Error = Self::Error>;
}
/// Blanket implementation to automatically implement [ReceivesTc] when the [alloc] feature
/// Blanket implementation to automatically implement [PacketSourceExt] when the [alloc] feature
/// is enabled.
#[cfg(feature = "alloc")]
impl<T> TmPacketSource for T
impl<T> PacketSourceExt for T
where
T: TmPacketSourceCore + Send + 'static,
T: PacketSource + 'static,
{
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast(&self) -> &dyn TmPacketSourceCore<Error = Self::Error> {
fn upcast(&self) -> &dyn PacketSource<Error = Self::Error> {
self
}
// Remove this once trait upcasting coercion has been implemented.
// Tracking issue: https://github.com/rust-lang/rust/issues/65991
fn upcast_mut(&mut self) -> &mut dyn TmPacketSourceCore<Error = Self::Error> {
fn upcast_mut(&mut self) -> &mut dyn PacketSource<Error = Self::Error> {
self
}
}
/// Helper trait for any generic (static) store which allows storing raw or CCSDS packets.
pub trait CcsdsPacketPool {
fn add_ccsds_tc(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<PoolAddr, PoolError> {
self.add_raw_tc(tc_raw)
}
fn add_raw_tc(&mut self, tc_raw: &[u8]) -> Result<PoolAddr, PoolError>;
}
/// Helper trait for any generic (static) store which allows storing ECSS PUS Telecommand packets.
pub trait PusTcPool {
fn add_pus_tc(&mut self, pus_tc: &PusTcReader) -> Result<PoolAddr, PoolError>;
}
/// Helper trait for any generic (static) store which allows storing ECSS PUS Telemetry packets.
pub trait PusTmPool {
fn add_pus_tm_from_reader(&mut self, pus_tm: &PusTmReader) -> Result<PoolAddr, PoolError>;
fn add_pus_tm_from_creator(&mut self, pus_tm: &PusTmCreator) -> Result<PoolAddr, PoolError>;
}
/// Generic trait for any sender component able to send packets stored inside a pool structure.
pub trait PacketInPoolSender: Send {
fn send_packet(
&self,
sender_id: ComponentId,
store_addr: PoolAddr,
) -> Result<(), GenericSendError>;
}
#[cfg(feature = "alloc")]
pub mod alloc_mod {
use alloc::vec::Vec;
use super::*;
/// Simple type modelling packet stored in the heap. This structure is intended to
/// be used when sending a packet via a message queue, so it also contains the sender ID.
#[derive(Debug, PartialEq, Eq, Clone)]
pub struct PacketAsVec {
pub sender_id: ComponentId,
pub packet: Vec<u8>,
}
impl PacketAsVec {
pub fn new(sender_id: ComponentId, packet: Vec<u8>) -> Self {
Self { sender_id, packet }
}
}
}
#[cfg(feature = "std")]
pub mod std_mod {
use core::cell::RefCell;
#[cfg(feature = "crossbeam")]
use crossbeam_channel as cb;
use spacepackets::ecss::WritablePusPacket;
use thiserror::Error;
use crate::pool::PoolProvider;
use crate::pus::{EcssTmSender, EcssTmtcError, PacketSenderPusTc};
use super::*;
/// Newtype wrapper around the [SharedStaticMemoryPool] to enable extension helper traits on
/// top of the regular shared memory pool API.
#[derive(Clone)]
pub struct SharedPacketPool(pub SharedStaticMemoryPool);
impl SharedPacketPool {
pub fn new(pool: &SharedStaticMemoryPool) -> Self {
Self(pool.clone())
}
}
impl PusTcPool for SharedPacketPool {
fn add_pus_tc(&mut self, pus_tc: &PusTcReader) -> Result<PoolAddr, PoolError> {
let mut pg = self.0.write().map_err(|_| PoolError::LockError)?;
let addr = pg.free_element(pus_tc.len_packed(), |buf| {
buf[0..pus_tc.len_packed()].copy_from_slice(pus_tc.raw_data());
})?;
Ok(addr)
}
}
impl PusTmPool for SharedPacketPool {
fn add_pus_tm_from_reader(&mut self, pus_tm: &PusTmReader) -> Result<PoolAddr, PoolError> {
let mut pg = self.0.write().map_err(|_| PoolError::LockError)?;
let addr = pg.free_element(pus_tm.len_packed(), |buf| {
buf[0..pus_tm.len_packed()].copy_from_slice(pus_tm.raw_data());
})?;
Ok(addr)
}
fn add_pus_tm_from_creator(
&mut self,
pus_tm: &PusTmCreator,
) -> Result<PoolAddr, PoolError> {
let mut pg = self.0.write().map_err(|_| PoolError::LockError)?;
let mut result = Ok(0);
let addr = pg.free_element(pus_tm.len_written(), |buf| {
result = pus_tm.write_to_bytes(buf);
})?;
result?;
Ok(addr)
}
}
impl CcsdsPacketPool for SharedPacketPool {
fn add_raw_tc(&mut self, tc_raw: &[u8]) -> Result<PoolAddr, PoolError> {
let mut pg = self.0.write().map_err(|_| PoolError::LockError)?;
let addr = pg.free_element(tc_raw.len(), |buf| {
buf[0..tc_raw.len()].copy_from_slice(tc_raw);
})?;
Ok(addr)
}
}
#[cfg(feature = "std")]
impl PacketSenderRaw for mpsc::Sender<PacketAsVec> {
type Error = GenericSendError;
fn send_packet(&self, sender_id: ComponentId, packet: &[u8]) -> Result<(), Self::Error> {
self.send(PacketAsVec::new(sender_id, packet.to_vec()))
.map_err(|_| GenericSendError::RxDisconnected)
}
}
#[cfg(feature = "std")]
impl PacketSenderRaw for mpsc::SyncSender<PacketAsVec> {
type Error = GenericSendError;
fn send_packet(&self, sender_id: ComponentId, tc_raw: &[u8]) -> Result<(), Self::Error> {
self.try_send(PacketAsVec::new(sender_id, tc_raw.to_vec()))
.map_err(|e| match e {
mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None),
mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected,
})
}
}
#[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum StoreAndSendError {
#[error("Store error: {0}")]
Store(#[from] PoolError),
#[error("Genreric send error: {0}")]
Send(#[from] GenericSendError),
}
pub use crate::pool::SharedStaticMemoryPool;
impl PacketInPoolSender for mpsc::Sender<PacketInPool> {
fn send_packet(
&self,
sender_id: ComponentId,
store_addr: PoolAddr,
) -> Result<(), GenericSendError> {
self.send(PacketInPool::new(sender_id, store_addr))
.map_err(|_| GenericSendError::RxDisconnected)
}
}
impl PacketInPoolSender for mpsc::SyncSender<PacketInPool> {
fn send_packet(
&self,
sender_id: ComponentId,
store_addr: PoolAddr,
) -> Result<(), GenericSendError> {
self.try_send(PacketInPool::new(sender_id, store_addr))
.map_err(|e| match e {
mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None),
mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected,
})
}
}
#[cfg(feature = "crossbeam")]
impl PacketInPoolSender for cb::Sender<PacketInPool> {
fn send_packet(
&self,
sender_id: ComponentId,
store_addr: PoolAddr,
) -> Result<(), GenericSendError> {
self.try_send(PacketInPool::new(sender_id, store_addr))
.map_err(|e| match e {
cb::TrySendError::Full(_) => GenericSendError::QueueFull(None),
cb::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected,
})
}
}
/// This is the primary structure used to send packets stored in a dedicated memory pool
/// structure.
#[derive(Clone)]
pub struct PacketSenderWithSharedPool<
Sender: PacketInPoolSender = mpsc::SyncSender<PacketInPool>,
PacketPool: CcsdsPacketPool = SharedPacketPool,
> {
pub sender: Sender,
pub shared_pool: RefCell<PacketPool>,
}
impl<Sender: PacketInPoolSender> PacketSenderWithSharedPool<Sender, SharedPacketPool> {
pub fn new_with_shared_packet_pool(
packet_sender: Sender,
shared_pool: &SharedStaticMemoryPool,
) -> Self {
Self {
sender: packet_sender,
shared_pool: RefCell::new(SharedPacketPool::new(shared_pool)),
}
}
}
impl<Sender: PacketInPoolSender, PacketStore: CcsdsPacketPool>
PacketSenderWithSharedPool<Sender, PacketStore>
{
pub fn new(packet_sender: Sender, shared_pool: PacketStore) -> Self {
Self {
sender: packet_sender,
shared_pool: RefCell::new(shared_pool),
}
}
}
impl<Sender: PacketInPoolSender, PacketStore: CcsdsPacketPool + Clone>
PacketSenderWithSharedPool<Sender, PacketStore>
{
pub fn shared_packet_store(&self) -> PacketStore {
let pool = self.shared_pool.borrow();
pool.clone()
}
}
impl<Sender: PacketInPoolSender, PacketStore: CcsdsPacketPool + Send> PacketSenderRaw
for PacketSenderWithSharedPool<Sender, PacketStore>
{
type Error = StoreAndSendError;
fn send_packet(&self, sender_id: ComponentId, packet: &[u8]) -> Result<(), Self::Error> {
let mut shared_pool = self.shared_pool.borrow_mut();
let store_addr = shared_pool.add_raw_tc(packet)?;
drop(shared_pool);
self.sender
.send_packet(sender_id, store_addr)
.map_err(StoreAndSendError::Send)?;
Ok(())
}
}
impl<Sender: PacketInPoolSender, PacketStore: CcsdsPacketPool + PusTcPool + Send>
PacketSenderPusTc for PacketSenderWithSharedPool<Sender, PacketStore>
{
type Error = StoreAndSendError;
fn send_pus_tc(
&self,
sender_id: ComponentId,
_: &SpHeader,
pus_tc: &PusTcReader,
) -> Result<(), Self::Error> {
let mut shared_pool = self.shared_pool.borrow_mut();
let store_addr = shared_pool.add_raw_tc(pus_tc.raw_data())?;
drop(shared_pool);
self.sender
.send_packet(sender_id, store_addr)
.map_err(StoreAndSendError::Send)?;
Ok(())
}
}
impl<Sender: PacketInPoolSender, PacketStore: CcsdsPacketPool + Send> PacketSenderCcsds
for PacketSenderWithSharedPool<Sender, PacketStore>
{
type Error = StoreAndSendError;
fn send_ccsds(
&self,
sender_id: ComponentId,
_sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
self.send_packet(sender_id, tc_raw)
}
}
impl<Sender: PacketInPoolSender, PacketStore: CcsdsPacketPool + PusTmPool + Send> EcssTmSender
for PacketSenderWithSharedPool<Sender, PacketStore>
{
fn send_tm(
&self,
sender_id: crate::ComponentId,
tm: crate::pus::PusTmVariant,
) -> Result<(), crate::pus::EcssTmtcError> {
let send_addr = |store_addr: PoolAddr| {
self.sender
.send_packet(sender_id, store_addr)
.map_err(EcssTmtcError::Send)
};
match tm {
crate::pus::PusTmVariant::InStore(store_addr) => send_addr(store_addr),
crate::pus::PusTmVariant::Direct(tm_creator) => {
let mut pool = self.shared_pool.borrow_mut();
let store_addr = pool.add_pus_tm_from_creator(&tm_creator)?;
send_addr(store_addr)
}
}
}
}
}
#[cfg(test)]
pub(crate) mod tests {
use alloc::vec;
use std::sync::RwLock;
use crate::pool::{PoolProviderWithGuards, StaticMemoryPool, StaticPoolConfig};
use super::*;
use std::sync::mpsc;
pub(crate) fn send_with_sender<SendError>(
sender_id: ComponentId,
packet_sender: &(impl PacketSenderRaw<Error = SendError> + ?Sized),
packet: &[u8],
) -> Result<(), SendError> {
packet_sender.send_packet(sender_id, packet)
}
#[test]
fn test_basic_mpsc_channel_sender_bounded() {
let (tx, rx) = mpsc::channel();
let some_packet = vec![1, 2, 3, 4, 5];
send_with_sender(1, &tx, &some_packet).expect("failed to send packet");
let rx_packet = rx.try_recv().unwrap();
assert_eq!(some_packet, rx_packet.packet);
assert_eq!(1, rx_packet.sender_id);
}
#[test]
fn test_basic_mpsc_channel_receiver_dropped() {
let (tx, rx) = mpsc::channel();
let some_packet = vec![1, 2, 3, 4, 5];
drop(rx);
let result = send_with_sender(2, &tx, &some_packet);
assert!(result.is_err());
matches!(result.unwrap_err(), GenericSendError::RxDisconnected);
}
#[test]
fn test_basic_mpsc_sync_sender() {
let (tx, rx) = mpsc::sync_channel(3);
let some_packet = vec![1, 2, 3, 4, 5];
send_with_sender(3, &tx, &some_packet).expect("failed to send packet");
let rx_packet = rx.try_recv().unwrap();
assert_eq!(some_packet, rx_packet.packet);
assert_eq!(3, rx_packet.sender_id);
}
#[test]
fn test_basic_mpsc_sync_sender_receiver_dropped() {
let (tx, rx) = mpsc::sync_channel(3);
let some_packet = vec![1, 2, 3, 4, 5];
drop(rx);
let result = send_with_sender(0, &tx, &some_packet);
assert!(result.is_err());
matches!(result.unwrap_err(), GenericSendError::RxDisconnected);
}
#[test]
fn test_basic_mpsc_sync_sender_queue_full() {
let (tx, rx) = mpsc::sync_channel(1);
let some_packet = vec![1, 2, 3, 4, 5];
send_with_sender(0, &tx, &some_packet).expect("failed to send packet");
let result = send_with_sender(1, &tx, &some_packet);
assert!(result.is_err());
matches!(result.unwrap_err(), GenericSendError::QueueFull(None));
let rx_packet = rx.try_recv().unwrap();
assert_eq!(some_packet, rx_packet.packet);
}
#[test]
fn test_basic_shared_store_sender_unbounded_sender() {
let (tc_tx, tc_rx) = mpsc::channel();
let pool_cfg = StaticPoolConfig::new(vec![(2, 8)], true);
let shared_pool = SharedPacketPool::new(&SharedStaticMemoryPool::new(RwLock::new(
StaticMemoryPool::new(pool_cfg),
)));
let some_packet = vec![1, 2, 3, 4, 5];
let tc_sender = PacketSenderWithSharedPool::new(tc_tx, shared_pool.clone());
send_with_sender(5, &tc_sender, &some_packet).expect("failed to send packet");
let packet_in_pool = tc_rx.try_recv().unwrap();
let mut pool = shared_pool.0.write().unwrap();
let read_guard = pool.read_with_guard(packet_in_pool.store_addr);
assert_eq!(read_guard.read_as_vec().unwrap(), some_packet);
assert_eq!(packet_in_pool.sender_id, 5)
}
#[test]
fn test_basic_shared_store_sender() {
let (tc_tx, tc_rx) = mpsc::sync_channel(10);
let pool_cfg = StaticPoolConfig::new(vec![(2, 8)], true);
let shared_pool = SharedPacketPool::new(&SharedStaticMemoryPool::new(RwLock::new(
StaticMemoryPool::new(pool_cfg),
)));
let some_packet = vec![1, 2, 3, 4, 5];
let tc_sender = PacketSenderWithSharedPool::new(tc_tx, shared_pool.clone());
send_with_sender(5, &tc_sender, &some_packet).expect("failed to send packet");
let packet_in_pool = tc_rx.try_recv().unwrap();
let mut pool = shared_pool.0.write().unwrap();
let read_guard = pool.read_with_guard(packet_in_pool.store_addr);
assert_eq!(read_guard.read_as_vec().unwrap(), some_packet);
assert_eq!(packet_in_pool.sender_id, 5)
}
#[test]
fn test_basic_shared_store_sender_rx_dropped() {
let (tc_tx, tc_rx) = mpsc::sync_channel(10);
let pool_cfg = StaticPoolConfig::new(vec![(2, 8)], true);
let shared_pool = SharedPacketPool::new(&SharedStaticMemoryPool::new(RwLock::new(
StaticMemoryPool::new(pool_cfg),
)));
let some_packet = vec![1, 2, 3, 4, 5];
drop(tc_rx);
let tc_sender = PacketSenderWithSharedPool::new(tc_tx, shared_pool.clone());
let result = send_with_sender(2, &tc_sender, &some_packet);
assert!(result.is_err());
matches!(
result.unwrap_err(),
StoreAndSendError::Send(GenericSendError::RxDisconnected)
);
}
#[test]
fn test_basic_shared_store_sender_queue_full() {
let (tc_tx, tc_rx) = mpsc::sync_channel(1);
let pool_cfg = StaticPoolConfig::new(vec![(2, 8)], true);
let shared_pool = SharedPacketPool::new(&SharedStaticMemoryPool::new(RwLock::new(
StaticMemoryPool::new(pool_cfg),
)));
let some_packet = vec![1, 2, 3, 4, 5];
let tc_sender = PacketSenderWithSharedPool::new(tc_tx, shared_pool.clone());
send_with_sender(3, &tc_sender, &some_packet).expect("failed to send packet");
let result = send_with_sender(3, &tc_sender, &some_packet);
assert!(result.is_err());
matches!(
result.unwrap_err(),
StoreAndSendError::Send(GenericSendError::RxDisconnected)
);
let packet_in_pool = tc_rx.try_recv().unwrap();
let mut pool = shared_pool.0.write().unwrap();
let read_guard = pool.read_with_guard(packet_in_pool.store_addr);
assert_eq!(read_guard.read_as_vec().unwrap(), some_packet);
assert_eq!(packet_in_pool.sender_id, 3);
}
#[test]
fn test_basic_shared_store_store_error() {
let (tc_tx, tc_rx) = mpsc::sync_channel(1);
let pool_cfg = StaticPoolConfig::new(vec![(1, 8)], true);
let shared_pool = SharedPacketPool::new(&SharedStaticMemoryPool::new(RwLock::new(
StaticMemoryPool::new(pool_cfg),
)));
let some_packet = vec![1, 2, 3, 4, 5];
let tc_sender = PacketSenderWithSharedPool::new(tc_tx, shared_pool.clone());
send_with_sender(4, &tc_sender, &some_packet).expect("failed to send packet");
let result = send_with_sender(4, &tc_sender, &some_packet);
assert!(result.is_err());
matches!(
result.unwrap_err(),
StoreAndSendError::Store(PoolError::StoreFull(..))
);
let packet_in_pool = tc_rx.try_recv().unwrap();
let mut pool = shared_pool.0.write().unwrap();
let read_guard = pool.read_with_guard(packet_in_pool.store_addr);
assert_eq!(read_guard.read_as_vec().unwrap(), some_packet);
assert_eq!(packet_in_pool.sender_id, 4);
}
}

414
satrs/src/tmtc/pus_distrib.rs
View File

@ -1,414 +0,0 @@
//! ECSS PUS packet routing components.
//!
//! The routing components consist of two core components:
//! 1. [PusDistributor] component which dispatches received packets to a user-provided handler.
//! 2. [PusServiceDistributor] trait which should be implemented by the user-provided PUS packet
//! handler.
//!
//! The [PusDistributor] implements the [ReceivesEcssPusTc], [ReceivesCcsdsTc] and the
//! [ReceivesTcCore] trait which allows to pass raw packets, CCSDS packets and PUS TC packets into
//! it. Upon receiving a packet, it performs the following steps:
//!
//! 1. It tries to extract the [SpHeader] and [spacepackets::ecss::tc::PusTcReader] objects from
//! the raw bytestream. If this process fails, a [PusDistribError::PusError] is returned to the
//! user.
//! 2. If it was possible to extract both components, the packet will be passed to the
//! [PusServiceDistributor::distribute_packet] method provided by the user.
//!
//! # Example
//!
//! ```rust
//! use spacepackets::ecss::WritablePusPacket;
//! use satrs::tmtc::pus_distrib::{PusDistributor, PusServiceDistributor};
//! use satrs::tmtc::{ReceivesTc, ReceivesTcCore};
//! use spacepackets::SpHeader;
//! use spacepackets::ecss::tc::{PusTcCreator, PusTcReader};
//!
//! struct ConcretePusHandler {
//! handler_call_count: u32
//! }
//!
//! // This is a very simple possible service provider. It increments an internal call count field,
//! // which is used to verify the handler was called
//! impl PusServiceDistributor for ConcretePusHandler {
//! type Error = ();
//! fn distribute_packet(&mut self, service: u8, header: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
//! assert_eq!(service, 17);
//! assert_eq!(pus_tc.len_packed(), 13);
//! self.handler_call_count += 1;
//! Ok(())
//! }
//! }
//!
//! let service_handler = ConcretePusHandler {
//! handler_call_count: 0
//! };
//! let mut pus_distributor = PusDistributor::new(service_handler);
//!
//! // Create and pass PUS ping telecommand with a valid APID
//! let sp_header = SpHeader::new_for_unseg_tc(0x002, 0x34, 0);
//! let mut pus_tc = PusTcCreator::new_simple(sp_header, 17, 1, &[], true);
//! let mut test_buf: [u8; 32] = [0; 32];
//! let mut size = pus_tc
//! .write_to_bytes(test_buf.as_mut_slice())
//! .expect("Error writing TC to buffer");
//! let tc_slice = &test_buf[0..size];
//!
//! pus_distributor.pass_tc(tc_slice).expect("Passing PUS telecommand failed");
//!
//! // User helper function to retrieve concrete class. We check the call count here to verify
//! // that the PUS ping telecommand was routed successfully.
//! let concrete_handler = pus_distributor.service_distributor();
//! assert_eq!(concrete_handler.handler_call_count, 1);
//! ```
use crate::pus::ReceivesEcssPusTc;
use crate::tmtc::{ReceivesCcsdsTc, ReceivesTcCore};
use core::fmt::{Display, Formatter};
use spacepackets::ecss::tc::PusTcReader;
use spacepackets::ecss::{PusError, PusPacket};
use spacepackets::SpHeader;
#[cfg(feature = "std")]
use std::error::Error;
/// Trait for a generic distributor object which can distribute PUS packets based on packet
/// properties like the PUS service, space packet header or any other content of the PUS packet.
pub trait PusServiceDistributor {
type Error;
fn distribute_packet(
&mut self,
service: u8,
header: &SpHeader,
pus_tc: &PusTcReader,
) -> Result<(), Self::Error>;
}
/// Generic distributor object which dispatches received packets to a user provided handler.
pub struct PusDistributor<ServiceDistributor: PusServiceDistributor<Error = E>, E> {
service_distributor: ServiceDistributor,
}
impl<ServiceDistributor: PusServiceDistributor<Error = E>, E>
PusDistributor<ServiceDistributor, E>
{
pub fn new(service_provider: ServiceDistributor) -> Self {
PusDistributor {
service_distributor: service_provider,
}
}
}
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum PusDistribError<E> {
CustomError(E),
PusError(PusError),
}
impl<E: Display> Display for PusDistribError<E> {
fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
match self {
PusDistribError::CustomError(e) => write!(f, "pus distribution error: {e}"),
PusDistribError::PusError(e) => write!(f, "pus distribution error: {e}"),
}
}
}
#[cfg(feature = "std")]
impl<E: Error> Error for PusDistribError<E> {
fn source(&self) -> Option<&(dyn Error + 'static)> {
match self {
Self::CustomError(e) => e.source(),
Self::PusError(e) => e.source(),
}
}
}
impl<ServiceDistributor: PusServiceDistributor<Error = E>, E: 'static> ReceivesTcCore
for PusDistributor<ServiceDistributor, E>
{
type Error = PusDistribError<E>;
fn pass_tc(&mut self, tm_raw: &[u8]) -> Result<(), Self::Error> {
// Convert to ccsds and call pass_ccsds
let (sp_header, _) = SpHeader::from_be_bytes(tm_raw)
.map_err(|e| PusDistribError::PusError(PusError::ByteConversion(e)))?;
self.pass_ccsds(&sp_header, tm_raw)
}
}
impl<ServiceDistributor: PusServiceDistributor<Error = E>, E: 'static> ReceivesCcsdsTc
for PusDistributor<ServiceDistributor, E>
{
type Error = PusDistribError<E>;
fn pass_ccsds(&mut self, header: &SpHeader, tm_raw: &[u8]) -> Result<(), Self::Error> {
let (tc, _) = PusTcReader::new(tm_raw).map_err(|e| PusDistribError::PusError(e))?;
self.pass_pus_tc(header, &tc)
}
}
impl<ServiceDistributor: PusServiceDistributor<Error = E>, E: 'static> ReceivesEcssPusTc
for PusDistributor<ServiceDistributor, E>
{
type Error = PusDistribError<E>;
fn pass_pus_tc(&mut self, header: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
self.service_distributor
.distribute_packet(pus_tc.service(), header, pus_tc)
.map_err(|e| PusDistribError::CustomError(e))
}
}
impl<ServiceDistributor: PusServiceDistributor<Error = E>, E: 'static>
PusDistributor<ServiceDistributor, E>
{
pub fn service_distributor(&self) -> &ServiceDistributor {
&self.service_distributor
}
pub fn service_distributor_mut(&mut self) -> &mut ServiceDistributor {
&mut self.service_distributor
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::queue::GenericSendError;
use crate::tmtc::ccsds_distrib::tests::{
generate_ping_tc, generate_ping_tc_as_vec, BasicApidHandlerOwnedQueue,
BasicApidHandlerSharedQueue,
};
use crate::tmtc::ccsds_distrib::{CcsdsDistributor, CcsdsPacketHandler};
use crate::ValidatorU16Id;
use alloc::format;
use alloc::vec::Vec;
use spacepackets::ecss::PusError;
use spacepackets::CcsdsPacket;
#[cfg(feature = "std")]
use std::collections::VecDeque;
#[cfg(feature = "std")]
use std::sync::{Arc, Mutex};
fn is_send<T: Send>(_: &T) {}
pub struct PacketInfo {
pub service: u8,
pub apid: u16,
pub packet: Vec<u8>,
}
struct PusHandlerSharedQueue(Arc<Mutex<VecDeque<PacketInfo>>>);
#[derive(Default)]
struct PusHandlerOwnedQueue(VecDeque<PacketInfo>);
impl PusServiceDistributor for PusHandlerSharedQueue {
type Error = PusError;
fn distribute_packet(
&mut self,
service: u8,
sp_header: &SpHeader,
pus_tc: &PusTcReader,
) -> Result<(), Self::Error> {
let mut packet: Vec<u8> = Vec::new();
packet.extend_from_slice(pus_tc.raw_data());
self.0
.lock()
.expect("Mutex lock failed")
.push_back(PacketInfo {
service,
apid: sp_header.apid(),
packet,
});
Ok(())
}
}
impl PusServiceDistributor for PusHandlerOwnedQueue {
type Error = PusError;
fn distribute_packet(
&mut self,
service: u8,
sp_header: &SpHeader,
pus_tc: &PusTcReader,
) -> Result<(), Self::Error> {
let mut packet: Vec<u8> = Vec::new();
packet.extend_from_slice(pus_tc.raw_data());
self.0.push_back(PacketInfo {
service,
apid: sp_header.apid(),
packet,
});
Ok(())
}
}
struct ApidHandlerShared {
pub pus_distrib: PusDistributor<PusHandlerSharedQueue, PusError>,
pub handler_base: BasicApidHandlerSharedQueue,
}
struct ApidHandlerOwned {
pub pus_distrib: PusDistributor<PusHandlerOwnedQueue, PusError>,
handler_base: BasicApidHandlerOwnedQueue,
}
macro_rules! apid_handler_impl {
() => {
type Error = PusError;
fn handle_packet_with_valid_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
self.handler_base
.handle_packet_with_valid_apid(&sp_header, tc_raw)
.ok()
.expect("Unexpected error");
match self.pus_distrib.pass_ccsds(&sp_header, tc_raw) {
Ok(_) => Ok(()),
Err(e) => match e {
PusDistribError::CustomError(_) => Ok(()),
PusDistribError::PusError(e) => Err(e),
},
}
}
fn handle_packet_with_unknown_apid(
&mut self,
sp_header: &SpHeader,
tc_raw: &[u8],
) -> Result<(), Self::Error> {
self.handler_base
.handle_packet_with_unknown_apid(&sp_header, tc_raw)
.ok()
.expect("Unexpected error");
Ok(())
}
};
}
impl ValidatorU16Id for ApidHandlerOwned {
fn validate(&self, packet_id: u16) -> bool {
[0x000, 0x002].contains(&packet_id)
}
}
impl ValidatorU16Id for ApidHandlerShared {
fn validate(&self, packet_id: u16) -> bool {
[0x000, 0x002].contains(&packet_id)
}
}
impl CcsdsPacketHandler for ApidHandlerOwned {
apid_handler_impl!();
}
impl CcsdsPacketHandler for ApidHandlerShared {
apid_handler_impl!();
}
#[test]
fn test_pus_distribution_as_raw_packet() {
let mut pus_distrib = PusDistributor::new(PusHandlerOwnedQueue::default());
let tc = generate_ping_tc_as_vec();
let result = pus_distrib.pass_tc(&tc);
assert!(result.is_ok());
assert_eq!(pus_distrib.service_distributor_mut().0.len(), 1);
let packet_info = pus_distrib.service_distributor_mut().0.pop_front().unwrap();
assert_eq!(packet_info.service, 17);
assert_eq!(packet_info.apid, 0x002);
assert_eq!(packet_info.packet, tc);
}
#[test]
fn test_pus_distribution_combined_handler() {
let known_packet_queue = Arc::new(Mutex::default());
let unknown_packet_queue = Arc::new(Mutex::default());
let pus_queue = Arc::new(Mutex::default());
let pus_handler = PusHandlerSharedQueue(pus_queue.clone());
let handler_base = BasicApidHandlerSharedQueue {
known_packet_queue: known_packet_queue.clone(),
unknown_packet_queue: unknown_packet_queue.clone(),
};
let pus_distrib = PusDistributor::new(pus_handler);
is_send(&pus_distrib);
let apid_handler = ApidHandlerShared {
pus_distrib,
handler_base,
};
let mut ccsds_distrib = CcsdsDistributor::new(apid_handler);
let mut test_buf: [u8; 32] = [0; 32];
let tc_slice = generate_ping_tc(test_buf.as_mut_slice());
// Pass packet to distributor
ccsds_distrib
.pass_tc(tc_slice)
.expect("Passing TC slice failed");
let recvd_ccsds = known_packet_queue.lock().unwrap().pop_front();
assert!(unknown_packet_queue.lock().unwrap().is_empty());
assert!(recvd_ccsds.is_some());
let (apid, packet) = recvd_ccsds.unwrap();
assert_eq!(apid, 0x002);
assert_eq!(packet.as_slice(), tc_slice);
let recvd_pus = pus_queue.lock().unwrap().pop_front();
assert!(recvd_pus.is_some());
let packet_info = recvd_pus.unwrap();
assert_eq!(packet_info.service, 17);
assert_eq!(packet_info.apid, 0x002);
assert_eq!(packet_info.packet, tc_slice);
}
#[test]
fn test_accessing_combined_distributor() {
let pus_handler = PusHandlerOwnedQueue::default();
let handler_base = BasicApidHandlerOwnedQueue::default();
let pus_distrib = PusDistributor::new(pus_handler);
let apid_handler = ApidHandlerOwned {
pus_distrib,
handler_base,
};
let mut ccsds_distrib = CcsdsDistributor::new(apid_handler);
let mut test_buf: [u8; 32] = [0; 32];
let tc_slice = generate_ping_tc(test_buf.as_mut_slice());
ccsds_distrib
.pass_tc(tc_slice)
.expect("Passing TC slice failed");
let apid_handler_casted_back = ccsds_distrib.packet_handler_mut();
assert!(!apid_handler_casted_back
.handler_base
.known_packet_queue
.is_empty());
let handler_owned_queue = apid_handler_casted_back
.pus_distrib
.service_distributor_mut();
assert!(!handler_owned_queue.0.is_empty());
let packet_info = handler_owned_queue.0.pop_front().unwrap();
assert_eq!(packet_info.service, 17);
assert_eq!(packet_info.apid, 0x002);
assert_eq!(packet_info.packet, tc_slice);
}
#[test]
fn test_pus_distrib_error_custom_error() {
let error = PusDistribError::CustomError(GenericSendError::RxDisconnected);
let error_string = format!("{}", error);
assert_eq!(
error_string,
"pus distribution error: rx side has disconnected"
);
}
#[test]
fn test_pus_distrib_error_pus_error() {
let error = PusDistribError::<GenericSendError>::PusError(PusError::CrcCalculationMissing);
let error_string = format!("{}", error);
assert_eq!(
error_string,
"pus distribution error: crc16 was not calculated"
);
}
}

44
satrs/src/tmtc/tm_helper.rs
View File

@ -3,50 +3,6 @@ use spacepackets::time::cds::CdsTime;
use spacepackets::time::TimeWriter;
use spacepackets::SpHeader;
#[cfg(feature = "std")]
pub use std_mod::*;
#[cfg(feature = "std")]
pub mod std_mod {
use crate::pool::{
PoolProvider, SharedStaticMemoryPool, StaticMemoryPool, StoreAddr, StoreError,
};
use crate::pus::EcssTmtcError;
use spacepackets::ecss::tm::PusTmCreator;
use spacepackets::ecss::WritablePusPacket;
use std::sync::{Arc, RwLock};
#[derive(Clone)]
pub struct SharedTmPool(pub SharedStaticMemoryPool);
impl SharedTmPool {
pub fn new(shared_pool: StaticMemoryPool) -> Self {
Self(Arc::new(RwLock::new(shared_pool)))
}
pub fn clone_backing_pool(&self) -> SharedStaticMemoryPool {
self.0.clone()
}
pub fn shared_pool(&self) -> &SharedStaticMemoryPool {
&self.0
}
pub fn shared_pool_mut(&mut self) -> &mut SharedStaticMemoryPool {
&mut self.0
}
pub fn add_pus_tm(&self, pus_tm: &PusTmCreator) -> Result<StoreAddr, EcssTmtcError> {
let mut pg = self.0.write().map_err(|_| StoreError::LockError)?;
let addr = pg.free_element(pus_tm.len_written(), |buf| {
pus_tm
.write_to_bytes(buf)
.expect("writing PUS TM to store failed");
})?;
Ok(addr)
}
}
}
pub struct PusTmWithCdsShortHelper {
apid: u16,
cds_short_buf: [u8; 7],