2023-09-29 14:11:03 +02:00
|
|
|
use log::warn;
|
2024-02-12 15:51:37 +01:00
|
|
|
use satrs::pus::{EcssTcAndToken, ReceivesEcssPusTc};
|
|
|
|
use satrs::spacepackets::SpHeader;
|
2024-02-07 18:10:47 +01:00
|
|
|
use std::sync::mpsc::{self, Receiver, SendError, Sender, TryRecvError};
|
2023-07-08 13:20:08 +02:00
|
|
|
use thiserror::Error;
|
2022-08-29 01:33:32 +02:00
|
|
|
|
2023-09-27 00:21:03 +02:00
|
|
|
use crate::pus::PusReceiver;
|
2024-02-12 15:51:37 +01:00
|
|
|
use satrs::pool::{PoolProvider, SharedStaticMemoryPool, StoreAddr, StoreError};
|
|
|
|
use satrs::spacepackets::ecss::tc::PusTcReader;
|
|
|
|
use satrs::spacepackets::ecss::PusPacket;
|
|
|
|
use satrs::tmtc::ReceivesCcsdsTc;
|
2022-08-29 01:33:32 +02:00
|
|
|
|
2023-07-08 13:20:08 +02:00
|
|
|
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
2022-12-21 22:14:42 +01:00
|
|
|
pub enum MpscStoreAndSendError {
|
2023-07-08 13:20:08 +02:00
|
|
|
#[error("Store error: {0}")]
|
|
|
|
Store(#[from] StoreError),
|
|
|
|
#[error("TC send error: {0}")]
|
2024-01-31 01:32:03 +01:00
|
|
|
TcSend(#[from] SendError<EcssTcAndToken>),
|
2023-07-08 13:20:08 +02:00
|
|
|
#[error("TMTC send error: {0}")]
|
|
|
|
TmTcSend(#[from] SendError<StoreAddr>),
|
2022-12-21 22:14:42 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
#[derive(Clone)]
|
2024-02-07 18:10:47 +01:00
|
|
|
pub struct SharedTcPool {
|
2024-02-03 13:41:51 +01:00
|
|
|
pub pool: SharedStaticMemoryPool,
|
2022-12-21 22:14:42 +01:00
|
|
|
}
|
|
|
|
|
2024-02-07 18:10:47 +01:00
|
|
|
impl SharedTcPool {
|
2023-07-11 00:28:28 +02:00
|
|
|
pub fn add_pus_tc(&mut self, pus_tc: &PusTcReader) -> Result<StoreAddr, StoreError> {
|
2022-12-21 22:14:42 +01:00
|
|
|
let mut pg = self.pool.write().expect("error locking TC store");
|
2024-02-10 11:59:26 +01:00
|
|
|
let addr = pg.free_element(pus_tc.len_packed(), |buf| {
|
|
|
|
buf[0..pus_tc.len_packed()].copy_from_slice(pus_tc.raw_data());
|
|
|
|
})?;
|
2022-12-21 22:14:42 +01:00
|
|
|
Ok(addr)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#[derive(Clone)]
|
2024-02-07 18:10:47 +01:00
|
|
|
pub struct PusTcSourceProviderSharedPool {
|
2022-12-21 22:14:42 +01:00
|
|
|
pub tc_source: Sender<StoreAddr>,
|
2024-02-07 18:10:47 +01:00
|
|
|
pub shared_pool: SharedTcPool,
|
|
|
|
}
|
|
|
|
|
|
|
|
impl PusTcSourceProviderSharedPool {
|
|
|
|
#[allow(dead_code)]
|
|
|
|
pub fn clone_backing_pool(&self) -> SharedStaticMemoryPool {
|
|
|
|
self.shared_pool.pool.clone()
|
|
|
|
}
|
2022-12-21 22:14:42 +01:00
|
|
|
}
|
|
|
|
|
2024-02-07 18:10:47 +01:00
|
|
|
impl ReceivesEcssPusTc for PusTcSourceProviderSharedPool {
|
2022-12-21 22:14:42 +01:00
|
|
|
type Error = MpscStoreAndSendError;
|
|
|
|
|
2023-07-10 01:27:37 +02:00
|
|
|
fn pass_pus_tc(&mut self, _: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
|
2024-02-07 18:10:47 +01:00
|
|
|
let addr = self.shared_pool.add_pus_tc(pus_tc)?;
|
2022-12-21 22:14:42 +01:00
|
|
|
self.tc_source.send(addr)?;
|
|
|
|
Ok(())
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2024-02-07 18:10:47 +01:00
|
|
|
impl ReceivesCcsdsTc for PusTcSourceProviderSharedPool {
|
2022-12-21 22:14:42 +01:00
|
|
|
type Error = MpscStoreAndSendError;
|
|
|
|
|
|
|
|
fn pass_ccsds(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
|
2024-02-07 18:10:47 +01:00
|
|
|
let mut pool = self.shared_pool.pool.write().expect("locking pool failed");
|
2022-12-21 22:14:42 +01:00
|
|
|
let addr = pool.add(tc_raw)?;
|
|
|
|
drop(pool);
|
|
|
|
self.tc_source.send(addr)?;
|
|
|
|
Ok(())
|
|
|
|
}
|
|
|
|
}
|
2023-02-04 15:37:35 +01:00
|
|
|
|
2024-02-07 18:10:47 +01:00
|
|
|
// Newtype, can not implement necessary traits on MPSC sender directly because of orphan rules.
|
|
|
|
#[derive(Clone)]
|
|
|
|
pub struct PusTcSourceProviderDynamic(pub Sender<Vec<u8>>);
|
|
|
|
|
|
|
|
impl ReceivesEcssPusTc for PusTcSourceProviderDynamic {
|
|
|
|
type Error = SendError<Vec<u8>>;
|
|
|
|
|
|
|
|
fn pass_pus_tc(&mut self, _: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
|
|
|
|
self.0.send(pus_tc.raw_data().to_vec())?;
|
|
|
|
Ok(())
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
impl ReceivesCcsdsTc for PusTcSourceProviderDynamic {
|
|
|
|
type Error = mpsc::SendError<Vec<u8>>;
|
|
|
|
|
|
|
|
fn pass_ccsds(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
|
|
|
|
self.0.send(tc_raw.to_vec())?;
|
|
|
|
Ok(())
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2024-02-08 17:42:36 +01:00
|
|
|
// TC source components where static pools are the backing memory of the received telecommands.
|
|
|
|
pub struct TcSourceTaskStatic {
|
|
|
|
shared_tc_pool: SharedTcPool,
|
|
|
|
tc_receiver: Receiver<StoreAddr>,
|
2023-09-27 00:21:03 +02:00
|
|
|
tc_buf: [u8; 4096],
|
|
|
|
pus_receiver: PusReceiver,
|
2022-08-29 01:33:32 +02:00
|
|
|
}
|
|
|
|
|
2024-02-08 17:42:36 +01:00
|
|
|
impl TcSourceTaskStatic {
|
|
|
|
pub fn new(
|
|
|
|
shared_tc_pool: SharedTcPool,
|
|
|
|
tc_receiver: Receiver<StoreAddr>,
|
|
|
|
pus_receiver: PusReceiver,
|
|
|
|
) -> Self {
|
2023-09-27 00:21:03 +02:00
|
|
|
Self {
|
2024-02-08 17:42:36 +01:00
|
|
|
shared_tc_pool,
|
|
|
|
tc_receiver,
|
2023-09-27 00:21:03 +02:00
|
|
|
tc_buf: [0; 4096],
|
|
|
|
pus_receiver,
|
2022-12-21 22:14:42 +01:00
|
|
|
}
|
|
|
|
}
|
2023-09-27 00:21:03 +02:00
|
|
|
|
|
|
|
pub fn periodic_operation(&mut self) {
|
|
|
|
self.poll_tc();
|
2022-08-29 01:33:32 +02:00
|
|
|
}
|
|
|
|
|
2023-09-27 00:21:03 +02:00
|
|
|
pub fn poll_tc(&mut self) -> bool {
|
2024-02-08 17:42:36 +01:00
|
|
|
match self.tc_receiver.try_recv() {
|
2023-09-27 00:21:03 +02:00
|
|
|
Ok(addr) => {
|
|
|
|
let pool = self
|
2024-02-08 17:42:36 +01:00
|
|
|
.shared_tc_pool
|
2023-09-27 00:21:03 +02:00
|
|
|
.pool
|
|
|
|
.read()
|
|
|
|
.expect("locking tc pool failed");
|
2024-02-10 11:59:26 +01:00
|
|
|
pool.read(&addr, &mut self.tc_buf)
|
|
|
|
.expect("reading pool failed");
|
2023-09-27 00:21:03 +02:00
|
|
|
drop(pool);
|
|
|
|
match PusTcReader::new(&self.tc_buf) {
|
|
|
|
Ok((pus_tc, _)) => {
|
|
|
|
self.pus_receiver
|
2024-01-31 01:32:03 +01:00
|
|
|
.handle_tc_packet(
|
2024-02-12 15:51:37 +01:00
|
|
|
satrs::pus::TcInMemory::StoreAddr(addr),
|
2024-01-31 01:32:03 +01:00
|
|
|
pus_tc.service(),
|
|
|
|
&pus_tc,
|
|
|
|
)
|
2023-09-27 00:21:03 +02:00
|
|
|
.ok();
|
|
|
|
true
|
|
|
|
}
|
|
|
|
Err(e) => {
|
|
|
|
warn!("error creating PUS TC from raw data: {e}");
|
|
|
|
warn!("raw data: {:x?}", self.tc_buf);
|
|
|
|
true
|
|
|
|
}
|
2022-08-29 01:33:32 +02:00
|
|
|
}
|
2023-09-27 00:21:03 +02:00
|
|
|
}
|
|
|
|
Err(e) => match e {
|
|
|
|
TryRecvError::Empty => false,
|
|
|
|
TryRecvError::Disconnected => {
|
|
|
|
warn!("tmtc thread: sender disconnected");
|
|
|
|
false
|
2022-08-29 01:33:32 +02:00
|
|
|
}
|
|
|
|
},
|
2023-09-27 00:21:03 +02:00
|
|
|
}
|
2022-08-29 01:33:32 +02:00
|
|
|
}
|
|
|
|
}
|
2024-02-07 18:10:47 +01:00
|
|
|
|
2024-02-08 17:42:36 +01:00
|
|
|
// TC source components where the heap is the backing memory of the received telecommands.
|
|
|
|
pub struct TcSourceTaskDynamic {
|
2024-02-07 18:10:47 +01:00
|
|
|
pub tc_receiver: Receiver<Vec<u8>>,
|
|
|
|
pus_receiver: PusReceiver,
|
|
|
|
}
|
|
|
|
|
2024-02-08 17:42:36 +01:00
|
|
|
impl TcSourceTaskDynamic {
|
2024-02-07 18:10:47 +01:00
|
|
|
pub fn new(tc_receiver: Receiver<Vec<u8>>, pus_receiver: PusReceiver) -> Self {
|
|
|
|
Self {
|
|
|
|
tc_receiver,
|
|
|
|
pus_receiver,
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn periodic_operation(&mut self) {
|
|
|
|
self.poll_tc();
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn poll_tc(&mut self) -> bool {
|
|
|
|
match self.tc_receiver.try_recv() {
|
|
|
|
Ok(tc) => match PusTcReader::new(&tc) {
|
|
|
|
Ok((pus_tc, _)) => {
|
|
|
|
self.pus_receiver
|
|
|
|
.handle_tc_packet(
|
2024-02-12 15:51:37 +01:00
|
|
|
satrs::pus::TcInMemory::Vec(tc.clone()),
|
2024-02-07 18:10:47 +01:00
|
|
|
pus_tc.service(),
|
|
|
|
&pus_tc,
|
|
|
|
)
|
|
|
|
.ok();
|
|
|
|
true
|
|
|
|
}
|
|
|
|
Err(e) => {
|
|
|
|
warn!("error creating PUS TC from raw data: {e}");
|
|
|
|
warn!("raw data: {:x?}", tc);
|
|
|
|
true
|
|
|
|
}
|
|
|
|
},
|
|
|
|
Err(e) => match e {
|
|
|
|
TryRecvError::Empty => false,
|
|
|
|
TryRecvError::Disconnected => {
|
|
|
|
warn!("tmtc thread: sender disconnected");
|
|
|
|
false
|
|
|
|
}
|
|
|
|
},
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|