continuing obsw example
This commit is contained in:
parent
6ab691e87c
commit
18b417bddc
@ -1,6 +1,6 @@
|
||||
use crate::hal::host::udp_server::ReceiveResult::{IoError, ReceiverError};
|
||||
use crate::tmtc::ReceivesTc;
|
||||
use std::boxed::Box;
|
||||
use std::io::ErrorKind;
|
||||
use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
|
||||
use std::vec;
|
||||
use std::vec::Vec;
|
||||
@ -14,7 +14,8 @@ pub struct UdpTcServer<E> {
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum ReceiveResult<E> {
|
||||
IoError(std::io::Error),
|
||||
WouldBlock,
|
||||
OtherIoError(std::io::Error),
|
||||
ReceiverError(E),
|
||||
}
|
||||
|
||||
@ -24,23 +25,36 @@ impl<E> UdpTcServer<E> {
|
||||
max_recv_size: usize,
|
||||
tc_receiver: Box<dyn ReceivesTc<Error = E>>,
|
||||
) -> Result<Self, std::io::Error> {
|
||||
Ok(Self {
|
||||
let server = Self {
|
||||
socket: UdpSocket::bind(addr)?,
|
||||
recv_buf: vec![0; max_recv_size],
|
||||
sender_addr: None,
|
||||
tc_receiver,
|
||||
})
|
||||
};
|
||||
server
|
||||
.socket
|
||||
.set_nonblocking(true)
|
||||
.expect("Setting server non blocking failed");
|
||||
Ok(server)
|
||||
}
|
||||
|
||||
pub fn recv_tc(&mut self) -> Result<(usize, SocketAddr), ReceiveResult<E>> {
|
||||
let res = self
|
||||
.socket
|
||||
.recv_from(&mut self.recv_buf)
|
||||
.map_err(|e| IoError(e))?;
|
||||
self.sender_addr = Some(res.1);
|
||||
pub fn try_recv_tc(&mut self) -> Result<(usize, SocketAddr), ReceiveResult<E>> {
|
||||
// .map_err(|e| IoError(e))?;
|
||||
let res = match self.socket.recv_from(&mut self.recv_buf) {
|
||||
Ok(res) => res,
|
||||
Err(e) => {
|
||||
if e.kind() != ErrorKind::WouldBlock {
|
||||
return Err(ReceiveResult::WouldBlock);
|
||||
} else {
|
||||
return Err(ReceiveResult::OtherIoError(e));
|
||||
}
|
||||
}
|
||||
};
|
||||
let (num_bytes, from) = res;
|
||||
self.sender_addr = Some(from);
|
||||
self.tc_receiver
|
||||
.pass_tc(&self.recv_buf[0..res.0])
|
||||
.map_err(|e| ReceiverError(e))?;
|
||||
.pass_tc(&self.recv_buf[0..num_bytes])
|
||||
.map_err(|e| ReceiveResult::ReceiverError(e))?;
|
||||
Ok(res)
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,17 +1,20 @@
|
||||
use fsrc_core::hal::host::udp_server::{ReceiveResult, UdpTcServer};
|
||||
use fsrc_core::pool::{LocalPool, PoolCfg, StoreAddr};
|
||||
use fsrc_core::tmtc::{
|
||||
CcsdsDistributor, CcsdsError, CcsdsPacketHandler, PusDistributor, PusServiceProvider,
|
||||
ReceivesCcsdsTc,
|
||||
};
|
||||
use fsrc_example::{OBSW_SERVER_ADDR, SERVER_PORT};
|
||||
use spacepackets::tc::PusTc;
|
||||
use spacepackets::time::{CdsShortTimeProvider, TimeWriter};
|
||||
use spacepackets::tm::{PusTm, PusTmSecondaryHeader};
|
||||
use spacepackets::{CcsdsPacket, SpHeader};
|
||||
use std::net::{IpAddr, SocketAddr};
|
||||
use std::sync::{mpsc, Arc, Mutex};
|
||||
use std::thread;
|
||||
|
||||
const PUS_APID: u16 = 0x02;
|
||||
|
||||
struct PusReceiver {}
|
||||
|
||||
struct CcsdsReceiver {
|
||||
pus_handler: PusDistributor<()>,
|
||||
}
|
||||
@ -46,6 +49,14 @@ impl CcsdsPacketHandler for CcsdsReceiver {
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for CcsdsReceiver {}
|
||||
|
||||
struct PusReceiver {
|
||||
tm_apid: u16,
|
||||
tm_tx: mpsc::Sender<StoreAddr>,
|
||||
tm_store: Arc<Mutex<TmStore>>,
|
||||
}
|
||||
|
||||
impl PusServiceProvider for PusReceiver {
|
||||
type Error = ();
|
||||
|
||||
@ -59,38 +70,120 @@ impl PusServiceProvider for PusReceiver {
|
||||
println!("Received PUS ping command");
|
||||
let raw_data = pus_tc.raw().expect("Could not retrieve raw data");
|
||||
println!("Raw data: 0x{raw_data:x?}");
|
||||
let mut reply_header = SpHeader::tm(self.tm_apid, 0, 0).unwrap();
|
||||
let time_stamp = CdsShortTimeProvider::from_now().unwrap();
|
||||
let mut timestamp_buf = [0; 10];
|
||||
time_stamp.write_to_bytes(&mut timestamp_buf).unwrap();
|
||||
let tc_header = PusTmSecondaryHeader::new_simple(17, 2, ×tamp_buf);
|
||||
let ping_reply = PusTm::new(&mut reply_header, tc_header, None, true);
|
||||
let addr = self
|
||||
.tm_store
|
||||
.lock()
|
||||
.expect("Locking TM store failed")
|
||||
.add_pus_tm(&ping_reply);
|
||||
self.tm_tx
|
||||
.send(addr)
|
||||
.expect("Sending TM to TM funnel failed");
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
|
||||
let pus_receiver = PusReceiver {};
|
||||
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 mut udp_tmtc_server = UdpTcServer::new(addr, 2048, Box::new(ccsds_distributor))
|
||||
.expect("Creating UDP TMTC server failed");
|
||||
loop {
|
||||
let res = udp_tmtc_server.recv_tc();
|
||||
match res {
|
||||
Ok(_) => (),
|
||||
Err(e) => match e {
|
||||
ReceiveResult::ReceiverError(e) => match e {
|
||||
CcsdsError::PacketError(e) => {
|
||||
println!("Got packet error: {e:?}");
|
||||
}
|
||||
CcsdsError::CustomError(_) => {
|
||||
println!("Unknown receiver error")
|
||||
}
|
||||
},
|
||||
ReceiveResult::IoError(e) => {
|
||||
println!("IO error {e}");
|
||||
}
|
||||
},
|
||||
}
|
||||
struct TmStore {
|
||||
pool: LocalPool,
|
||||
}
|
||||
|
||||
impl TmStore {
|
||||
fn add_pus_tm(&mut self, pus_tm: &PusTm) -> StoreAddr {
|
||||
let (addr, mut buf) = self
|
||||
.pool
|
||||
.free_element(pus_tm.len_packed())
|
||||
.expect("Store error");
|
||||
pus_tm
|
||||
.write_to(&mut buf)
|
||||
.expect("Writing PUS TM to store failed");
|
||||
addr
|
||||
}
|
||||
}
|
||||
|
||||
struct TmFunnel {
|
||||
tm_funnel_rx: mpsc::Receiver<StoreAddr>,
|
||||
tm_server_tx: mpsc::Sender<StoreAddr>,
|
||||
}
|
||||
|
||||
struct UdpTmtcServer {
|
||||
udp_tc_server: UdpTcServer<CcsdsError<()>>,
|
||||
tm_rx: mpsc::Receiver<StoreAddr>,
|
||||
tm_store: Arc<Mutex<TmStore>>,
|
||||
}
|
||||
|
||||
unsafe impl Send for UdpTmtcServer {}
|
||||
|
||||
fn main() {
|
||||
let pool_cfg = PoolCfg::new(vec![(8, 32), (4, 64), (2, 128)]);
|
||||
let tm_pool = LocalPool::new(pool_cfg);
|
||||
let tm_store = Arc::new(Mutex::new(TmStore { pool: tm_pool }));
|
||||
let addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
|
||||
let (tm_creator_tx, tm_funnel_rx) = mpsc::channel();
|
||||
let (tm_server_tx, tm_server_rx) = mpsc::channel();
|
||||
|
||||
let jh0 = thread::spawn(move || {
|
||||
let pus_receiver = PusReceiver {
|
||||
tm_apid: PUS_APID,
|
||||
tm_tx: tm_creator_tx.clone(),
|
||||
tm_store: 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: tm_store.clone(),
|
||||
};
|
||||
loop {
|
||||
let res = udp_tmtc_server.udp_tc_server.try_recv_tc();
|
||||
match res {
|
||||
Ok(_) => (),
|
||||
Err(e) => match e {
|
||||
ReceiveResult::ReceiverError(e) => match e {
|
||||
CcsdsError::PacketError(e) => {
|
||||
println!("Got packet error: {e:?}");
|
||||
}
|
||||
CcsdsError::CustomError(_) => {
|
||||
println!("Unknown receiver error")
|
||||
}
|
||||
},
|
||||
ReceiveResult::OtherIoError(e) => {
|
||||
println!("IO error {e}");
|
||||
}
|
||||
ReceiveResult::WouldBlock => {
|
||||
// TODO: Send TM Here
|
||||
}
|
||||
},
|
||||
}
|
||||
}
|
||||
});
|
||||
let jh1 = thread::spawn(move || {
|
||||
let tm_funnel = TmFunnel {
|
||||
tm_server_tx,
|
||||
tm_funnel_rx,
|
||||
};
|
||||
loop {
|
||||
let res = tm_funnel.tm_funnel_rx.recv();
|
||||
if res.is_ok() {
|
||||
let addr = res.unwrap();
|
||||
tm_funnel
|
||||
.tm_server_tx
|
||||
.send(addr)
|
||||
.expect("Sending TM to server failed");
|
||||
}
|
||||
}
|
||||
});
|
||||
jh0.join().expect("Joining UDP TMTC server thread failed");
|
||||
jh1.join().expect("Joining TM Funnel thread failed");
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user