almost finished completely
This commit is contained in:
165
satrs-example/src/pus/action.rs
Normal file
165
satrs-example/src/pus/action.rs
Normal file
@ -0,0 +1,165 @@
|
||||
use crate::requests::{ActionRequest, Request, RequestWithToken};
|
||||
use log::{error, warn};
|
||||
use satrs_core::pool::{SharedPool, StoreAddr};
|
||||
use satrs_core::pus::verification::{
|
||||
FailParams, StdVerifReporterWithSender, TcStateAccepted, VerificationToken,
|
||||
};
|
||||
use satrs_core::pus::{
|
||||
AcceptedTc, PusPacketHandlerResult, PusPacketHandlingError, PusServiceBase, PusServiceHandler,
|
||||
};
|
||||
use satrs_core::spacepackets::ecss::PusPacket;
|
||||
use satrs_core::spacepackets::tc::PusTc;
|
||||
use satrs_core::tmtc::tm_helper::SharedTmStore;
|
||||
use satrs_core::tmtc::TargetId;
|
||||
use satrs_example::tmtc_err;
|
||||
use std::collections::HashMap;
|
||||
use std::sync::mpsc::{Receiver, Sender};
|
||||
|
||||
pub struct PusService8ActionHandler {
|
||||
psb: PusServiceBase,
|
||||
request_handlers: HashMap<TargetId, Sender<RequestWithToken>>,
|
||||
}
|
||||
|
||||
impl PusService8ActionHandler {
|
||||
pub fn new(
|
||||
receiver: Receiver<AcceptedTc>,
|
||||
tc_pool: SharedPool,
|
||||
tm_tx: Sender<StoreAddr>,
|
||||
tm_store: SharedTmStore,
|
||||
tm_apid: u16,
|
||||
verification_handler: StdVerifReporterWithSender,
|
||||
request_handlers: HashMap<TargetId, Sender<RequestWithToken>>,
|
||||
) -> Self {
|
||||
Self {
|
||||
psb: PusServiceBase::new(
|
||||
receiver,
|
||||
tc_pool,
|
||||
tm_tx,
|
||||
tm_store,
|
||||
tm_apid,
|
||||
verification_handler,
|
||||
),
|
||||
request_handlers,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl PusServiceHandler for PusService8ActionHandler {
|
||||
fn psb_mut(&mut self) -> &mut PusServiceBase {
|
||||
&mut self.psb
|
||||
}
|
||||
fn psb(&self) -> &PusServiceBase {
|
||||
&self.psb
|
||||
}
|
||||
|
||||
fn handle_one_tc(
|
||||
&mut self,
|
||||
addr: StoreAddr,
|
||||
token: VerificationToken<TcStateAccepted>,
|
||||
) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
||||
self.copy_tc_to_buf(addr)?;
|
||||
let (tc, _) = PusTc::from_bytes(&self.psb().pus_buf).unwrap();
|
||||
let subservice = tc.subservice();
|
||||
let mut partial_error = None;
|
||||
let time_stamp = self.psb().get_current_timestamp(&mut partial_error);
|
||||
match subservice {
|
||||
128 => {
|
||||
let user_data = tc.user_data();
|
||||
if user_data.is_none() || user_data.unwrap().len() < 8 {
|
||||
self.psb_mut()
|
||||
.verification_handler
|
||||
.start_failure(
|
||||
token,
|
||||
FailParams::new(
|
||||
Some(&time_stamp),
|
||||
&tmtc_err::NOT_ENOUGH_APP_DATA,
|
||||
None,
|
||||
),
|
||||
)
|
||||
.expect("Sending start failure failed");
|
||||
return Err(PusPacketHandlingError::NotEnoughAppData(
|
||||
"Expected at least 4 bytes".into(),
|
||||
));
|
||||
}
|
||||
let user_data = user_data.unwrap();
|
||||
let target_id = u32::from_be_bytes(user_data[0..4].try_into().unwrap());
|
||||
let action_id = u32::from_be_bytes(user_data[4..8].try_into().unwrap());
|
||||
if let Some(sender) = self.request_handlers.get(&target_id) {
|
||||
sender
|
||||
.send(RequestWithToken::new(
|
||||
target_id,
|
||||
Request::Action(ActionRequest::CmdWithU32Id((
|
||||
action_id,
|
||||
Vec::from(&user_data[8..]),
|
||||
))),
|
||||
token,
|
||||
))
|
||||
.expect("Forwarding action request failed");
|
||||
} else {
|
||||
let mut fail_data: [u8; 4] = [0; 4];
|
||||
fail_data.copy_from_slice(&target_id.to_be_bytes());
|
||||
self.psb_mut()
|
||||
.verification_handler
|
||||
.start_failure(
|
||||
token,
|
||||
FailParams::new(
|
||||
Some(&time_stamp),
|
||||
&tmtc_err::UNKNOWN_TARGET_ID,
|
||||
Some(&fail_data),
|
||||
),
|
||||
)
|
||||
.expect("Sending start failure failed");
|
||||
return Err(PusPacketHandlingError::OtherError(format!(
|
||||
"Unknown target ID {target_id}"
|
||||
)));
|
||||
}
|
||||
}
|
||||
_ => {
|
||||
let fail_data = [subservice];
|
||||
self.psb_mut()
|
||||
.verification_handler
|
||||
.start_failure(
|
||||
token,
|
||||
FailParams::new(
|
||||
Some(&time_stamp),
|
||||
&tmtc_err::INVALID_PUS_SUBSERVICE,
|
||||
Some(&fail_data),
|
||||
),
|
||||
)
|
||||
.expect("Sending start failure failed");
|
||||
return Err(PusPacketHandlingError::InvalidSubservice(subservice));
|
||||
}
|
||||
}
|
||||
Ok(PusPacketHandlerResult::RequestHandled)
|
||||
}
|
||||
}
|
||||
|
||||
pub struct Pus8Wrapper {
|
||||
pub(crate) pus_8_handler: PusService8ActionHandler,
|
||||
}
|
||||
|
||||
impl Pus8Wrapper {
|
||||
pub fn handle_next_packet(&mut self) -> bool {
|
||||
match self.pus_8_handler.handle_next_packet() {
|
||||
Ok(result) => match result {
|
||||
PusPacketHandlerResult::RequestHandled => {}
|
||||
PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
|
||||
warn!("PUS 8 partial packet handling success: {e:?}")
|
||||
}
|
||||
PusPacketHandlerResult::CustomSubservice(invalid, _) => {
|
||||
warn!("PUS 8 invalid subservice {invalid}");
|
||||
}
|
||||
PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
|
||||
warn!("PUS 8 subservice {subservice} not implemented");
|
||||
}
|
||||
PusPacketHandlerResult::Empty => {
|
||||
return true;
|
||||
}
|
||||
},
|
||||
Err(error) => {
|
||||
error!("PUS packet handling error: {error:?}")
|
||||
}
|
||||
}
|
||||
false
|
||||
}
|
||||
}
|
||||
@ -12,6 +12,7 @@ use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
|
||||
use satrs_example::{tmtc_err, CustomPusServiceId};
|
||||
use std::sync::mpsc::Sender;
|
||||
|
||||
pub mod action;
|
||||
pub mod event;
|
||||
pub mod scheduler;
|
||||
pub mod test;
|
||||
|
||||
@ -2,6 +2,7 @@ use log::{info, warn};
|
||||
use satrs_core::events::EventU32;
|
||||
use satrs_core::params::Params;
|
||||
use satrs_core::pus::test::PusService17TestHandler;
|
||||
use satrs_core::pus::verification::FailParams;
|
||||
use satrs_core::pus::{PusPacketHandlerResult, PusServiceHandler};
|
||||
use satrs_core::spacepackets::ecss::PusPacket;
|
||||
use satrs_core::spacepackets::tc::PusTc;
|
||||
@ -37,8 +38,9 @@ impl Service17CustomWrapper {
|
||||
warn!("PUS17: Subservice {subservice} not implemented")
|
||||
}
|
||||
PusPacketHandlerResult::CustomSubservice(subservice, token) => {
|
||||
let (buf, _) = self.pus17_handler.pus_tc_buf();
|
||||
let (tc, _) = PusTc::from_bytes(buf).unwrap();
|
||||
let psb_mut = self.pus17_handler.psb_mut();
|
||||
let buf = psb_mut.pus_buf;
|
||||
let (tc, _) = PusTc::from_bytes(&buf).unwrap();
|
||||
let time_stamper = TimeProvider::from_now_with_u16_days().unwrap();
|
||||
let mut stamp_buf: [u8; 7] = [0; 7];
|
||||
time_stamper.write_to_bytes(&mut stamp_buf).unwrap();
|
||||
@ -47,23 +49,26 @@ impl Service17CustomWrapper {
|
||||
self.test_srv_event_sender
|
||||
.send((TEST_EVENT.into(), None))
|
||||
.expect("Sending test event failed");
|
||||
let start_token = self
|
||||
.pus17_handler
|
||||
.verification_reporter()
|
||||
let start_token = psb_mut
|
||||
.verification_handler
|
||||
.start_success(token, Some(&stamp_buf))
|
||||
.expect("Error sending start success");
|
||||
self.pus17_handler
|
||||
.verification_reporter()
|
||||
psb_mut
|
||||
.verification_handler
|
||||
.completion_success(start_token, Some(&stamp_buf))
|
||||
.expect("Error sending completion success");
|
||||
} else {
|
||||
let fail_data = [tc.subservice()];
|
||||
self.pus17_handler
|
||||
.psb_mut()
|
||||
.report_start_failure(
|
||||
.verification_handler
|
||||
.start_failure(
|
||||
token,
|
||||
&tmtc_err::INVALID_PUS_SUBSERVICE,
|
||||
Some(&fail_data),
|
||||
FailParams::new(
|
||||
Some(&stamp_buf),
|
||||
&tmtc_err::INVALID_PUS_SUBSERVICE,
|
||||
Some(&fail_data),
|
||||
),
|
||||
)
|
||||
.expect("Sending start failure verification failed");
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user