sat-rs/satrs-example/src/pus/mod.rs

211 lines
7.4 KiB
Rust
Raw Normal View History

2023-07-04 15:17:43 +02:00
use crate::tmtc::MpscStoreAndSendError;
2023-07-08 13:20:08 +02:00
use log::warn;
use satrs::pus::verification::{
FailParams, StdVerifReporterWithSender, VerificationReportingProvider,
};
use satrs::pus::{
EcssTcAndToken, GenericRoutingError, PusPacketHandlerResult, PusRoutingErrorHandler, TcInMemory,
};
use satrs::spacepackets::ecss::tc::PusTcReader;
use satrs::spacepackets::ecss::PusServiceId;
use satrs::spacepackets::time::cds::TimeProvider;
use satrs::spacepackets::time::TimeWriter;
use satrs_example::config::{tmtc_err, CustomPusServiceId};
2023-07-05 11:25:23 +02:00
use std::sync::mpsc::Sender;
2022-08-29 01:33:32 +02:00
2023-07-05 21:08:04 +02:00
pub mod action;
2023-07-05 14:25:51 +02:00
pub mod event;
2023-07-05 21:10:45 +02:00
pub mod hk;
2023-07-04 18:51:54 +02:00
pub mod scheduler;
pub mod stack;
2023-07-03 18:22:08 +02:00
pub mod test;
2023-07-03 01:33:13 +02:00
pub struct PusTcMpscRouter {
2024-01-31 01:32:03 +01:00
pub test_service_receiver: Sender<EcssTcAndToken>,
pub event_service_receiver: Sender<EcssTcAndToken>,
pub sched_service_receiver: Sender<EcssTcAndToken>,
pub hk_service_receiver: Sender<EcssTcAndToken>,
pub action_service_receiver: Sender<EcssTcAndToken>,
2023-07-02 20:49:45 +02:00
}
2023-07-03 01:33:13 +02:00
2022-09-03 13:47:25 +02:00
pub struct PusReceiver {
2023-02-04 15:35:18 +01:00
pub verif_reporter: StdVerifReporterWithSender,
2023-07-03 01:33:13 +02:00
pub pus_router: PusTcMpscRouter,
2023-07-06 00:49:18 +02:00
stamp_helper: TimeStampHelper,
2023-02-04 15:35:18 +01:00
}
struct TimeStampHelper {
stamper: TimeProvider,
time_stamp: [u8; 7],
}
impl TimeStampHelper {
pub fn new() -> Self {
Self {
stamper: TimeProvider::new_with_u16_days(0, 0),
time_stamp: [0; 7],
}
}
pub fn stamp(&self) -> &[u8] {
&self.time_stamp
}
pub fn update_from_now(&mut self) {
self.stamper
.update_from_now()
.expect("Updating timestamp failed");
self.stamper
.write_to_bytes(&mut self.time_stamp)
.expect("Writing timestamp failed");
}
}
2022-09-03 13:47:25 +02:00
impl PusReceiver {
2023-07-06 00:49:18 +02:00
pub fn new(verif_reporter: StdVerifReporterWithSender, pus_router: PusTcMpscRouter) -> Self {
2022-09-03 13:47:25 +02:00
Self {
2023-07-06 00:49:18 +02:00
verif_reporter,
pus_router,
stamp_helper: TimeStampHelper::new(),
2022-09-03 13:47:25 +02:00
}
}
}
2023-07-03 18:22:08 +02:00
impl PusReceiver {
pub fn handle_tc_packet(
2022-09-03 13:47:25 +02:00
&mut self,
2024-01-31 01:32:03 +01:00
tc_in_memory: TcInMemory,
2022-09-03 13:47:25 +02:00
service: u8,
2023-07-11 00:44:48 +02:00
pus_tc: &PusTcReader,
2023-07-08 13:20:08 +02:00
) -> Result<PusPacketHandlerResult, MpscStoreAndSendError> {
2023-07-06 00:49:18 +02:00
let init_token = self.verif_reporter.add_tc(pus_tc);
self.stamp_helper.update_from_now();
2023-02-15 11:19:23 +01:00
let accepted_token = self
2023-07-06 00:49:18 +02:00
.verif_reporter
.acceptance_success(init_token, self.stamp_helper.stamp())
2022-09-10 20:18:19 +02:00
.expect("Acceptance success failure");
let service = PusServiceId::try_from(service);
match service {
Ok(standard_service) => match standard_service {
2023-07-04 18:51:54 +02:00
PusServiceId::Test => {
2024-01-31 01:32:03 +01:00
self.pus_router.test_service_receiver.send(EcssTcAndToken {
tc_in_memory,
token: Some(accepted_token.into()),
})?
}
PusServiceId::Housekeeping => {
self.pus_router.hk_service_receiver.send(EcssTcAndToken {
tc_in_memory,
token: Some(accepted_token.into()),
})?
}
PusServiceId::Event => {
self.pus_router
.event_service_receiver
.send(EcssTcAndToken {
tc_in_memory,
token: Some(accepted_token.into()),
})?
}
PusServiceId::Scheduling => {
2023-07-08 13:20:08 +02:00
self.pus_router
2024-01-31 01:32:03 +01:00
.sched_service_receiver
.send(EcssTcAndToken {
tc_in_memory,
token: Some(accepted_token.into()),
})?
2023-07-04 18:51:54 +02:00
}
2023-07-08 13:20:08 +02:00
_ => {
let result = self.verif_reporter.start_failure(
accepted_token,
FailParams::new(
self.stamp_helper.stamp(),
&tmtc_err::PUS_SERVICE_NOT_IMPLEMENTED,
&[standard_service as u8],
),
2023-07-08 13:20:08 +02:00
);
if result.is_err() {
warn!("Sending verification failure failed");
}
}
},
Err(e) => {
if let Ok(custom_service) = CustomPusServiceId::try_from(e.number) {
match custom_service {
CustomPusServiceId::Mode => {
2023-07-11 00:46:22 +02:00
// TODO: Fix mode service.
2023-07-03 18:22:08 +02:00
//self.handle_mode_service(pus_tc, accepted_token)
}
CustomPusServiceId::Health => {}
}
} else {
2023-07-06 00:49:18 +02:00
self.verif_reporter
.start_failure(
accepted_token,
FailParams::new(
self.stamp_helper.stamp(),
&tmtc_err::INVALID_PUS_SUBSERVICE,
&[e.number],
),
)
.expect("Start failure verification failed")
}
}
2022-09-03 13:47:25 +02:00
}
2023-07-08 13:20:08 +02:00
Ok(PusPacketHandlerResult::RequestHandled)
2022-09-03 13:47:25 +02:00
}
}
#[derive(Default)]
pub struct GenericRoutingErrorHandler<const SERVICE_ID: u8> {}
impl<const SERVICE_ID: u8> PusRoutingErrorHandler for GenericRoutingErrorHandler<SERVICE_ID> {
type Error = satrs::pus::GenericRoutingError;
fn handle_error(
&self,
target_id: satrs::TargetId,
token: satrs::pus::verification::VerificationToken<
satrs::pus::verification::TcStateAccepted,
>,
_tc: &PusTcReader,
error: Self::Error,
time_stamp: &[u8],
verif_reporter: &impl VerificationReportingProvider,
) {
warn!("Routing request for service {SERVICE_ID} failed: {error:?}");
match error {
GenericRoutingError::UnknownTargetId(id) => {
let mut fail_data: [u8; 8] = [0; 8];
fail_data.copy_from_slice(&id.to_be_bytes());
verif_reporter
.start_failure(
token,
FailParams::new(time_stamp, &tmtc_err::UNKNOWN_TARGET_ID, &fail_data),
)
.expect("Sending start failure failed");
}
GenericRoutingError::SendError(_) => {
let mut fail_data: [u8; 8] = [0; 8];
fail_data.copy_from_slice(&target_id.to_be_bytes());
verif_reporter
.start_failure(
token,
FailParams::new(time_stamp, &tmtc_err::ROUTING_ERROR, &fail_data),
)
.expect("Sending start failure failed");
}
GenericRoutingError::NotEnoughAppData { expected, found } => {
let mut context_info = (found as u32).to_be_bytes().to_vec();
context_info.extend_from_slice(&(expected as u32).to_be_bytes());
verif_reporter
.start_failure(
token,
FailParams::new(time_stamp, &tmtc_err::NOT_ENOUGH_APP_DATA, &context_info),
)
.expect("Sending start failure failed");
}
}
}
}