some things in rust are really hard
This commit is contained in:
parent
17c5b3d93e
commit
2ba93b9942
@ -64,7 +64,7 @@ impl PusTmWithCdsShortHelper {
|
||||
self.create_pus_tm_common(service, subservice, source_data, seq_count)
|
||||
}
|
||||
|
||||
pub fn create_pus_tm_with_stamp<'a>(
|
||||
pub fn create_pus_tm_with_stamper<'a>(
|
||||
&'a mut self,
|
||||
service: u8,
|
||||
subservice: u8,
|
||||
@ -76,6 +76,17 @@ impl PusTmWithCdsShortHelper {
|
||||
self.create_pus_tm_common(service, subservice, source_data, seq_count)
|
||||
}
|
||||
|
||||
pub fn create_pus_tm_with_stamp<'a>(
|
||||
&'a mut self,
|
||||
service: u8,
|
||||
subservice: u8,
|
||||
source_data: Option<&'a [u8]>,
|
||||
timestamp: &'a [u8],
|
||||
seq_count: u16,
|
||||
) -> PusTm {
|
||||
self.create_pus_tm_common(service, subservice, source_data, seq_count)
|
||||
}
|
||||
|
||||
fn create_pus_tm_common<'a>(
|
||||
&'a self,
|
||||
service: u8,
|
||||
|
@ -170,9 +170,9 @@ fn main() {
|
||||
let mut pus17_handler = PusService17TestHandler::new(
|
||||
pus_test_rx,
|
||||
tc_store.pool.clone(),
|
||||
PusTmWithCdsShortHelper::new(PUS_APID),
|
||||
tm_funnel_tx.clone(),
|
||||
tm_store.clone(),
|
||||
PUS_APID,
|
||||
verif_reporter.clone(),
|
||||
);
|
||||
let mut srv_17_wrapper = Service17CustomWrapper {
|
||||
@ -345,7 +345,7 @@ fn main() {
|
||||
.spawn(move || loop {
|
||||
let queue_empty = srv_17_wrapper.perform_operation();
|
||||
if queue_empty {
|
||||
thread::sleep(Duration::from_millis(400));
|
||||
thread::sleep(Duration::from_millis(200));
|
||||
}
|
||||
})
|
||||
.unwrap();
|
||||
|
@ -11,7 +11,7 @@ use satrs_core::pus::hk;
|
||||
use satrs_core::pus::mode::Subservice;
|
||||
use satrs_core::pus::scheduling::PusScheduler;
|
||||
use satrs_core::pus::verification::{
|
||||
pus_11_generic_tc_check, FailParams, StdVerifReporterWithSender, TcStateAccepted,
|
||||
pus_11_generic_tc_check, FailParams, StdVerifReporterWithSender, TcStateAccepted, TcStateToken,
|
||||
VerificationToken,
|
||||
};
|
||||
use satrs_core::pus::{event, EcssTcSenderCore, GenericTcCheckError, MpscTmtcInStoreSender};
|
||||
@ -19,7 +19,7 @@ use satrs_core::pus::{mode, EcssTcSender};
|
||||
use satrs_core::res_code::ResultU16;
|
||||
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
|
||||
use satrs_core::spacepackets::ecss::{scheduling, PusError, PusServiceId};
|
||||
use satrs_core::spacepackets::time::{CcsdsTimeProvider, StdTimestampError};
|
||||
use satrs_core::spacepackets::time::{CcsdsTimeProvider, StdTimestampError, TimestampError};
|
||||
use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
|
||||
use satrs_core::tmtc::{AddressableId, PusServiceProvider, TargetId};
|
||||
use satrs_core::{
|
||||
@ -31,7 +31,7 @@ use std::cell::RefCell;
|
||||
use std::collections::HashMap;
|
||||
use std::convert::TryFrom;
|
||||
use std::rc::Rc;
|
||||
use std::sync::mpsc::{Receiver, SendError, Sender};
|
||||
use std::sync::mpsc::{Receiver, SendError, Sender, TryRecvError};
|
||||
|
||||
pub mod scheduler;
|
||||
pub mod test;
|
||||
@ -40,10 +40,9 @@ pub mod test;
|
||||
pub enum PusPacketHandlingError {
|
||||
PusError(PusError),
|
||||
WrongService(u8),
|
||||
NotEnoughAppData(String),
|
||||
StoreError(StoreError),
|
||||
RwGuardError(String),
|
||||
TimeError(StdTimestampError),
|
||||
TmSendError(String),
|
||||
QueueDisconnected,
|
||||
OtherError(String),
|
||||
}
|
||||
@ -54,24 +53,44 @@ impl From<PusError> for PusPacketHandlingError {
|
||||
}
|
||||
}
|
||||
|
||||
impl From<StdTimestampError> for PusPacketHandlingError {
|
||||
impl From<StoreError> for PusPacketHandlingError {
|
||||
fn from(value: StoreError) -> Self {
|
||||
Self::StoreError(value)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum PartialPusHandlingError {
|
||||
TimeError(StdTimestampError),
|
||||
TmSendError(String),
|
||||
VerificationError,
|
||||
}
|
||||
impl From<StdTimestampError> for PartialPusHandlingError {
|
||||
fn from(value: StdTimestampError) -> Self {
|
||||
Self::TimeError(value)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
|
||||
impl From<TimestampError> for PartialPusHandlingError {
|
||||
fn from(value: TimestampError) -> Self {
|
||||
Self::TimeError(StdTimestampError::TimestampError(value))
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum PusPacketHandlerResult {
|
||||
RequestHandled,
|
||||
RequestHandledPartialSuccess(PartialPusHandlingError),
|
||||
CustomSubservice(VerificationToken<TcStateAccepted>),
|
||||
Empty,
|
||||
}
|
||||
|
||||
pub struct PusServiceBase {
|
||||
tc_rx: Receiver<AcceptedTc>,
|
||||
tc_store: SharedPool,
|
||||
tm_helper: PusTmWithCdsShortHelper,
|
||||
tm_tx: Sender<StoreAddr>,
|
||||
tm_store: SharedTmStore,
|
||||
tm_apid: u16,
|
||||
verification_handler: StdVerifReporterWithSender,
|
||||
stamp_buf: [u8; 7],
|
||||
pus_buf: [u8; 2048],
|
||||
@ -82,15 +101,15 @@ impl PusServiceBase {
|
||||
pub fn new(
|
||||
receiver: Receiver<AcceptedTc>,
|
||||
tc_pool: SharedPool,
|
||||
tm_helper: PusTmWithCdsShortHelper,
|
||||
tm_tx: Sender<StoreAddr>,
|
||||
tm_store: SharedTmStore,
|
||||
tm_apid: u16,
|
||||
verification_handler: StdVerifReporterWithSender,
|
||||
) -> Self {
|
||||
Self {
|
||||
tc_rx: receiver,
|
||||
tc_store: tc_pool,
|
||||
tm_helper,
|
||||
tm_apid,
|
||||
tm_tx,
|
||||
tm_store,
|
||||
verification_handler,
|
||||
@ -99,6 +118,40 @@ impl PusServiceBase {
|
||||
pus_size: 0,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn handle_next_packet<
|
||||
T: FnOnce(
|
||||
StoreAddr,
|
||||
VerificationToken<TcStateAccepted>,
|
||||
) -> Result<PusPacketHandlerResult, PusPacketHandlingError>,
|
||||
>(
|
||||
&mut self,
|
||||
handle_one_packet: T,
|
||||
) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
||||
return match self.tc_rx.try_recv() {
|
||||
Ok((addr, token)) => handle_one_packet(addr, token),
|
||||
Err(e) => match e {
|
||||
TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
|
||||
TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
pub fn update_stamp(&mut self) -> Result<(), PartialPusHandlingError> {
|
||||
let time_provider = TimeProvider::from_now_with_u16_days()
|
||||
.map_err(|e| PartialPusHandlingError::TimeError(e));
|
||||
return if time_provider.is_ok() {
|
||||
// Can not fail, buffer is large enough.
|
||||
time_provider
|
||||
.unwrap()
|
||||
.write_to_bytes(&mut self.stamp_buf)
|
||||
.unwrap();
|
||||
Ok(())
|
||||
} else {
|
||||
self.stamp_buf = [0; 7];
|
||||
Err(time_provider.unwrap_err())
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
// pub trait PusTcRouter {
|
||||
|
@ -1,4 +1,7 @@
|
||||
use crate::pus::{AcceptedTc, PusPacketHandlerResult, PusPacketHandlingError, PusServiceBase};
|
||||
use crate::pus::{
|
||||
AcceptedTc, PartialPusHandlingError, PusPacketHandlerResult, PusPacketHandlingError,
|
||||
PusServiceBase,
|
||||
};
|
||||
use delegate::delegate;
|
||||
use satrs_core::pool::{SharedPool, StoreAddr};
|
||||
use satrs_core::pus::scheduling::PusScheduler;
|
||||
@ -7,9 +10,10 @@ use satrs_core::pus::verification::{
|
||||
VerificationToken,
|
||||
};
|
||||
use satrs_core::pus::GenericTcCheckError;
|
||||
use satrs_core::spacepackets::ecss::scheduling;
|
||||
use satrs_core::spacepackets::ecss::{scheduling, PusPacket};
|
||||
use satrs_core::spacepackets::tc::PusTc;
|
||||
use satrs_core::spacepackets::time::cds::TimeProvider;
|
||||
use satrs_core::spacepackets::time::TimeWriter;
|
||||
use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
|
||||
use satrs_example::tmtc_err;
|
||||
use std::sync::mpsc::{Receiver, Sender, TryRecvError};
|
||||
@ -23,9 +27,9 @@ impl PusService11SchedHandler {
|
||||
pub fn new(
|
||||
receiver: Receiver<AcceptedTc>,
|
||||
tc_pool: SharedPool,
|
||||
tm_helper: PusTmWithCdsShortHelper,
|
||||
tm_tx: Sender<StoreAddr>,
|
||||
tm_store: SharedTmStore,
|
||||
tm_apid: u16,
|
||||
verification_handler: StdVerifReporterWithSender,
|
||||
scheduler: PusScheduler,
|
||||
) -> Self {
|
||||
@ -33,9 +37,9 @@ impl PusService11SchedHandler {
|
||||
psb: PusServiceBase::new(
|
||||
receiver,
|
||||
tc_pool,
|
||||
tm_helper,
|
||||
tm_tx,
|
||||
tm_store,
|
||||
tm_apid,
|
||||
verification_handler,
|
||||
),
|
||||
scheduler,
|
||||
@ -44,68 +48,36 @@ impl PusService11SchedHandler {
|
||||
|
||||
pub fn handle_next_packet(&mut self) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
||||
return match self.psb.tc_rx.try_recv() {
|
||||
Ok((addr, token)) => {
|
||||
if self.handle_one_tc(addr, token)? {
|
||||
return Ok(PusPacketHandlerResult::RequestHandled);
|
||||
}
|
||||
Ok(PusPacketHandlerResult::CustomSubservice(token))
|
||||
}
|
||||
Ok((addr, token)) => self.handle_one_tc(addr, token),
|
||||
Err(e) => match e {
|
||||
TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
|
||||
TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
pub fn handle_one_tc(
|
||||
&mut self,
|
||||
addr: StoreAddr,
|
||||
token: VerificationToken<TcStateAccepted>,
|
||||
) -> Result<bool, PusPacketHandlingError> {
|
||||
let time_provider = TimeProvider::from_now_with_u16_days().unwrap();
|
||||
// TODO: Better error handling
|
||||
) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
||||
let mut partial_result = self.psb.update_stamp().err();
|
||||
{
|
||||
// Keep locked section as short as possible.
|
||||
let mut tc_pool = self.psb.tc_store.write().unwrap();
|
||||
let mut tc_pool = self
|
||||
.psb
|
||||
.tc_store
|
||||
.write()
|
||||
.map_err(|e| PusPacketHandlingError::RwGuardError(format!("{e}")))?;
|
||||
let tc_guard = tc_pool.read_with_guard(addr);
|
||||
let tc_raw = tc_guard.read().unwrap();
|
||||
self.psb.pus_buf[0..tc_raw.len()].copy_from_slice(tc_raw);
|
||||
}
|
||||
let (tc, tc_size) = PusTc::from_bytes(&self.psb.pus_buf).unwrap();
|
||||
let subservice = match pus_11_generic_tc_check(&tc) {
|
||||
Ok(subservice) => subservice,
|
||||
Err(e) => match e {
|
||||
GenericTcCheckError::NotEnoughAppData => {
|
||||
self.psb
|
||||
.verification_handler
|
||||
.start_failure(
|
||||
token,
|
||||
FailParams::new(
|
||||
Some(&self.psb.stamp_buf),
|
||||
&tmtc_err::NOT_ENOUGH_APP_DATA,
|
||||
None,
|
||||
),
|
||||
)
|
||||
.expect("could not sent verification error");
|
||||
return;
|
||||
}
|
||||
GenericTcCheckError::InvalidSubservice => {
|
||||
self.psb
|
||||
.verification_handler
|
||||
.start_failure(
|
||||
token,
|
||||
FailParams::new(
|
||||
Some(&self.psb.stamp_buf),
|
||||
&tmtc_err::INVALID_PUS_SUBSERVICE,
|
||||
None,
|
||||
),
|
||||
)
|
||||
.expect("could not sent verification error");
|
||||
return;
|
||||
}
|
||||
},
|
||||
};
|
||||
match subservice {
|
||||
let std_service = scheduling::Subservice::try_from(tc.subservice());
|
||||
if std_service.is_err() {
|
||||
return Ok(PusPacketHandlerResult::CustomSubservice(token));
|
||||
}
|
||||
match std_service.unwrap() {
|
||||
scheduling::Subservice::TcEnableScheduling => {
|
||||
let start_token = self
|
||||
.psb
|
||||
@ -175,7 +147,10 @@ impl PusService11SchedHandler {
|
||||
.completion_success(start_token, Some(&self.psb.stamp_buf))
|
||||
.expect("sending completion success failed");
|
||||
}
|
||||
_ => {}
|
||||
_ => {
|
||||
return Ok(PusPacketHandlerResult::CustomSubservice(token));
|
||||
}
|
||||
}
|
||||
Ok(PusPacketHandlerResult::CustomSubservice(token))
|
||||
}
|
||||
}
|
||||
|
@ -1,4 +1,7 @@
|
||||
use crate::pus::{AcceptedTc, PusPacketHandlerResult, PusPacketHandlingError, PusServiceBase};
|
||||
use crate::pus::{
|
||||
AcceptedTc, PartialPusHandlingError, PusPacketHandlerResult, PusPacketHandlingError,
|
||||
PusServiceBase,
|
||||
};
|
||||
use delegate::delegate;
|
||||
use log::{error, info, warn};
|
||||
use satrs_core::events::EventU32;
|
||||
@ -13,7 +16,8 @@ use satrs_core::spacepackets::ecss::{PusError, PusPacket};
|
||||
use satrs_core::spacepackets::tc::PusTc;
|
||||
use satrs_core::spacepackets::time::cds::TimeProvider;
|
||||
use satrs_core::spacepackets::time::{StdTimestampError, TimeWriter};
|
||||
use satrs_core::spacepackets::tm::PusTm;
|
||||
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
|
||||
use satrs_core::spacepackets::SpHeader;
|
||||
use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
|
||||
use satrs_example::{tmtc_err, TEST_EVENT};
|
||||
use std::sync::mpsc::{Receiver, Sender, TryRecvError};
|
||||
@ -39,6 +43,13 @@ impl Service17CustomWrapper {
|
||||
info!("Sent ping reply PUS TM[17,2]");
|
||||
handled_pings += 1;
|
||||
}
|
||||
PusPacketHandlerResult::RequestHandledPartialSuccess(partial_err) => {
|
||||
warn!(
|
||||
"Handled PUS ping command with partial success: {:?}",
|
||||
partial_err
|
||||
);
|
||||
handled_pings += 1;
|
||||
}
|
||||
PusPacketHandlerResult::CustomSubservice(token) => {
|
||||
let (buf, _) = self.pus17_handler.pus_tc_buf();
|
||||
let (tc, size) = PusTc::from_bytes(buf).unwrap();
|
||||
@ -90,18 +101,18 @@ impl PusService17TestHandler {
|
||||
pub fn new(
|
||||
receiver: Receiver<AcceptedTc>,
|
||||
tc_pool: SharedPool,
|
||||
tm_helper: PusTmWithCdsShortHelper,
|
||||
tm_tx: Sender<StoreAddr>,
|
||||
tm_store: SharedTmStore,
|
||||
tm_apid: u16,
|
||||
verification_handler: StdVerifReporterWithSender,
|
||||
) -> Self {
|
||||
Self {
|
||||
psb: PusServiceBase::new(
|
||||
receiver,
|
||||
tc_pool,
|
||||
tm_helper,
|
||||
tm_tx,
|
||||
tm_store,
|
||||
tm_apid,
|
||||
verification_handler,
|
||||
),
|
||||
}
|
||||
@ -117,12 +128,7 @@ impl PusService17TestHandler {
|
||||
|
||||
pub fn handle_next_packet(&mut self) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
||||
return match self.psb.tc_rx.try_recv() {
|
||||
Ok((addr, token)) => {
|
||||
if self.handle_one_tc(addr, token)? {
|
||||
return Ok(PusPacketHandlerResult::RequestHandled);
|
||||
}
|
||||
Ok(PusPacketHandlerResult::CustomSubservice(token))
|
||||
}
|
||||
Ok((addr, token)) => self.handle_one_tc(addr, token),
|
||||
Err(e) => match e {
|
||||
TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
|
||||
TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
|
||||
@ -134,7 +140,8 @@ impl PusService17TestHandler {
|
||||
&mut self,
|
||||
addr: StoreAddr,
|
||||
token: VerificationToken<TcStateAccepted>,
|
||||
) -> Result<bool, PusPacketHandlingError> {
|
||||
) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
||||
let mut partial_result = None;
|
||||
{
|
||||
// Keep locked section as short as possible.
|
||||
let mut tc_pool = self
|
||||
@ -143,39 +150,40 @@ impl PusService17TestHandler {
|
||||
.write()
|
||||
.map_err(|e| PusPacketHandlingError::RwGuardError(format!("{e}")))?;
|
||||
let tc_guard = tc_pool.read_with_guard(addr);
|
||||
let tc_raw = tc_guard.read().expect("Reading pool guard failed");
|
||||
let tc_raw = tc_guard.read()?;
|
||||
self.psb.pus_buf[0..tc_raw.len()].copy_from_slice(tc_raw);
|
||||
}
|
||||
let mut partial_error = None;
|
||||
let (tc, tc_size) = PusTc::from_bytes(&self.psb.pus_buf)?;
|
||||
if tc.service() != 17 {
|
||||
return Err(PusPacketHandlingError::WrongService(tc.service()));
|
||||
}
|
||||
if tc.subservice() == 1 {
|
||||
let time_provider = TimeProvider::from_now_with_u16_days()?;
|
||||
// Can not fail, buffer is large enough.
|
||||
time_provider
|
||||
.write_to_bytes(&mut self.psb.stamp_buf)
|
||||
.unwrap();
|
||||
partial_result = self.psb.update_stamp().err();
|
||||
let result = self
|
||||
.psb
|
||||
.verification_handler
|
||||
.start_success(token, Some(&self.psb.stamp_buf));
|
||||
.start_success(token, Some(&self.psb.stamp_buf))
|
||||
.map_err(|e| PartialPusHandlingError::VerificationError);
|
||||
let start_token = if result.is_err() {
|
||||
error!("Could not send start success verification");
|
||||
partial_error = Some(result.unwrap_err());
|
||||
None
|
||||
} else {
|
||||
Some(result.unwrap())
|
||||
};
|
||||
// Sequence count will be handled centrally in TM funnel.
|
||||
let ping_reply =
|
||||
self.psb
|
||||
.tm_helper
|
||||
.create_pus_tm_with_stamp(17, 2, None, &time_provider, 0);
|
||||
let mut reply_header = SpHeader::tm_unseg(self.psb.tm_apid, 0, 0).unwrap();
|
||||
let tc_header = PusTmSecondaryHeader::new_simple(17, 2, &self.psb.stamp_buf);
|
||||
let ping_reply = PusTm::new(&mut reply_header, tc_header, None, true);
|
||||
let addr = self.psb.tm_store.add_pus_tm(&ping_reply);
|
||||
self.psb
|
||||
if let Err(e) = self
|
||||
.psb
|
||||
.tm_tx
|
||||
.send(addr)
|
||||
.map_err(|e| PusPacketHandlingError::TmSendError(format!("{e}")))?;
|
||||
.map_err(|e| PartialPusHandlingError::TmSendError(format!("{e}")))
|
||||
{
|
||||
partial_error = Some(e);
|
||||
}
|
||||
if let Some(start_token) = start_token {
|
||||
if self
|
||||
.psb
|
||||
@ -183,11 +191,16 @@ impl PusService17TestHandler {
|
||||
.completion_success(start_token, Some(&self.psb.stamp_buf))
|
||||
.is_err()
|
||||
{
|
||||
error!("Could not send completion success verification");
|
||||
partial_error = Some(PartialPusHandlingError::VerificationError)
|
||||
}
|
||||
}
|
||||
return Ok(true);
|
||||
if partial_error.is_some() {
|
||||
return Ok(PusPacketHandlerResult::RequestHandledPartialSuccess(
|
||||
partial_error.unwrap(),
|
||||
));
|
||||
}
|
||||
return Ok(PusPacketHandlerResult::RequestHandled);
|
||||
}
|
||||
Ok(false)
|
||||
Ok(PusPacketHandlerResult::CustomSubservice(token))
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user