104 lines
3.2 KiB
Rust
104 lines
3.2 KiB
Rust
use fsrc_core::hal::host::udp_server::{ReceiveResult, UdpTcServer};
|
|
use std::net::SocketAddr;
|
|
use std::sync::{mpsc, Arc, Mutex};
|
|
use std::thread;
|
|
use std::time::Duration;
|
|
|
|
use crate::ccsds::CcsdsReceiver;
|
|
use crate::pus::PusReceiver;
|
|
use crate::UdpTmtcServer;
|
|
use fsrc_core::pool::{LocalPool, StoreAddr};
|
|
use fsrc_core::tmtc::{CcsdsDistributor, CcsdsError, PusDistributor};
|
|
use spacepackets::tm::PusTm;
|
|
|
|
pub const PUS_APID: u16 = 0x02;
|
|
|
|
pub struct TmStore {
|
|
pub pool: LocalPool,
|
|
}
|
|
|
|
impl TmStore {
|
|
pub fn add_pus_tm(&mut self, pus_tm: &PusTm) -> StoreAddr {
|
|
let (addr, buf) = self
|
|
.pool
|
|
.free_element(pus_tm.len_packed())
|
|
.expect("Store error");
|
|
pus_tm
|
|
.write_to(buf)
|
|
.expect("Writing PUS TM to store failed");
|
|
addr
|
|
}
|
|
}
|
|
|
|
pub fn core_tmtc_task(
|
|
tm_creator_tx: mpsc::Sender<StoreAddr>,
|
|
tm_server_rx: mpsc::Receiver<StoreAddr>,
|
|
tm_store: Arc<Mutex<TmStore>>,
|
|
addr: SocketAddr,
|
|
) {
|
|
let pus_receiver = PusReceiver::new(PUS_APID, tm_creator_tx, tm_store.clone());
|
|
let pus_distributor = PusDistributor::new(Box::new(pus_receiver));
|
|
let ccsds_receiver = CcsdsReceiver {
|
|
pus_handler: pus_distributor,
|
|
};
|
|
let ccsds_distributor = CcsdsDistributor::new(Box::new(ccsds_receiver));
|
|
let udp_tc_server = UdpTcServer::new(addr, 2048, Box::new(ccsds_distributor))
|
|
.expect("Creating UDP TMTC server failed");
|
|
let mut udp_tmtc_server = UdpTmtcServer {
|
|
udp_tc_server,
|
|
tm_rx: tm_server_rx,
|
|
tm_store,
|
|
};
|
|
loop {
|
|
core_tmtc_loop(&mut udp_tmtc_server);
|
|
thread::sleep(Duration::from_millis(400));
|
|
}
|
|
}
|
|
|
|
fn core_tmtc_loop(udp_tmtc_server: &mut UdpTmtcServer) {
|
|
while core_tc_handling(udp_tmtc_server) {}
|
|
if let Some(recv_addr) = udp_tmtc_server.udp_tc_server.last_sender() {
|
|
core_tm_handling(udp_tmtc_server, &recv_addr);
|
|
}
|
|
}
|
|
|
|
fn core_tc_handling(udp_tmtc_server: &mut UdpTmtcServer) -> bool {
|
|
match udp_tmtc_server.udp_tc_server.try_recv_tc() {
|
|
Ok(_) => true,
|
|
Err(e) => match e {
|
|
ReceiveResult::ReceiverError(e) => match e {
|
|
CcsdsError::PacketError(e) => {
|
|
println!("Got packet error: {e:?}");
|
|
true
|
|
}
|
|
CcsdsError::CustomError(_) => {
|
|
println!("Unknown receiver error");
|
|
true
|
|
}
|
|
},
|
|
ReceiveResult::OtherIoError(e) => {
|
|
println!("IO error {e}");
|
|
false
|
|
}
|
|
ReceiveResult::WouldBlock => false,
|
|
},
|
|
}
|
|
}
|
|
|
|
fn core_tm_handling(udp_tmtc_server: &mut UdpTmtcServer, recv_addr: &SocketAddr) {
|
|
while let Ok(addr) = udp_tmtc_server.tm_rx.try_recv() {
|
|
let mut store_lock = udp_tmtc_server
|
|
.tm_store
|
|
.lock()
|
|
.expect("Locking TM store failed");
|
|
let pg = store_lock.pool.read_with_guard(addr);
|
|
let buf = pg.read().expect("Error reading TM pool data");
|
|
println!("Sending TM");
|
|
udp_tmtc_server
|
|
.udp_tc_server
|
|
.socket
|
|
.send_to(buf, recv_addr)
|
|
.expect("Sending TM failed");
|
|
}
|
|
}
|