sat-rs/satrs-example/src/tmtc.rs

229 lines
6.8 KiB
Rust
Raw Normal View History

2023-07-08 13:24:43 +02:00
use log::{info, warn};
2023-09-18 15:01:40 +02:00
use satrs_core::hal::std::udp_server::{ReceiveResult, UdpTcServer};
2022-08-29 01:33:32 +02:00
use std::net::SocketAddr;
use std::sync::mpsc::{Receiver, SendError, Sender, TryRecvError};
2022-08-29 01:33:32 +02:00
use std::thread;
use std::time::Duration;
2023-07-08 13:20:08 +02:00
use thiserror::Error;
2022-08-29 01:33:32 +02:00
2022-09-03 13:47:25 +02:00
use crate::ccsds::CcsdsReceiver;
2023-07-06 00:49:18 +02:00
use crate::pus::{PusReceiver, PusTcMpscRouter};
use satrs_core::pool::{SharedPool, StoreAddr, StoreError};
use satrs_core::pus::verification::StdVerifReporterWithSender;
2023-07-10 00:29:31 +02:00
use satrs_core::pus::{ReceivesEcssPusTc, TcAddrWithToken};
2023-07-11 00:44:48 +02:00
use satrs_core::spacepackets::ecss::tc::PusTcReader;
2023-07-11 00:28:28 +02:00
use satrs_core::spacepackets::ecss::PusPacket;
2023-07-05 11:25:23 +02:00
use satrs_core::spacepackets::SpHeader;
2023-07-04 15:17:43 +02:00
use satrs_core::tmtc::tm_helper::SharedTmStore;
2023-07-09 18:38:09 +02:00
use satrs_core::tmtc::{CcsdsDistributor, CcsdsError, ReceivesCcsdsTc};
2022-08-29 01:33:32 +02:00
pub struct TmArgs {
2023-07-04 15:17:43 +02:00
pub tm_store: SharedTmStore,
pub tm_sink_sender: Sender<StoreAddr>,
pub tm_server_rx: Receiver<StoreAddr>,
}
pub struct TcArgs {
pub tc_source: PusTcSource,
pub tc_receiver: Receiver<StoreAddr>,
}
2023-01-26 10:58:44 +01:00
impl TcArgs {
#[allow(dead_code)]
2023-01-26 10:58:44 +01:00
fn split(self) -> (PusTcSource, Receiver<StoreAddr>) {
(self.tc_source, self.tc_receiver)
}
}
2023-07-08 13:20:08 +02:00
#[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum MpscStoreAndSendError {
2023-07-08 13:20:08 +02:00
#[error("Store error: {0}")]
Store(#[from] StoreError),
#[error("TC send error: {0}")]
2023-07-10 00:29:31 +02:00
TcSend(#[from] SendError<TcAddrWithToken>),
2023-07-08 13:20:08 +02:00
#[error("TMTC send error: {0}")]
TmTcSend(#[from] SendError<StoreAddr>),
}
#[derive(Clone)]
pub struct TcStore {
pub pool: SharedPool,
}
impl TcStore {
2023-07-11 00:28:28 +02:00
pub fn add_pus_tc(&mut self, pus_tc: &PusTcReader) -> Result<StoreAddr, StoreError> {
let mut pg = self.pool.write().expect("error locking TC store");
let (addr, buf) = pg.free_element(pus_tc.len_packed())?;
2023-07-11 00:28:28 +02:00
buf[0..pus_tc.len_packed()].copy_from_slice(pus_tc.raw_data());
Ok(addr)
}
}
pub struct TmFunnel {
pub tm_funnel_rx: Receiver<StoreAddr>,
pub tm_server_tx: Sender<StoreAddr>,
}
pub struct UdpTmtcServer {
udp_tc_server: UdpTcServer<CcsdsError<MpscStoreAndSendError>>,
tm_rx: Receiver<StoreAddr>,
tm_store: SharedPool,
}
#[derive(Clone)]
pub struct PusTcSource {
pub tc_source: Sender<StoreAddr>,
pub tc_store: TcStore,
}
impl ReceivesEcssPusTc for PusTcSource {
type Error = MpscStoreAndSendError;
2023-07-10 01:27:37 +02:00
fn pass_pus_tc(&mut self, _: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
let addr = self.tc_store.add_pus_tc(pus_tc)?;
self.tc_source.send(addr)?;
Ok(())
}
}
impl ReceivesCcsdsTc for PusTcSource {
type Error = MpscStoreAndSendError;
fn pass_ccsds(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
let mut pool = self.tc_store.pool.write().expect("locking pool failed");
let addr = pool.add(tc_raw)?;
drop(pool);
self.tc_source.send(addr)?;
Ok(())
}
}
2023-02-04 15:37:35 +01:00
2023-07-04 18:51:54 +02:00
pub fn core_tmtc_task(
2023-07-06 00:49:18 +02:00
socket_addr: SocketAddr,
2023-07-04 18:51:54 +02:00
mut tc_args: TcArgs,
tm_args: TmArgs,
2023-07-06 00:49:18 +02:00
verif_reporter: StdVerifReporterWithSender,
2023-07-04 18:51:54 +02:00
pus_router: PusTcMpscRouter,
) {
2023-07-06 00:49:18 +02:00
let mut pus_receiver = PusReceiver::new(verif_reporter, pus_router);
2023-01-26 15:19:53 +01:00
2022-08-29 01:33:32 +02:00
let ccsds_receiver = CcsdsReceiver {
tc_source: tc_args.tc_source.clone(),
2022-08-29 01:33:32 +02:00
};
2023-01-26 15:19:53 +01:00
2022-08-29 01:33:32 +02:00
let ccsds_distributor = CcsdsDistributor::new(Box::new(ccsds_receiver));
2023-01-26 15:19:53 +01:00
2023-07-06 00:49:18 +02:00
let udp_tc_server = UdpTcServer::new(socket_addr, 2048, Box::new(ccsds_distributor))
2023-07-08 13:24:43 +02:00
.expect("creating UDP TMTC server failed");
2022-09-10 13:34:04 +02:00
2022-08-29 01:33:32 +02:00
let mut udp_tmtc_server = UdpTmtcServer {
udp_tc_server,
tm_rx: tm_args.tm_server_rx,
2023-07-10 00:29:31 +02:00
tm_store: tm_args.tm_store.clone_backing_pool(),
2022-08-29 01:33:32 +02:00
};
2023-01-26 10:58:44 +01:00
let mut tc_buf: [u8; 4096] = [0; 4096];
2022-08-29 01:33:32 +02:00
loop {
2023-01-26 10:58:44 +01:00
core_tmtc_loop(
&mut udp_tmtc_server,
2023-01-27 15:37:28 +01:00
&mut tc_args,
&mut tc_buf,
2023-01-26 10:58:44 +01:00
&mut pus_receiver,
);
2022-08-29 01:33:32 +02:00
thread::sleep(Duration::from_millis(400));
}
}
fn core_tmtc_loop(
udp_tmtc_server: &mut UdpTmtcServer,
2023-01-27 15:37:28 +01:00
tc_args: &mut TcArgs,
tc_buf: &mut [u8],
pus_receiver: &mut PusReceiver,
) {
while poll_tc_server(udp_tmtc_server) {}
2023-01-27 15:37:28 +01:00
match tc_args.tc_receiver.try_recv() {
Ok(addr) => {
let pool = tc_args
.tc_source
.tc_store
.pool
.read()
.expect("locking tc pool failed");
let data = pool.read(&addr).expect("reading pool failed");
tc_buf[0..data.len()].copy_from_slice(data);
drop(pool);
2023-07-11 00:44:48 +02:00
match PusTcReader::new(tc_buf) {
Ok((pus_tc, _)) => {
pus_receiver
2023-07-03 18:22:08 +02:00
.handle_tc_packet(addr, pus_tc.service(), &pus_tc)
.ok();
}
Err(e) => {
2023-07-08 13:24:43 +02:00
warn!("error creating PUS TC from raw data: {e}");
warn!("raw data: {tc_buf:x?}");
}
}
}
Err(e) => {
if let TryRecvError::Disconnected = e {
2023-07-08 13:24:43 +02:00
warn!("tmtc thread: sender disconnected")
}
}
}
2022-08-29 01:33:32 +02:00
if let Some(recv_addr) = udp_tmtc_server.udp_tc_server.last_sender() {
core_tm_handling(udp_tmtc_server, &recv_addr);
}
}
fn poll_tc_server(udp_tmtc_server: &mut UdpTmtcServer) -> bool {
2022-08-29 01:33:32 +02:00
match udp_tmtc_server.udp_tc_server.try_recv_tc() {
Ok(_) => true,
Err(e) => match e {
ReceiveResult::ReceiverError(e) => match e {
2023-01-27 00:25:51 +01:00
CcsdsError::ByteConversionError(e) => {
2023-07-08 13:24:43 +02:00
warn!("packet error: {e:?}");
2022-08-29 01:33:32 +02:00
true
}
2023-07-08 13:24:43 +02:00
CcsdsError::CustomError(e) => {
warn!("mpsc store and send error {e:?}");
2022-08-29 01:33:32 +02:00
true
}
},
2022-09-11 15:23:11 +02:00
ReceiveResult::IoError(e) => {
2023-07-08 13:24:43 +02:00
warn!("IO error {e}");
2022-08-29 01:33:32 +02:00
false
}
2022-09-11 16:20:30 +02:00
ReceiveResult::NothingReceived => false,
2022-08-29 01:33:32 +02:00
},
}
}
fn core_tm_handling(udp_tmtc_server: &mut UdpTmtcServer, recv_addr: &SocketAddr) {
while let Ok(addr) = udp_tmtc_server.tm_rx.try_recv() {
2023-07-08 13:37:27 +02:00
let store_lock = udp_tmtc_server.tm_store.write();
2023-07-08 13:28:53 +02:00
if store_lock.is_err() {
warn!("Locking TM store failed");
continue;
}
let mut store_lock = store_lock.unwrap();
2022-09-10 13:34:04 +02:00
let pg = store_lock.read_with_guard(addr);
2023-07-08 13:28:53 +02:00
let read_res = pg.read();
if read_res.is_err() {
warn!("Error reading TM pool data");
continue;
}
let buf = read_res.unwrap();
if buf.len() > 9 {
let service = buf[7];
let subservice = buf[8];
info!("Sending PUS TM[{service},{subservice}]")
} else {
info!("Sending PUS TM");
}
2023-07-08 13:26:49 +02:00
let result = udp_tmtc_server.udp_tc_server.socket.send_to(buf, recv_addr);
if let Err(e) = result {
warn!("Sending TM with UDP socket failed: {e}")
}
2022-08-29 01:33:32 +02:00
}
}