Some checks failed
Rust/sat-rs/pipeline/pr-main There was a failure building this commit
77 lines
2.5 KiB
Rust
77 lines
2.5 KiB
Rust
use std::{net::SocketAddr, sync::mpsc::Receiver};
|
|
|
|
use log::{info, warn};
|
|
use satrs_core::{
|
|
hal::std::udp_server::{ReceiveResult, UdpTcServer},
|
|
pool::{SharedPool, StoreAddr},
|
|
tmtc::CcsdsError,
|
|
};
|
|
|
|
use crate::tmtc::MpscStoreAndSendError;
|
|
|
|
pub struct UdpTmtcServer {
|
|
pub udp_tc_server: UdpTcServer<CcsdsError<MpscStoreAndSendError>>,
|
|
pub tm_rx: Receiver<StoreAddr>,
|
|
pub tm_store: SharedPool,
|
|
}
|
|
impl UdpTmtcServer {
|
|
pub fn periodic_operation(&mut self) {
|
|
while self.poll_tc_server() {}
|
|
if let Some(recv_addr) = self.udp_tc_server.last_sender() {
|
|
self.send_tm_to_udp_client(&recv_addr);
|
|
}
|
|
}
|
|
|
|
fn poll_tc_server(&mut self) -> bool {
|
|
match self.udp_tc_server.try_recv_tc() {
|
|
Ok(_) => true,
|
|
Err(e) => match e {
|
|
ReceiveResult::ReceiverError(e) => match e {
|
|
CcsdsError::ByteConversionError(e) => {
|
|
warn!("packet error: {e:?}");
|
|
true
|
|
}
|
|
CcsdsError::CustomError(e) => {
|
|
warn!("mpsc store and send error {e:?}");
|
|
true
|
|
}
|
|
},
|
|
ReceiveResult::IoError(e) => {
|
|
warn!("IO error {e}");
|
|
false
|
|
}
|
|
ReceiveResult::NothingReceived => false,
|
|
},
|
|
}
|
|
}
|
|
|
|
fn send_tm_to_udp_client(&mut self, recv_addr: &SocketAddr) {
|
|
while let Ok(addr) = self.tm_rx.try_recv() {
|
|
let store_lock = self.tm_store.write();
|
|
if store_lock.is_err() {
|
|
warn!("Locking TM store failed");
|
|
continue;
|
|
}
|
|
let mut store_lock = store_lock.unwrap();
|
|
let pg = store_lock.read_with_guard(addr);
|
|
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");
|
|
}
|
|
let result = self.udp_tc_server.socket.send_to(buf, recv_addr);
|
|
if let Err(e) = result {
|
|
warn!("Sending TM with UDP socket failed: {e}")
|
|
}
|
|
}
|
|
}
|
|
}
|