2022-11-12 23:37:19 +01:00
|
|
|
use fsrc_core::events::EventU32;
|
2022-08-29 01:33:32 +02:00
|
|
|
use fsrc_core::hal::host::udp_server::{ReceiveResult, UdpTcServer};
|
2022-11-12 23:37:19 +01:00
|
|
|
use fsrc_core::params::Params;
|
2022-08-29 01:33:32 +02:00
|
|
|
use std::net::SocketAddr;
|
2022-09-10 13:34:04 +02:00
|
|
|
use std::sync::mpsc;
|
2022-11-12 23:37:19 +01:00
|
|
|
use std::sync::mpsc::Sender;
|
2022-08-29 01:33:32 +02:00
|
|
|
use std::thread;
|
|
|
|
use std::time::Duration;
|
|
|
|
|
2022-09-03 13:47:25 +02:00
|
|
|
use crate::ccsds::CcsdsReceiver;
|
|
|
|
use crate::pus::PusReceiver;
|
2022-08-29 01:33:32 +02:00
|
|
|
use crate::UdpTmtcServer;
|
2022-09-10 13:34:04 +02:00
|
|
|
use fsrc_core::pool::{SharedPool, StoreAddr};
|
2022-11-13 21:07:16 +01:00
|
|
|
use fsrc_core::pus::event_man::EventRequestWithToken;
|
2022-09-10 13:34:04 +02:00
|
|
|
use fsrc_core::pus::verification::SharedStdVerifReporterWithSender;
|
2022-09-03 13:47:25 +02:00
|
|
|
use fsrc_core::tmtc::{CcsdsDistributor, CcsdsError, PusDistributor};
|
2022-08-29 01:33:32 +02:00
|
|
|
use spacepackets::tm::PusTm;
|
|
|
|
|
|
|
|
pub const PUS_APID: u16 = 0x02;
|
|
|
|
|
2022-11-13 21:07:16 +01:00
|
|
|
pub struct CoreTmtcArgs {
|
|
|
|
pub tm_store: TmStore,
|
|
|
|
pub tm_sender: Sender<StoreAddr>,
|
|
|
|
pub event_sender: Sender<(EventU32, Option<Params>)>,
|
|
|
|
pub event_request_tx: Sender<EventRequestWithToken>,
|
|
|
|
}
|
|
|
|
|
2022-09-10 13:34:04 +02:00
|
|
|
#[derive(Clone)]
|
2022-08-29 01:33:32 +02:00
|
|
|
pub struct TmStore {
|
2022-09-10 13:34:04 +02:00
|
|
|
pub pool: SharedPool,
|
2022-08-29 01:33:32 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
impl TmStore {
|
2022-09-03 13:47:25 +02:00
|
|
|
pub fn add_pus_tm(&mut self, pus_tm: &PusTm) -> StoreAddr {
|
2022-09-10 13:34:04 +02:00
|
|
|
let mut pg = self.pool.write().expect("Error locking TM store");
|
|
|
|
let (addr, buf) = pg.free_element(pus_tm.len_packed()).expect("Store error");
|
2022-08-29 01:33:32 +02:00
|
|
|
pus_tm
|
2022-09-11 20:51:14 +02:00
|
|
|
.write_to_bytes(buf)
|
2022-08-29 01:33:32 +02:00
|
|
|
.expect("Writing PUS TM to store failed");
|
|
|
|
addr
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn core_tmtc_task(
|
2022-11-13 21:07:16 +01:00
|
|
|
args: CoreTmtcArgs,
|
2022-08-29 01:33:32 +02:00
|
|
|
tm_server_rx: mpsc::Receiver<StoreAddr>,
|
|
|
|
addr: SocketAddr,
|
2022-09-10 13:34:04 +02:00
|
|
|
verif_reporter: SharedStdVerifReporterWithSender,
|
2022-08-29 01:33:32 +02:00
|
|
|
) {
|
2022-09-10 13:34:04 +02:00
|
|
|
let pus_receiver = PusReceiver::new(
|
|
|
|
PUS_APID,
|
2022-11-13 21:07:16 +01:00
|
|
|
args.tm_sender,
|
|
|
|
args.tm_store.clone(),
|
2022-09-10 13:34:04 +02:00
|
|
|
verif_reporter,
|
2022-11-19 12:16:51 +01:00
|
|
|
args.event_request_tx,
|
2022-09-10 13:34:04 +02:00
|
|
|
);
|
2022-08-29 01:33:32 +02:00
|
|
|
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");
|
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_server_rx,
|
2022-11-13 21:07:16 +01:00
|
|
|
tm_store: args.tm_store.pool.clone(),
|
2022-08-29 01:33:32 +02:00
|
|
|
};
|
|
|
|
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
|
|
|
|
}
|
|
|
|
},
|
2022-09-11 15:23:11 +02:00
|
|
|
ReceiveResult::IoError(e) => {
|
2022-08-29 01:33:32 +02:00
|
|
|
println!("IO error {e}");
|
|
|
|
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() {
|
|
|
|
let mut store_lock = udp_tmtc_server
|
|
|
|
.tm_store
|
2022-09-10 13:34:04 +02:00
|
|
|
.write()
|
2022-08-29 01:33:32 +02:00
|
|
|
.expect("Locking TM store failed");
|
2022-09-10 13:34:04 +02:00
|
|
|
let pg = store_lock.read_with_guard(addr);
|
2022-08-29 01:33:32 +02:00
|
|
|
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");
|
|
|
|
}
|
|
|
|
}
|